main.cpp

00001 /**
00002  * Drive a robot forwards or backwards by using a PID controller to vary
00003  * the PWM signal to H-bridges connected to the motors to attempt to maintain
00004  * a constant velocity.
00005  */
00006 
00007 #include "Motor.h"
00008 #include "QEI.h"
00009 #include "PID.h"
00010 
00011 Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
00012 Motor rightMotor(p22, p17, p18); //pwm, inA, inB
00013 QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
00014 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
00015 //Tuning parameters calculated from step tests;
00016 //see http://mbed.org/cookbook/PID for examples.
00017 PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td
00018 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
00019 
00020 int main() {
00021 
00022     leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
00023     rightMotor.period(0.00005);
00024 
00025     //Input and output limits have been determined
00026     //empirically with the specific motors being used.
00027     //Change appropriately for different motors.
00028     //Input  units: counts per second.
00029     //Output units: PwmOut duty cycle as %.
00030     //Default limits are for moving forward.
00031     leftPid.setInputLimits(0, 3000);
00032     leftPid.setOutputLimits(0.0, 0.9);
00033     leftPid.setMode(AUTO_MODE);
00034     rightPid.setInputLimits(0, 3200);
00035     rightPid.setOutputLimits(0.0, 0.9);
00036     rightPid.setMode(AUTO_MODE);
00037 
00038 
00039     int leftPulses      = 0; //How far the left wheel has travelled.
00040     int leftPrevPulses  = 0; //The previous reading of how far the left wheel
00041     //has travelled.
00042     float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
00043     //second.
00044     int rightPulses     = 0; //How far the right wheel has travelled.
00045     int rightPrevPulses = 0; //The previous reading of how far the right wheel
00046     //has travelled.
00047     float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
00048     //second.
00049     int distance     = 6000; //Number of pulses to travel.
00050 
00051     wait(5); //Wait a few seconds before we start moving.
00052 
00053     //Velocity to mantain in pulses per second.
00054     leftPid.setSetPoint(1000);
00055     rightPid.setSetPoint(975);
00056 
00057     while ((leftPulses < distance) || (rightPulses < distance)) {
00058 
00059         //Get the current pulse count and subtract the previous one to
00060         //calculate the current velocity in counts per second.
00061         leftPulses = leftQei.getPulses();
00062         leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
00063         leftPrevPulses = leftPulses;
00064         //Use the absolute value of velocity as the PID controller works
00065         //in the % (unsigned) domain and will get confused with -ve values.
00066         leftPid.setProcessValue(fabs(leftVelocity));
00067         leftMotor.speed(leftPid.compute());
00068 
00069         rightPulses = rightQei.getPulses();
00070         rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
00071         rightPrevPulses = rightPulses;
00072         rightPid.setProcessValue(fabs(rightVelocity));
00073         rightMotor.speed(rightPid.compute());
00074 
00075         wait(0.01);
00076 
00077     }
00078 
00079     leftMotor.brake();
00080     rightMotor.brake();
00081 
00082 }