Velocity Control of the OpenServo V3 with Back-EMF only
The new OpenServoV3 boards offer a new nifty feature for measuring the velocity of the driven motor without any additional sensors. The measurement principle is called Back-EMF (Back ElectroMotive Force). Here the electro-magnetic field which induces a voltage back to the turning coil is measurement. While supplying the motor with the needed current most of the time, the supply voltage is switched of periodically for a very short interval. For this interval the motor is used as a generator and the generated voltage is measured. This new feature provides the possibility to control the motor velocity without any additional sensor. Based on the current P(I)D algorithm for position control of the OpenServo I've implemented a PI+FF controller for velocity control like proposed by Barry Carter in this thread.
Technical Terms
Some readers may not be familiar with the technical terms in the following document so I will start with a short description of the important ones:
Setpoint velocity is the velocity which is commanded to the controller. The algorithm tries to drive the motor in such a way that the difference between setpoint and actual velocity vanishes.
Actuating variable is the output signal of the controller. In our case it's the motor PWM.
Motor PWM is the pulse width modulated signal which drives the H-bridge amplifier. It can be seen as proportional to the voltage applied to the dc motor.
The Algorithm
First a short description of each component of the PI+FF algorithm:
P is the proportional component. It contributes to the actuating variable (motor PWM in our case) with the weighted difference between setpoint velocity and actual velocity.
I is the integral component. It sums up the difference between setpoint and measured velocity of the past. If there is a steady difference the integrator will increase over time until the steady error vanishes.
FF is the feed forward component. It contributes to the acuating variable proportional to the setpoint velocity. This takes account to the fact that we need a certain motor PWM to reach a given velocity even in the absence of any disturbance. This component provides the biggest proportion to the actuating variable while the P and I components "only" smoothing out the disturbance like external load.
Another function you need if you implement an I-component is an anti-windup. It's the maximum value the integrator can charge to compensate a steady error. If this limitation is omitted the integrator charges to very high values whenever the actuating variable exceeds it's natural limit. For example if we command a jump from zero to a high velocity the motor PWM will most likely exceeds it's maximum value until the motor has reached the setpoint velocity (the real signal to the motor is truncated of course to protect the hardware). Meanwhile the I-component will still increase because of the persisting difference between setpoint and measured velocity. When the motor is reaching the setpoint velocity, the P-component vanishes and the FF-component should be exactly the PWM value needed to hold the setpoint velocity but the I-component causes an overshoot. To reduce this effect the I-component has to be limited.
Implementation
The complete implementation can be found on the CVS in /OpenServo/AVR_VelocityControl.
1 // The proportional component to the PD+FF is the velocity error.
2 p_component = seek_velocity - current_velocity;
3 // Increment the integral component
4 i_component += (int32_t) p_component * (int32_t) i_gain;
5 // Saturate the integrator for anti wind-up
6 if (i_component > anti_windup) i_component = anti_windup;
7 if (i_component < -anti_windup) i_component = -anti_windup;
8 // Start with zero PWM output.
9 pwm_output = 0;
10 // Apply the feed forward component of the PWM output.
11 pwm_output += (int32_t) seek_velocity * (int32_t) feed_forward_gain;
12 // Apply the proportional component of the PWM output.
13 pwm_output += (int32_t) p_component * (int32_t) p_gain;
14 // Apply the integral component of the PWM output.
15 pwm_output += i_component;
16 // Shift by 8 to account for the multiply by the 8:8 fixed point gain values.
17 pwm_output >>= 8;
Results
In the following plot you can see a comparison between the behavior of the controlled and the uncontrolled motor.
The green curve is the velocity of the controlled motor (velocity is commanded). At time step 100 the setpoint velocity is set to 150 and at time step 300 it's reset to zero again. The blue curve shows the velocity of the uncontrolled motor (PWM is commanded). At time step 100 the motor PWM is set to a value which correspond to a velocity of 150 (I've just tried different PWM values). At timestep 300 it's reset to zero again.
Discussion
For a further discussion of the velocity control see the following thread: