turn on the first motor gradually until it hits 50% of its total
power. Then, the second motor will be activated until the
system is in equilibrium as close as possible to 0 radians.
The function is only called until the bias values are
determined for both motors. Once these values are
determined, the PID controller routine is called.
The code for the calibration routine looks as follows:
void acCalibMotorBias(float half1, float half2)
{
static int calibState = 0;
static int conta = 0;
static int flag = 0;
static double ramp1 = MIN_CTRL_EFFORT1;
static double ramp2 = MIN_CTRL_EFFORT2;
// Calibration state machine
switch( calibState )
{
case 0:
if( ramp1 < half1 )
{
// Ramp the first motor to 50% of the max
power possible:
ramp1 += DELTA_RAMP;
acCtrlEffortMotor1(ramp1);
}
else
{
// Set the motor #1 bias -> motor1Bias
motor1Bias = ramp1;
calibState = 1;
}
break;
case 1:
if(half2 > 0 && flag == 0){
// Ramp the first motor to 50% of the max
power possible:
ramp2 += DELTA_RAMP;
acCtrlEffortMotor2(ramp2);
}
else
{
// Set the motor #2 bias -> motor1Bias
motor2Bias = ramp2;
flag = 1;
if ( conta == 3000 )
{
calibState = 2;
biasCalibFlag = 1;
calibration is over
}
conta++;
// Indicate the
}
}
}
Tuning The Controller For Two Motors
Once again, we used Zigler and Nichols' method as a
guideline to tune the system with two motors. We first
tuned Kp and then Ki. Figure 12 shows the results for the
tuned system.
runAeroPendubotController()
!acIsMotorBiasCalib()
True
Calibration Motors
acCalibMotorBias(50%,50%)
False
Read Encoders
θ and θref
Calculate Error
e = θref -θ
Calculate Control Effort
u = acPidController(e, θ, Kp, Ki, Kd, T)
Compensate Friction
U1 = uBias1 + u
U2 = uBias2 - u
Activate Motors
acCtrlEffortMotor1(U1)
acCtrlEffortMotor2(U2)
■ FIGURE 11.
Software Flow
Diagram.
Conclusions
The first prototype of the PenAeroBot has proven to
be very useful. This platform has allowed us to explore
the implementation of a digital PID controller and it has
also taught us a lot about the robot's properties. The
Time vs Theta
0.12
0.10
Theta (rad)
0.08
0.06
0.04
0.02
0.00
01
23456789101112
Time (s)
Theta (rad) Theta ref (rad)
■ FIGURE 12. Kp=0.01, Ki=0.3, Kd=0, Tetharef =0.08 (rad).
SERVO 02.2009
57