Servo - February 2009

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