main.cpp

00001 /**
00002  * Includes.
00003  */
00004 #include "Motor.h"
00005 #include "QEI.h"
00006 #include "PID.h"
00007 #include "IMU.h"
00008 
00009 /**
00010  * Defines.
00011  */
00012 #define BUFFER_SIZE 1024 //Used for data logging arrays
00013 #define RATE 0.01        //PID loop timing
00014 
00015 //PID tuning constants.
00016 #define L_KC 0.4312 //Forward left motor Kc
00017 #define L_TI 0.1    //Forward left motor Ti
00018 #define L_TD 0.0    //Forward left motor Td
00019 #define R_KC 0.4620 //Forward right motor Kc
00020 #define R_TI 0.1    //Forward right motor Ti
00021 #define R_TD 0.0    //Forward right motor Td
00022 
00023 //PID input/output limits.
00024 #define L_INPUT_MIN 0     //Forward left motor minimum input
00025 #define L_INPUT_MAX 3000  //Forward left motor maximum input
00026 #define L_OUTPUT_MIN 0.0  //Forward left motor minimum output
00027 #define L_OUTPUT_MAX 0.9  //Forward left motor maximum output
00028 
00029 #define R_INPUT_MIN 0     //Forward right motor input minimum
00030 #define R_INPUT_MAX 3200  //Forward right motor input maximum
00031 #define R_OUTPUT_MIN 0.0  //Forward right motor output minimum
00032 #define R_OUTPUT_MAX 0.9  //Forward right motor output maximum
00033 
00034 //Physical dimensions.
00035 #define PULSES_PER_REV        624
00036 #define WHEEL_DIAMETER        58.928 //mm
00037 #define ROTATION_DISTANCE     220.0  //mm
00038 #define REVS_PER_ROTATION     (ROTATION_DISTANCE / WHEEL_DIAMETER)
00039 #define PULSES_PER_ROTATION   (REVS_PER_ROTATION * PULSES_PER_REV)
00040 #define PULSES_PER_MM         (PULSES_PER_REV / WHEEL_DIAMETER)
00041 #define DISTANCE_PER_PULSE    (WHEEL_DIAMETER / PULSES_PER_REV)
00042 #define ENCODING              2 //Use X2 encoding
00043 #define WHEEL_DISTANCE        (ROTATION_DISTANCE / DISTANCE_PER_PULSE)
00044 
00045 /**
00046  * Function Prototypes
00047  */
00048 void initializeMotors(void);
00049 void initializePid(void);
00050 //cmd = "move", "turn"
00051 //direction = "forward", "backward", "left", "right"
00052 //length = distance in metres or angle in degrees
00053 void processCommand(char* cmd, char* direction, float length);
00054 
00055 /**
00056  * Globals
00057  */
00058 
00059 //Debug.
00060 Serial pc(USBTX, USBRX);
00061 
00062 //Motor control.
00063 Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
00064 Motor rightMotor(p22, p17, p18); //pwm, inA, inB
00065 QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
00066 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
00067 PID leftPid(L_KC, L_TI, L_TD, RATE);  //Kc, Ti, Td
00068 PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td
00069 //IMU and peripherals run at a different rate to the main PID loop.
00070 IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE);
00071 
00072 //Filesystem.
00073 LocalFileSystem local("local");
00074 
00075 //Left motor working variables.
00076 int leftPulses     = 0;
00077 int leftPrevPulses = 0;
00078 float leftVelocity = 0.0;
00079 float leftVelocityBuffer[BUFFER_SIZE];
00080 
00081 //Right motor working variables.
00082 int rightPulses     = 0;
00083 int rightPrevPulses = 0;
00084 float rightVelocity = 0.0;
00085 float rightVelocityBuffer[BUFFER_SIZE];
00086 
00087 //General working variables.
00088 float heading       = 0.0;
00089 float prevHeading   = 0.0;
00090 float degreesTurned = 0.0;
00091 float positionSetPoint = 0.0;
00092 float headingSetPoint  = 0.0;
00093 
00094 //Store the initial and end heading during a straight line section
00095 //in order to be able to correct turns.
00096 float startHeading = 0.0;
00097 float endHeading   = 0.0;
00098 
00099 //Command Parser.
00100 char  cmd0[16]; //{"move", "turn"}
00101 char  cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
00102 float cmd2 = 0; //{distance in METRES, angle in DEGREES}
00103 
00104 void initializeMotors(void) {
00105 
00106     //Set motor PWM periods to 20KHz.
00107     leftMotor.period(0.00005);
00108     leftMotor.speed(0.0);
00109 
00110     rightMotor.period(0.00005);
00111     rightMotor.speed(0.0);
00112 
00113 }
00114 
00115 void initializePid(void) {
00116 
00117     //Input and output limits have been determined
00118     //empirically with the specific motors being used.
00119     //Change appropriately for different motors.
00120     //Input  units: counts per second.
00121     //Output units: PwmOut duty cycle as %.
00122     //Default limits are for moving forward.
00123     leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX);
00124     leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX);
00125     leftPid.setMode(AUTO_MODE);
00126     rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX);
00127     rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX);
00128     rightPid.setMode(AUTO_MODE);
00129 
00130 }
00131 
00132 void processCommand(char* cmd, char* direction, float length) {
00133 
00134     //move command.
00135     if (strcmp(cmd, "move") == 0) {
00136 
00137         int i = 0; //Data log array index.
00138 
00139         //The PID controller works in the % (unsigned) domain, so we'll
00140         //need to multiply the output by -1 if our motors need
00141         //to go "backwards".
00142         int leftDirection  = 1;
00143         int rightDirection = 1;
00144 
00145         //Convert from metres into millimetres.
00146         length *= 1000;
00147         //Work out how many pulses are required to go that many millimetres.
00148         length *= PULSES_PER_MM;
00149         //Make sure we scale the number of pulses according to our encoding method.
00150         length /= ENCODING;
00151 
00152         positionSetPoint = length;
00153 
00154         if (strcmp(direction, "forward") == 0) {
00155             //Leave left and rightDirection as +ve.
00156         } else if (strcmp(cmd1, "backward") == 0) {
00157             //Change left and rightDirection to -ve.
00158             leftDirection  = -1;
00159             rightDirection = -1;
00160         }
00161 
00162         startHeading = imu.getYaw();
00163 
00164         //With left set point == right set point, the rover veers off to the
00165         //right - by slowing the right motor down slightly it goes relatively
00166         //straight.
00167         leftPid.setSetPoint(1000);
00168         rightPid.setSetPoint(975);
00169 
00170         //Keep going until we've moved the correct distance defined by the
00171         //position set point. Take the absolute value of the pulses as we might
00172         //be moving backwards.
00173         while ((abs(leftPulses) < positionSetPoint) &&
00174                 (abs(rightPulses) < positionSetPoint)) {
00175 
00176             //Get the current pulse count and subtract the previous one to
00177             //calculate the current velocity in counts per second.
00178             leftPulses = leftQei.getPulses();
00179             leftVelocity = (leftPulses - leftPrevPulses) / RATE;
00180             leftPrevPulses = leftPulses;
00181             //Use the absolute value of velocity as the PID controller works
00182             //in the % (unsigned) domain and will get confused with -ve values.
00183             leftPid.setProcessValue(fabs(leftVelocity));
00184             leftMotor.speed(leftPid.compute() * leftDirection);
00185 
00186             rightPulses = rightQei.getPulses();
00187             rightVelocity = (rightPulses - rightPrevPulses) / RATE;
00188             rightPrevPulses = rightPulses;
00189             rightPid.setProcessValue(fabs(rightVelocity));
00190             rightMotor.speed(rightPid.compute() * rightDirection);
00191 
00192             i++;
00193 
00194             wait(RATE);
00195 
00196         }
00197 
00198         leftMotor.brake();
00199         rightMotor.brake();
00200 
00201         endHeading = imu.getYaw();
00202 
00203     }
00204     //turn command.
00205     else if (strcmp(cmd0, "turn") == 0) {
00206 
00207         //The PID controller works in the % (unsigned) domain, so we'll
00208         //need to multiply the output by -1 for the motor which needs
00209         //to go "backwards".
00210         int leftDirection  = 1;
00211         int rightDirection = 1;
00212         headingSetPoint = length + (endHeading - startHeading);
00213 
00214         //In case the rover tries to [pointlessly] turn >360 degrees.
00215         if (headingSetPoint > 359.8) {
00216 
00217             headingSetPoint -= 359.8;
00218 
00219         }
00220         
00221         //Set up the right previous heading for the initial degreesTurned
00222         //calculation.
00223         prevHeading = fabs(imu.getYaw());
00224 
00225         if (strcmp(cmd1, "left") == 0) {
00226 
00227             //When turning left, the left motor needs to go backwards
00228             //while the right motor goes forwards.
00229             leftDirection  = -1;
00230 
00231         } else if (strcmp(cmd1, "right") == 0) {
00232 
00233             //When turning right, the right motor needs to go backwards
00234             //while the left motor goes forwards.
00235             rightDirection = -1;
00236 
00237         }
00238 
00239         //Turn slowly to ensure we don't overshoot the angle by missing
00240         //the relatively slow readings [in comparison to the PID loop speed]
00241         //from the IMU.
00242         leftPid.setSetPoint(500);
00243         rightPid.setSetPoint(500);
00244 
00245         //Keep turning until we've moved through the correct angle as defined
00246         //by the heading set point.
00247         while (degreesTurned < headingSetPoint) {
00248 
00249             //Get the new heading and subtract the previous heading to
00250             //determine how many more degrees we've moved through.
00251             //As we're only interested in the relative angle (as opposed to
00252             //absolute) we'll take the absolute value of the heading to avoid
00253             //any nastiness when trying to calculate angles as they jump from
00254             //179.9 to -179.9 degrees.
00255             heading = fabs(imu.getYaw());
00256             degreesTurned += fabs(heading - prevHeading);
00257             prevHeading = heading;
00258 
00259             //Get the current pulse count and subtract the previous one to
00260             //calculate the current velocity in counts per second.
00261             leftPulses = leftQei.getPulses();
00262             leftVelocity = (leftPulses - leftPrevPulses) / RATE;
00263             leftPrevPulses = leftPulses;
00264             //Use the absolute value of velocity as the PID controller works
00265             //in the % (unsigned) domain and will get confused with -ve values.
00266             leftPid.setProcessValue(fabs(leftVelocity));
00267             leftMotor.speed(leftPid.compute() * leftDirection);
00268 
00269             rightPulses = rightQei.getPulses();
00270             rightVelocity = (rightPulses - rightPrevPulses) / RATE;
00271             rightPrevPulses = rightPulses;
00272             rightPid.setProcessValue(fabs(rightVelocity));
00273             rightMotor.speed(rightPid.compute() * rightDirection);
00274 
00275             wait(RATE);
00276 
00277         }
00278 
00279         leftMotor.brake();
00280         rightMotor.brake();
00281 
00282     }
00283 
00284     //Reset working variables.
00285     leftQei.reset();
00286     rightQei.reset();
00287 
00288     leftPulses     = 0;
00289     leftPrevPulses = 0;
00290     leftVelocity   = 0.0;
00291     leftMotor.speed(0.0);
00292 
00293     rightPulses     = 0;
00294     rightPrevPulses = 0;
00295     rightVelocity   = 0.0;
00296     rightMotor.speed(0.0);
00297 
00298     leftPid.setSetPoint(0.0);
00299     leftPid.setProcessValue(0.0);
00300     rightPid.setSetPoint(0.0);
00301     rightPid.setProcessValue(0.0);
00302 
00303     positionSetPoint = 0.0;
00304     headingSetPoint  = 0.0;
00305     heading          = 0.0;
00306     prevHeading      = 0.0;
00307     degreesTurned    = 0.0;
00308 
00309 }
00310 
00311 int main() {
00312 
00313     pc.printf("Starting mbed rover test...\n");
00314 
00315     initializeMotors();
00316     initializePid();
00317 
00318     //Some delay before we start moving.
00319     wait(5);
00320 
00321     //Open the command script.
00322     FILE* commands = fopen("/local/commands.txt", "r");
00323 
00324     //Check if we were successful in opening the command script.
00325     if (commands == NULL) {
00326         pc.printf("Could not open command file...\n");
00327         exit(EXIT_FAILURE);
00328     } else {
00329         pc.printf("Succesfully opened command file!\n");
00330     }
00331 
00332     //While there's another line to read from the command script.
00333     while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) {
00334 
00335         pc.printf("%s %s %f\n", cmd0, cmd1, cmd2);
00336 
00337         processCommand(cmd0, cmd1, cmd2);
00338 
00339         wait(1);
00340 
00341     }
00342 
00343     fclose(commands);
00344 
00345 }