Trapezoidal velocity profile for a stepper motor

A simple way to achieve an acceleration profile for a stepper motor (fixed step size).

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 \( \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} \):
\begin{equation}
s_s = \frac{p}{n_r}, \Delta s = s_s \cdot n_s
\end{equation}

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\):
\begin{equation}
s = (i_s + 0.5) \cdot s_s, i_s \in [0, n_s – 1]
\end{equation}

The velocity of a stepper motor is thereby translated into the delay time \(t_d\) before the next discrete step occurs:
\begin{equation}
t_d = \frac{s_s}{v}
\end{equation}

Linear acceleration phase

In the acceleration phase, our linearly accelerated velocity profile is described as:
\begin{equation}
s = \frac{1}{2} \cdot a \cdot t^2
\label{eq:lin_acc_dist}
\end{equation}
with the velocity \(v\) being defined as:
\begin{equation}
v = a \cdot t
\label{eq:lin_acc_vel}
\end{equation}
Solving this equation for \(v(s)\) leads to:
\begin{equation}
v = \sqrt{2 \cdot s \cdot a}
\end{equation}

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:
\begin{equation}
v = v_m – a \cdot (t – t_2)
\end{equation}
or solved after \((t-t_2)\)
\begin{equation}
t – t_2 = \frac{v_m – v}{a}
\end{equation}
with its distance counterpart
\begin{equation}
s = s_2 + v_m \cdot (t – t_2) – \frac{1}{2} \cdot a \cdot (t – t_2)^2
\end{equation}
or
\begin{equation}
-s = -s_2 – v_m \cdot \frac{v_m – v}{a} + \frac{(v_m – v)^2}{2 \cdot a}
\end{equation}

\begin{equation}
0 = \frac{1}{2 \cdot a} \cdot (v_m – v)^2 + \frac{-v_m}{a} \cdot (v_m – v) + (s – s_2)
\end{equation}

Solving the quadratic equation through
\begin{equation}
x_{1,2} = \frac{-b \pm \sqrt{b^2 – 4 ac }}{2a}
\end{equation}

leads to

\begin{equation}
(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} }
\end{equation}

\begin{equation}
(v_m – v)_{1,2} = v_m \pm \cdot \sqrt{v_m^2 – 2 \cdot a \cdot (s – s_2) }
\end{equation}

\begin{equation}
v = \sqrt{v_m^2 – 2 \cdot a \cdot (s- s_2)}
\end{equation}

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
\begin{equation}
s_1 = \frac{v_m^2}{2 \cdot a}
\end{equation}
and the second one as
\begin{equation}
s_2 = \Delta s – s_1
\end{equation}

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 = \Delta s / 2\) and redefine \(v_m\):
\begin{equation}
v_m = \sqrt{\Delta s \cdot a}
\end{equation}

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);
    }  
}