New Stepper Motor Library

I did measure it with and without anything connected, with the same result.

if I do a digital write to the same pins they output 3.3 volts as expected. I'm sorta clueless.
 
Just to make sure that Teensy and scope are working, I'd test if this gives 10µs pulses on your scope:

Code:
constexpr int pin = 0;

void setup() {
  pinMode(pin, OUTPUT);
}

void loop() {
  digitalWriteFast(pin, HIGH);
  delayMicroseconds(10);
  digitalWriteFast(pin, LOW);

  delayMicroseconds(200);
}
 
strange, I think you got the point, though I'm not sure why the servo motors don't run.
I tested it with an arduino uno before and they had no problem.

here's my scope output generated by your test code: Scope.jpg

I'm again clueless now.
 
I measured directly at the teensy pin and for now I use a ULN2803A darlington transistor array as a driver.
of course the teensy output doesn't switch the transistors properly.
for any reason it seems to be a timer issue. I suspected platformio to be the problem, but I got the same result using arduino IDE
 
If you can post a minimal example sketch showing the effect I can try to reproduce and dig further into it.
 
Thanks luni for all your help.

Finally I figured there where a few issues, that in combination made no sense and caused me to not figure the actual problem.

First problem: my oscilloscope obviosly suddenly died, or shows wrong values on a wrong timebase, on both channels wich was misleading
second problem (the actual problem why it didn't work): I made a mistake in the driver circuit -> forgot to pull up the outputs :D
 
Thanks for the great library! Have you given any thought to adding support for s-curves?

I wrote a small python script that can be used to test/simulate different "motion profiles" using TeensyStep.

Code:
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()
TeensyStepSim.jpg
 

Attachments

  • TeensyStepSim.jpg
    TeensyStepSim.jpg
    66.5 KB · Views: 201
Last edited:
Have you given any thought to adding support for s-curves?
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?
 
That would be great! In my own tweaked version here I made it delegate the initial (from doStep) and update calculations (from the accUpdateChannel) to a class that can implement the methods as needed.

Code:
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; 
    }
};

Then made a subclass for the trapezoidal profile and am working on another subclass for s-curves.
 
Yep, this is exactly what I was thinking of. I would implement a base class with all the house keeping functionality and the Bresenham part.
  • I'd add two abstract members, say start and updateSpeed. You would need to override them in your derived controller classes.
  • void start() would be called right before the movement starts. This would be the place where you could pre-calculate all values which will stay constant during the movement.
  • uint32 updateSpeed(currentPos) would be called whenever a new speed value is needed. You simply would calculate the new speed based on the passed in current position and return it.

If you are willing to do the testing I can try to implement it tomorrow.
 
Custom acceleration profile

@frmdstryr: As discussed yesterday, I splitted the original StepControl class into the abstract base class StepControlBase and derived the new StepControl from this class. The code is uploaded to the development branch of the GitHub repository (https://github.com/luni64/TeensyStep/tree/develop )

To generate any special acceleration profile you need to to the following:

  1. Derive a new controller class from StepControlBase (you can use my StepControl as a template, see below)
  2. Override the following functions in your controller:
    • uint32_t prepareMovement(uint32_t targetPos, uint32_t targetSpeed, uint32_t pullInSpeed, uint32_t acceleration)
      In that function you can setup whatever you need during the movement. The passed in parameters should be clear from the names. You need to return the starting frequency in steps/s from this function. You only need to take care of the acceleration, the Bresenham algorithm is handled by the base class.
    • uint32_t updateSpeed(uint32_t currentPosition)
      This function will be called whenever a new speed is required. You need to calculate that speed based on the passed in position and return it in steps/s
    • uint32_t StepControl::initiateStopping(uint32_t currentPosition)
      This function will be called when the user requests a stop. Do whatever needs to be done to calculate a stopping profile and return the new target position.

Sounds more complicated than it really is. Let me know if that works for you. Here the current code for my standard acceleration profile as an example:

Code:
#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;
    }
}
 
