Part 7: Programming for the Encoder and Position Control
The complete source code for this tutorial can be downloaded here.
In many situations, such as with humanoid robots, it is not enough to just control the speed of the motor. When it comes to moving arms and legs, precise control of the rotor’s position is also required. This problem offers an opportunity to introduce some very basic concepts of control theory.
The controller used on the HUBO Lab Motor Controller is known as PD, which is a simplified version of PID (Proportional-Integral-Derivative). PID controllers work by calculating three things: the present error between the current output and the desired (P), the sum of the errors over time (I), and how the error is changing (D). Each of these values is then multiplied by a constant referred to as a gain and added together to produce the new output value. The process is iterative, with the last output being used to determine the new output until some final condition is met. Because of this feedback, PID control is considered closed loop.
HMC_CommonDefinitions.h
#define DEFAULT_INTPOL_SSIZE 20 // Default interpolation step size
#define DEFALT_POSCTRL_KP 15 // Default 'P' gain for Position
#define DEFALT_POSCTRL_KD 0 // Default 'D' gain for Position Control
The above constants are defined in the Position Control section of HMC_CommonDefinitions.h. The first constant, DEFAULT_INTPOL_SSIZE, represents the number of interpolation steps that will be made in order to approach the desired position. The other two constants are the gains used in the PD control.
HMC_Encoder-PosControl.h
extern INT16 EncoderValue;
extern void ReadEncoder(void);
extern void PosControlLoop(void);
The above declarations found in HMC_Encoder-PosControl.h are for a variable and two functions defined in HMC_ Encoder-PosControl.c that will be called from other source files.
HMC_Encoder-PosControl.c
void ReadEncoder(void)
{
static INT16 oldEncoderValue;
oldEncoderValue = EncoderValue;
EncoderValue = EvaRegs.T2CNT; /// Update Encoder Value
MotorParams.Pos += (sLWord)(EncoderValue - oldEncoderValue);
}
ReadEncoder() is the first function defined in HMC_Encoder-PosControl.c, and as its name implies, it is used to retrieve the position of the rotor from the encoder. Because the F2811 has a built-in timer capable of receiving and counting quadrature encoder pulses, reading the encoder value is as simple as taking the value from the appropriate register (of course, the timer has to be configured and was done so in HMC_HallEffect-PWM.c). Notice that Instead of using the encoder value directly, which is stored in a 16-bit register, the difference between it and the previously read encoder value is added to the 32-bit MotorParams.Pos variable. The reason for doing this is that it allows a greater range of position values and prevents issues with overflow.
void SetMotorPWM(sLWord newPWM)
{
if(newPWM < 0)
{
MotorParams.Direction = 0;
MotorParams.PWMSignal = -newPWM + PWM_DEADZONE;
}
else
{
MotorParams.Direction = 1;
MotorParams.PWMSignal = newPWM + PWM_DEADZONE;
}
if(MotorParams.PWMSignal >= PWM_SIGNALMAX)
MotorParams.PWMSignal = PWM_SIGNALMAX - PWM_SAFTEYGAP;
}
The next function to be defined is SetMotorPWM(), which recovers a direction and PWM signal value from the signed value produced by the PD controller. It then sets both in the MotorParams structure. The function also ensures the signal ends up positive and falls between a minimum (PWM_DEADZONE) and maximum (PWM_SIGNALMAX - PWM_SAFTEYGAP).
void PosControlLoop(void)
{
static sLWord PWMKpValue, PWMKdValue;
GpioDataRegs.GPGTOGGLE.bit.GPIOG5=1; /// Toggles LED
if(MotorParams.PosControlGo)
{
MotorParams.IntpolStepCount++;
MotorParams.NextRefPos =
(sLWord)(
(float)MotorParams.OrigRefPos + (float)
((MotorParams.IntpolStepCount * MotorParams.DeltaRefPos) /
MotorParams.IntpolStepSize));
// When count equals IntpolStepSize, disable PD control
if(MotorParams.IntpolStepCount >= MotorParams.IntpolStepSize)
MotorParams.PosControlGo = DISABLE;
}
The culminating function in this file is PosControlLoop(), which implements the PD controller. The first part of this function handles interpolating a destination for the current iteration of the control loop. Rather than trying to reach the final destination directly, the PD controller calculates several closer destinations that must be reached over a set number of steps stored in MotorParams.IntpolStepSize. Using a counter (MotorParams.IntpolStepCount) to keep track of the current step and MotorParams.DeltaRefPos, it interpolates where the rotor should be and stores this intermediate destination in MotorParams.NextRefPos. The value of MotorParams.DeltaRefPos represents the difference between the original and final destination positions of the rotor, stored in the variables MotorParams.OrigRefPos and MotorParams.DestRefPos respectively. This calculation is made by code elsewhere in the program prior to the first iteration of the PD controller.
The benefits of this approach are that it simplifies the code by eliminating time calculations and offers a set endpoint. Typically, a motor is connected to a gearing mechanism, such as a harmonic drive which can have a gearing ratio of 100:1. In such situations, the margin of tolerable error on the motor side is greatly expanded. By taking advantage of this and using a hard number of control loop iterations, problems such as calculating gains so they minimize oscillation around the desired destination are greatly reduced.
ReadEncoder(); /// Reading Encoder also updates motor's Pos
/// Actual PD Position Control
{
PWMKpValue = (MotorParams.NextRefPos - MotorParams.Pos) * MotorParams.KP;
PWMKdValue = (((MotorParams.NextRefPos -
(MotorParams.DeltaRefPos / MotorParams.IntpolStepSize)) -
MotorParams.PrevPos) - (MotorParams.NextRefPos - MotorParams.Pos)) *
MotorParams.KD;
SetMotorPWM((sLWord)((float)((PWMKpValue - PWMKdValue) / 1000)));
}
MotorParams.PrevPos = MotorParams.Pos; /// Store current position as previous
}
The second half this function includes the actual PD calculations. It starts by calling ReadEncoder() to update the position values. It then calculates the proportional and derivative errors, applies the corresponding gains, and finally forms the PWM signal value that is to be passed to SetMotorPWM(). Note that since the derivative error requires the previous proportional error, MotorParams.PrevPos exists to store the previous position. Such a variable is not needed for the previous destination because that can easily be recalculated.
HMC_Main.c
void main(void)
{
.
.
.
for(;;)
{
if(FreqFlag1kHz)
{
if(MotorParams.PWMRun)
asm(" NOP"); /// Nothing needs to be done
else if(MotorParams.PosControlRun)
PosControlLoop();
}
ReadEncoder();
FreqFlag1kHz = 0;
}
.
.
.
}
The infinite loop that run inside of main() is what handles calling each iteration of PosControlLoop(). It runs at a frequency of 1 kHz, regulated by the FreqFlag1kHz, if and only if the MotorParams.PosControlRun variable is set to ENABLE. Regardless of whether or not the position control loop is iterated, ReadEncoder() is always called and MotorParams.Pos is always updated. This happens at as frequently as the DSP will allow.
HMC_CAN.c
void ParseCAN(void)
{
if(ReceiveCAN(CMD_MBOX, RxData, &RxDLC) == TRUE) // if any CMD is received
{
switch(RxData[0])
{
case CANCMD_ZEROENC: // Reset Motor Parameters
InitializeMotorParams();
break;
.
.
.
case CANCMD_POSCONTROL: // Enable/Disable position control mode
if(RxData[1] == ENABLE)
{
// **** to prevent sudden move
MotorParams.PWMRun = DISABLE;
ReadEncoder();
MotorParams.NextRefPos = MotorParams.PrevPos = MotorParams.Pos;
MotorParams.PosControlGo = DISABLE;
MotorParams.PosControlRun = ENABLE;
// **** to prevent sudden move
// Setup Position References
MotorParams.OrigRefPos = MotorParams.NextRefPos;
// Read New Reference
MotorParams.DestRefPos = RxData[6];
MotorParams.DestRefPos = (MotorParams.DestRefPos << 8) | RxData[5];
MotorParams.DestRefPos = (MotorParams.DestRefPos << 8) | RxData[4];
// Check for sign bit, adjust MotorParams.DestRefPos if needed
if((MotorParams.DestRefPos >> 23) == 1)
MotorParams.DestRefPos =
-(sLWord)(MotorParams.DestRefPos & 0x007FFFFF);
MotorParams.DeltaRefPos =
MotorParams.DestRefPos - MotorParams.OrigRefPos;
MotorParams.IntpolStepCount = 0;
MotorParams.PosControlGo = ENABLE;
}
else if(RxData[1] == DISABLE)
{
MotorParams.PosControlRun = DISABLE;
MotorParams.PWMSignal = 0;
MotorParams.PWMRun = DISABLE;
}
break;
}
}
}
There are two CAN commands that relate to the encoder and position control. The first command simply resets the motor’s position values via InitializeMotorParams(). This is useful and often necessary when using position control after running the motor in direct PWM mode (which can cause the position to reach very high numbers).
To activate position control, the second command is used. Similar to how the direct PWM command works, the second byte of the position control command enables or disables the position control loop by setting MotorParams.PosControlRun. Disabling the position control loop also causes MotorParams.PWMSignal to be set zero, which effectively stops the motor.
When enabled, the position reference variables are re-initialized and the MotorParams.DestRefPos is set. Its value comes from fifth, sixth, and seventh bytes of the CAN message (the third and fourth bytes are ignored). These are combined to form a 24-bit signed number representing a desired encoder position, with the seventh byte being the most significant. After MotorParams.DeltaRefPos is calculated and MotorParams.IntpolStepCount is reset, MotorParams.PosControlGo is enabled.
It was not made clear earlier, but there are two distinct ‘gate keeping’ variables related to position control, MotorParams.PosControlRun and MotorParams.PosControlGo. PosControlGo controls whether or not a new destination is interpolated in PosControlLoop(). Disabling this, however, does not disable the PD controller. That is accomplished via PosControlRun. When the PD controller is active but interpolation is not, the controller will try to maintain the last destination reference it sought. This functionality proves a necessary when there is a torque applied to the motor that could externally move it off position.
Testing the Results:
As with the other tests, start by sending a CAN message containing the motor enable command. Although not necessary, the zero encoder command can be sent by simply sending a CAN message with the first byte containing 0x0A. Now send the position control command. This requires the first byte of the message to contain the value 0x0E (the command identifier), the second byte to contain a value of 0x01 (enable/disable), and a random value for the fifth, sixth, and seventh bytes (although when first testing, it is recommended that the fifth and seventh bytes be 0x00). Once the command is sent, the rotor should spin for a momment and come to a stop. Change the three position value bytes to be all 0x00. The rotor should spin back in the other direction, returning to the zero position.
There are a couple of things to keep in mind when performing this test. For best results, some sort of indicator should be drawn on or attached to the rotor so that its position can be tracked. Also, it may be noticed that the rotor can be manually moved off position a great deal before resistance is felt. The reason for this is that the default gains used in the code are set with the assumption that the motor is connected to a gearing mechanism and therefore some error is tolerable. Because of this, the position the motor stops in may not be exactly the same every time. If desired, the gains can be modified to provide some compensation.
Final Notes
The HUBO Lab Motor Controller was designed to operate in a multi-motor system coordinated by one central computer/controller. For the sake of simplicity, much of the original code from which this tutorial is adapted was modified to remove unnecessary functionality and reworked to make understanding it as easy as possible. While many conceptual elements remained unchanged, the differences are significant enough to warrant study.
