Trapezodial velocity profile for a stepper motor

Math behind it

Discrete stepping and acceleration

When using a stepper motor, we want to avoid running it immediately at full speed especially if a mass is attached to the motor since it is quite likely to induce miss-stepping. In our case, we use a stepper attached to a lead screw forming a linear stage. The stage now performs a linear motion over a total distance defined as $\mathrm{\Delta }s$. This distance can be defined through a certain number of discrete steps $n_s$ each having a step length $s_s$ defined through the number of steps per revolution $n_r=400\frac{steps}{rev}$ and the pitch of the lead screw $p=8\frac{mm}{rev}$:

$$s_s=\frac{p}{n_r},\mathrm{\Delta }s=s_s\cdot n_s$$

In the ideal case we would like to follow a linear increase in velocity followed by a constant plateau of maximum movement velocity $v_m$ followed by a decrease in velocity. In case of the iterative routine in which we step the motor, it makes most sense to have a formula reversely telling us which velocity we want to drive for the upcoming step $i_s$ which has the center position $s$:

$$s=(i_s+0.5)\cdot s_s,i_s\in [0,n_s-1]$$

The velocity of a stepper motor is thereby translated into the delay time $t_d$ before the next discrete step occurs:

$$t_d=\frac{s_s}{v}$$

Linear acceleration phase

In the acceleration phase, our linearly accelerated velocity profile is described as:

$$s=\frac{1}{2}\cdot a\cdot t^2$$

with the velocity $v$ being defined as:

$$v=a\cdot t$$

Solving this equation for $v(s)$ leads to:

$$v=\sqrt{2\cdot s\cdot a}$$

Constant velocity phase

In this case we just have the velocity at a constant level $v_m$.

Linear deceleration phase

For the deceleration phase, we have the following description of the velocity profile:

$$v=v_m-a\cdot (t-t_2)\$$$

or solved after $(t-t_2)$

$$t-t_2=\frac{v_m-v}{a}$$

with its distance counterpart

$$s=s_2+v_m\cdot (t-t_2)-\frac{1}{2}\cdot a\cdot (t-t_2{)}^2$$

or

$$-s=-s_2-v_m\cdot \frac{v_m-v}{a}+\frac{(v_m-v{)}^2}{2\cdot a}$$$$0=\frac{1}{2\cdot a}\cdot (v_m-v{)}^2+\frac{-v_m}{a}\cdot (v_m-v)+(s-s_2)$$

Solving the quadratic equation through

$$x_{1,2}=\frac{-b\pm \sqrt{b^2-4ac}}{2a}$$

leads to

$$(v_m-v{)}_{1,2}=\frac{\frac{v_m}{a}\pm \sqrt{\frac{v_m^2}{a^2}-\frac{2}{a}\cdot (s-s_2)}}{\frac{1}{a}}$$$$(v_m-v{)}_{1,2}=v_m\pm \cdot \sqrt{v_m^2-2\cdot a\cdot (s-s_2)}$$$$v=\sqrt{v_m^2-2\cdot a\cdot (s-s_2)}$$

Case switch

In practice we now need to find the thresholds for the three phases. $s_1$ is defined as the position where full speed is reached, $s_2$ is the position where we start decelerating.

We can extract the first distance as

$$s_1=\frac{v_m^2}{2\cdot a}$$

and the second one as

$$s_2=\mathrm{\Delta }s-s_1$$

If $s_1$ becomes larger then $s_2$ it means that full velocity will not be reached due to a short travel length or low acceleration. In this case we set $s_1=s_2=\mathrm{\Delta }s/2$ and redefine $v_m$:

$$v_m=\sqrt{\mathrm{\Delta }s\cdot a}$$

Microcontroller implementation (Teensy)

// define before
// ss [float] --> step size in mm
// vmax [float] --> maximum desired linear velocity in mm / s
// acc [float] --> acceleration in mm/(s*s)
// deltaS [float] --> distance to travel
// fallAndRise [bool] --> true if rising and falling edge lead to stepping, otherwise false

uint32_t absSteps = round(abs(deltaS) / ss); // number of steps we need to do

// define end point of acceleration and start point of deceleration
float s_1 = vmax * vmax / (2 * acc);
float s_2 = deltaS - s_1;

if (s_1 > s_2) // if we don't even reach full speed
{
    s_1 = deltaS / 2;
    s_2 = deltaS / 2;
    vmax = sqrt(deltaS * acc);
}

float vcurr = 0;
bool stepPolarity = 0;
for (int32_t incStep = 0; incStep < absSteps; incStep++)
{
    // calculate central position of current step
    const float s = ((float) incStep + 0.5) * ss;

    // calculate velocity at current step
    if (s < s_1)
    {
	vcurr = sqrt(2 * s * acc);
    }
    else if (s < s_2)
    {
	vcurr = vmax;
    }
    else
    {
	vcurr = sqrt(vmax * vmax - 2 * (s - s_2) * acc);
    }

    // convert velocity to delay
    const uint32_t tDelay = round(ss / vcurr * 1e6); // in micros

    if (fallAndRise) {
        stepPolarity = !stepPolarity;
        digitalWriteFast(pinStep, stepPolarity);
        delayMicroseconds(tDelay);
    } else {
        digitalWriteFast(pinStep, HIGH);
        delayMicroseconds(tDelay / 2.0);
        digitalWriteFast(pinStep, LOW);
        delayMicroseconds(tDelay / 2.0);
    }
}