Last edited:
Tried it out. Works well! Thank you!

One more thing. I've been using TeensyStep to have a engraver follow a path. I know there's already a callback that can be fired when a move completes but at that point the timer is stopped already and must be started again for the next move which may cause jerk (assuming the velocity profile is changed so that it doesn't stop at each point).

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.
 
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.

What you are looking for is probably a motion planner. I.e. you give it a list of 2d (3d) data and it then generates movements from point to point without stopping in between. Unfortunately this is something quite difficult to implement. It is not about just working through a list of points, 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. To stay in sync, the x-motor needs to be decelerated first. The situation is completely different when both motors where moving before. What I want to say is: "no, I probably won't open that can of worms..."

But, there are perfect solutions available for that kind of problems. grbl is kind of standard in the open source DIY CNC scene. It was made for exactly this kind of applications, comes with a g-code interpreter, can control up to 3 motors with up to 30000 steps/s. I don't know if someone ported it to ARM yet. (It was developed for UNOs, i.e. it runs on a simple ATmega328P). Anyway, a couple of years ago I did a project using a 328p as "grbl coprocessor" for a Teensy which worked nicely. The encircled part is the grbl coprocessor.

grbl2.png

Here you can see it in action (one motor only, the video was meant to show the performance of the encoder code and the websocket server running on the Teensy)
.

If you are interested I can post schematics, I should also have a couple of empty boards lying around somewhere...
 
Sure I'd be interested in checking out the schematics if you can pm them to me. Yes, I've heard of grbl but haven't tried it yet as i'd prefer to do everything on the teensy.

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).


TeensyStepSCurve.jpg


Also regarding ,

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

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.
 
Sure I'd be interested in checking out the schematics
Here you are: View attachment drive-download-20181205T110944Z-001.zip

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).

Sounds great! When you fixed the issues, would it be possible to get a copy for the examples folder (Pull request would be even better)?

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.
Looking forward to see your results, would be a really useful feature indeed.
 
Just wanted to say thanks to luni for some great code, and thanks to frmdstryr for helping to improve it. I was moving my linear test jig in a matter of minutes using luni's library & a Teensy 3.6. It was only a few minutes after that that I realized S-curve ramping was needed. Now, it appears it will be realized soon. Thank you both again for your efforts!
 
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.

It would be a really cool feature if they could be re-targeted while moving. Probably would make the library quite attractive to the robotics community as well.
 
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.

It would be a really cool feature if they could be re-targeted while moving. Probably would make the library quite attractive to the robotics community as well.
 
New rotational controller

I finally uploaded a first test version of the new rotational controller https://github.com/luni64/TeensyStep/tree/develop_newRotate . It allows to change the speed of the attached steppers on the fly. While speed is changed all attached motors stay in sync.
I'll update the documentation in the next days but the usage is quite simple: To change the rotational speed you simply call controller.overrideSpeed(float f) where f = 1 rotates the motors at the original speeds, f = 0.5 at half speed, f=-1 full speed but other direction. f=0 stops the motors...

Here a quick video showing two synced motors with manually changed overall speed:

Here the corresponding code:

Code:
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
  }
}

And here the recorded s-t diagram recorded with the sketch from above. I manually changed the speed at ~5000ms and ~12000ms.

st.PNG

Any tests and bug reports very welcome :)
 
Last edited:
Hi Luni. That is excellent, especially the ability to reverse the motors by providing a negative speed. Is it possible yet to change the target position as well while the motors are moving?

I'm also curious which stepper drivers you are using in that video.
 
Is it possible yet to change the target position as well while the motors are moving?
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.

I'm also curious which stepper drivers you are using in that video.
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#post193501
 
Hi Luni,

Thank you so much for your efforts! Your original library has proven to be very useful.

Any tests and bug reports very welcome :)

I tried your newest version of the rotational controller on a Teensy 3.5 but I can´t seem to get it to compile using the Arduino IDE. I'm getting the errors below. Do you have any idea what could be wrong?

Many thanks!

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>::eek: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.
 
Back
Top