# 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}$$:

s_s = \frac{p}{n_r}, \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
\label{eq:lin_acc_dist}

with the velocity $$v$$ being defined as:

v = a \cdot t
\label{eq:lin_acc_vel}

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)

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

(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 = \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 = \Delta s / 2$$ and redefine $$v_m$$:

v_m = \sqrt{\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);
}
}


## 4 thoughts on “Trapezoidal velocity profile for a stepper motor”

1. jy says:

Hello,

Thanks for the post. I have a couple for questions though :
1. Shouldn’t there be one more ‘digitalWriteFast’ after the time delay to complete one cycle? Else it would be the case that the actual time duration for which the pin state is held will include one pass of the for loop.
2. Any particular reason why the PWM output from the arduino was not used in this example?

2. Hi jy,

thanks for the question. in the case of the controlled board I used, a switch in polarity represents a single step. I think there is other boards that use for example only positive flanks of a signal or only negative flanks of a signal as a stepping source. In that case of course you are right about the additional digitalWrite required for correct performance. I added a small section to the code snippet for this.

Reg the PWM question: I found that for accurate stepping the write approach is more usable then PWM because with PWM it is more difficult to keep track of the exact number of steps performed but maybe I am wrong here.

All the best and sorry for the late reply!

3. Frank says:

Very useful. One question:
Shouldn’t this line: uint32_t absSteps = round(abs(deltaS) / s); // number of steps we need to do
be uint32_t absSteps = round(abs(deltaS) / ss);
(Divide by ss rather than s)

Thank you.

1. Hi Frank, you are absolutely right, I corrected the line.