constexpr int pin = 0;
void setup() {
pinMode(pin, OUTPUT);
}
void loop() {
digitalWriteFast(pin, HIGH);
delayMicroseconds(10);
digitalWriteFast(pin, LOW);
delayMicroseconds(200);
}
import time
import math
import signal
import numpy as np
import matplotlib.pyplot as plt
# Teensy 3.2 at 96Mhz -> Bus at 48Mhz
F_BUS = 48e6
class MotionProfile(object):
speedMax = 20000
speedMin = 1000
accelMax = 8000
accelEnd = 0
decelStart = 0
distance = 0
dtMin = 0
dtMax = 0
dtAccel = 0
currentDelay = 0
def start(self, e):
self.distance = e
dv = self.speedMax-self.speedMin
dt = dv/float(self.accelMax)
d = dv*dt/2.0
self.accelEnd = min(self.distance/2, d)
self.decelStart = self.distance-self.accelEnd
self.dtMin = F_BUS/self.speedMin
self.dtMax = F_BUS/self.speedMax
self.dtAccel = math.sqrt(2*self.accelMax)
return self.dtMin
def update(self, e):
pos = self.distance-e
if pos < self.accelEnd:
return F_BUS/(self.dtAccel*math.sqrt(pos)+self.speedMin)
elif pos < self.decelStart:
return self.dtMax
else:
return F_BUS/(self.dtAccel*math.sqrt(e)+self.speedMin)
class Simulation(object):
""" Simulates the TeensyStep controller
"""
speedUpdateRate = 5000
profile = None
def __init__(self, **kwargs):
for k, v in kwargs.items():
setattr(self, k, v)
def run(self, target, time_limit=10, sample_smoothing=1,
simulation_speed=100.0):
"""
Run a simulation of a move from 0 to target steps
Parameters
----------
target: unsigned int
Number of steps to move
time_limit: float
Seconds to wait before stopping the simulation if target is not hit
sample_smoothing: unsigned int
Smooth samples over multiple points
simulation_speed: float
Scales the timers to "speed up" the simulation
"""
# t, p, v, a, j
sim = []
start_time = time.time()
profile = self.profile
position = 0
error = target-position
# Setup simulated timers
t1_expiry = profile.start(error) # PIT timer
t2_expiry = F_BUS/1000000*self.speedUpdateRate # FTM timer
t3_expiry = F_BUS/2 # Simulation watchdog timer
# Run until complete
print("Timers: PIT={}, FTM={}".format(t1_expiry, t2_expiry))
# Set initial values clk, pos, v, a, j, period
v, a, j, = 0, 0, 0
sim.append((-t2_expiry/3, position, v, a, j, 0))
sim.append((0, position, v, a, j, 0))
# Run simulation
clk, i, t1, t2, t3 = 0, 0, 0, 0, 0
while True:
clk += 1
t1 += 1
t2 += 1
t3 += 1
if t1_expiry and t1 >= t1_expiry/simulation_speed: # PIT timer
t1 = 0 # clear timer
if error > 0:
position += 1 # do step
error = target-position
else:
t1_expiry = 0 # Stop timer
if t2 >= t2_expiry/simulation_speed: # FTM timer
t2 = 0 # clear timer
# Change speed
if t1_expiry:
t1_expiry = profile.update(error)
i += 1
# If we log too much we get jittery data
if i >= sample_smoothing:
i = 0
# Log simulation data
last = sim[-1]
t = (clk-last[0])/simulation_speed
v = (position-last[1])/t
a = (v-last[2])/t
j = (a-last[3])/t
sim.append((clk, position, v, a, j, t1_expiry))
# In case nothing happens abort
if t3 >= t3_expiry/simulation_speed:
t3 = 0
if error == 0:
break
if time.time()-start_time > time_limit:
print("Aborted: {} {}".format(clk, error))
break
print("Complete: {}".format(round(time.time()-start_time), 2))
return sim
def main():
# Simulate the given motion profile
sim = Simulation(profile=MotionProfile())
data = sim.run(100000, time_limit=2, sample_smoothing=10)
# Plot the data
t = [s[0] for s in data]
fig, plots = plt.subplots(5, 1)
plots[0].set_title("Simulation")
for i, (label, color) in enumerate((
('Position', 'red'),
('Velocity', 'blue'),
('Accel', 'green'),
('Jerk', 'purple'),
('Cycles', 'orange')
)):
p = plots[i]
ydata = [s[i+1] for s in data]
p.plot(t, ydata, color=color, label=label)
p.set_ylabel(label)
plots[-1].set_xlabel('Clock')
plt.show()
if __name__ == '__main__':
# Keep ctrl c
signal.signal(signal.SIGINT, signal.SIG_DFL)
main()
Yes, thought of that but the need was never big enough to really implement it. I could offer to implement a hook function which will be called whenever a new acceleration value is needed. You could then implement any acceleration you feel is appropriate for your application. Would that make sense for you?Have you given any thought to adding support for s-curves?
class MotionProfile {
public:
/**
* Initialize the motion profile
* @return time in steps/s
*/
virtual uint32_t start(const StepControl &controller) {
return update(controller);
}
/**
* Calculate the timer delay for the next step which in
* turn sets the velocity.
* @return time in steps/s
*/
virtual uint32_t update(const StepControl &controller) {
return 10000;
}
};
#include "StepControlBase.h"
class StepControl : public StepControlBase<>
{
protected:
uint32_t accelerationEnd;
uint32_t decelerationStart;
uint32_t sqrt_2a;
uint32_t s_tgt, v_tgt, v_min;
inline uint32_t prepareMovement(uint32_t targetPos, uint32_t targetSpeed, uint32_t pullInSpeed, uint32_t acceleration);
inline uint32_t updateSpeed(uint32_t currentPosition);
inline uint32_t initiateStopping(uint32_t currentPosition);
};
// IMPLEMENTATION --------------------------------------------------------------------------------------------------
uint32_t StepControl::prepareMovement(uint32_t targetPos, uint32_t targetSpeed, uint32_t pullInSpeed, uint32_t a)
{
s_tgt = targetPos;
v_tgt = targetSpeed;
v_min = pullInSpeed;
if (v_tgt > v_min) // target speed larger than pull in speed
{ // -> acceleration required
float dv = v_tgt - v_min;
float ae = dv * dv / (2.0f * a); // length of acceleration phase (steps)
sqrt_2a = sqrtf(2.0f * a); // precalculated factor, needed during acceleration
accelerationEnd = std::min((uint32_t)ae, s_tgt / 2); // limit acceleration phase to half of total steps
decelerationStart = s_tgt - accelerationEnd;
return v_min; // start movment with the pull in speed
}
else // target speed smaller that pull in speed
{ // -> no acceleration necessary
accelerationEnd = 0;
decelerationStart = s_tgt;
return v_tgt;
}
}
uint32_t StepControl::updateSpeed(uint32_t currentPosition)
{
// acceleration phase -------------------------------------
if (currentPosition < accelerationEnd)
return sqrt_2a * sqrtf(currentPosition) + v_min;
// constant speed phase ------------------------------------
if (currentPosition < decelerationStart)
return v_tgt;
//deceleration phase --------------------------------------
return sqrt_2a * sqrtf(s_tgt - currentPosition) + v_min;
}
uint32_t StepControl::initiateStopping(uint32_t s_cur)
{
if (s_cur <= decelerationStart) // we are already decelerating, nothing to change...
{
return s_tgt;
}
else // accelerating or constant speed phase
{
uint32_t newTarget;
if (s_cur < accelerationEnd) // still accelerating
newTarget = s_cur * 2; // will take the same distance to stop as we already traveled
else // constant speed phase
newTarget = s_cur + accelerationEnd;
accelerationEnd = 0;
decelerationStart = s_cur; // start decelerating now
return newTarget;
}
}
What are your thoughts on providing a way to keep the timer running if more immediate moves are needed? Something like a `virtual void onComplete()` that can be overridden to only stop the timer if there's no more moves waiting and modified doMove to only restart the timer if it's not already running.
you also need to do a proper acceleration / deceleration in between. E.g. if you move from say (x,y) = (0,0) to (1000, 0) to (1000,1000) you need to plan what do to with the y motor which needs to accelerate from zero
Here you are: View attachment drive-download-20181205T110944Z-001.zipSure I'd be interested in checking out the schematics
S-curve's are working in the simulation but there's some issues that seem to be related to floating point math / overflows or something on the actual teensy (3.2). It works fine for 25k steps/s on my desk but at 60k steps/s on the engraver I'm getting strange behavior on larger distance moves (works well for movements up to around 120k steps).
Looking forward to see your results, would be a really useful feature indeed.Correct, I plan to keep track of the current state of each motor and scale the velocities based on the dot products of the path vectors using a look ahead algorithm on a queue of points. Perhaps I'm over simplifying it but I believe it should just work like how the current line following algorithm adjusts the speed of each motor.
I didn't initially realize that I could create multiple controllers that could operate motors independently of each other. My system is very different from a CNC application, and the motors need to operate completely independently of each other. This seems to be working now that I have created two controllers. I'm now even more impressed with this library, as I was considering having a separate Teensy for each motor so that I could use this library.
Regarding the modified library that provides the callback.... do I understand correctly that the change essentially allows the motor to move to several different target positions sequentially, and between each move it stops and then starts the move to the next target?
Regarding your comment "If you change the target of one of the synced motors what should happen with the others?"...... As you can see in my application the motors aren't synced, so it would have no effect. They operate completely independently of each other..... e.g. one may be stopped while the other is moving, or they could be both moving with no correlation between them.
ResponsiveAnalogRead pot(15, true);
Stepper m1(0, 1), m2(2, 3);
RotateControl<> controller;
void setup()
{
while (!Serial);
m1.setAcceleration(15000);
m2.setAcceleration(15000);
m1.setMaxSpeed(-20000);
m2.setMaxSpeed(40000);
controller.rotateAsync(m1, m2);
}
elapsedMillis stopwatch = 0;
void loop()
{
pot.update();
if (pot.hasChanged())
{
float factor = (pot.getValue() - 511) / 511.0; //Convert the pot value to a factor from -1.0f ... 1.0f
controller.overrideSpeed(factor);
}
if(stopwatch > 10)
{
stopwatch = 0;
Serial.printf("%d\t%i\t%i\n", millis(), m1.getPosition(), m2.getPosition()); // print values for s-t diagram
}
}
I could implement that for one motor per controller but not for a group of synced motors. I'll try to do that after the final release of the current version. However, for your application (antenna following rocket?) using the new override speed feature together with a standard PID algorithm might work as well. I'll try it out with dummy input data (can you send some?) and generate an example if it works.Is it possible yet to change the target position as well while the motors are moving?
This was done with standard DRV8825 drivers on a carrier board. https://forum.pjrc.com/threads/54684-Noise-but-no-Movement-with-3-Steppers-amp-TeensyStep?p=193501&viewfull=1#post193501I'm also curious which stepper drivers you are using in that video.
Any tests and bug reports very welcome
In file included from C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/WProgram.h:45:0,
from C:\Users\Fernando\AppData\Local\Temp\arduino_build_288355\pch\Arduino.h:6:
D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate\src/RotateControl.h: In member function 'void RotateControl<pulseWidth, accUpdatePeriod>:verrideSpeed(float)':
C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/wiring.h:130:18: error: expected unqualified-id before '(' token
#define round(x) ({ \
^
D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate\src/RotateControl.h:45:22: note: in expansion of macro 'round'
v_tgt = std::round(v_tgt_orig * fac);
^
C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/wiring.h:131:3: error: expected primary-expression before 'typeof'
typeof(x) _x = (x); \
^
D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate\src/RotateControl.h:45:22: note: in expansion of macro 'round'
v_tgt = std::round(v_tgt_orig * fac);
^
C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/wiring.h:131:3: error: expected '}' before 'typeof'
typeof(x) _x = (x); \
^
D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate\src/RotateControl.h:45:22: note: in expansion of macro 'round'
v_tgt = std::round(v_tgt_orig * fac);
^
C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/wiring.h:131:3: error: expected ')' before 'typeof'
typeof(x) _x = (x); \
^
D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate\src/RotateControl.h:45:22: note: in expansion of macro 'round'
v_tgt = std::round(v_tgt_orig * fac);
^
C:\Program Files (x86)\Arduino\hardware\teensy\avr\cores\teensy3/wiring.h:132:4: error: '_x' was not declared in this scope
(_x>=0) ? (long)(_x+0.5) : (long)(_x-0.5); \
^
D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate\src/RotateControl.h:45:22: note: in expansion of macro 'round'
v_tgt = std::round(v_tgt_orig * fac);
^
Using library ResponsiveAnalogRead at version 1.1.0 in folder: C:\Program Files (x86)\Arduino\hardware\teensy\avr\libraries\ResponsiveAnalogRead
Using library TeensyStep-develop_newRotate at version 0.98.1 in folder: D:\Documents\Arduino\libraries\TeensyStep-develop_newRotate
Error compiling for board Teensy 3.5.