main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "LSM9DS0.h"
00004 
00005 /*/////// PID control gains
00006  / These values need to be adjusted in accordance with the individual
00007  / actuators (motors) either by trial and error or computation. The information
00008  / here should help: http://developer.mbed.org/cookbook/PID
00009 *////////
00010 #define RATE  0.1
00011 #define Kp    0.0
00012 #define Ki    0.0
00013 #define Kd    0.0
00014 
00015 // SDO_XM and SDO_G are pulled up, so our addresses are:
00016 #define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
00017 #define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
00018 
00019 // peripheral objects
00020 DigitalOut f(LED1); // forward
00021 DigitalOut b(LED2); // backward
00022 DigitalOut l(LED3); // left
00023 DigitalOut r(LED4); // right
00024 //See http://mbed.org/cookbook/Motor
00025 //Connections to dual H-brdige driver for the two drive motors
00026 Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
00027 Motor right(p26, p25, p24, 1);
00028 AnalogIn ir_c(p20); // center IR sensor
00029 AnalogIn ir_l(p19); // left IR sensor
00030 AnalogIn ir_r(p18); // right IR sensor
00031 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // imu sensor
00032 Serial pc(USBTX, USBRX); // usb serial for debugging
00033 
00034 // function prototypes
00035 void setup();
00036 void initIMU();
00037 float getHeading();
00038 float updatePID();
00039 
00040 // global variables
00041 int count; // counter for IR
00042 float initHeading; // initial heading
00043 float calcHeading; // calculated heading
00044 float heading[1]; // index 0 = goal, index 1 = current
00045 float headingAvg[100];
00046 float sum;
00047 float previousError; // previous error
00048 float pidError; // error
00049 float steadyError; // steady-state error
00050 float P; // proportional error
00051 float I; // integral error
00052 float D; // derivative error
00053 float dt; // update frequency
00054 float kp; // proportional constant
00055 float ki; // integral constant
00056 float kd; // derivative constant
00057 float output; // PID controller output
00058 float motorSpeed1; // motor speed for left motor
00059 float motorSpeed2; // motor speed for right motor
00060 
00061 // function implementations
00062 void setup()
00063 {
00064     // Use the begin() function to initialize the LSM9DS0 library.
00065     // You can either call it with no parameters (the easy way):
00066     uint16_t status = imu.begin();
00067 
00068     //Make sure communication is working
00069     pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
00070     pc.printf("Should be 0x49D4\n\n");
00071 }
00072 
00073 // obtains the initial heading upon bootup
00074 void initIMU()
00075 {
00076     imu.readMag();
00077     initHeading = imu.calcHeading();
00078     heading[0] = initHeading;
00079     heading[1] = initHeading;
00080 }
00081 
00082 // obtains an updated heading
00083 float getHeading()
00084 {
00085     imu.readMag(); // reads current value
00086     calcHeading = imu.calcHeading(); // sets current value
00087 
00088     //pc.printf("Compass Heading (in degrees): ");
00089     //pc.printf("%2f\r\n",calcHeading);
00090     //pc.printf("%2f\r\n",heading[0]);
00091 
00092     return calcHeading;
00093 }
00094 
00095 // pid controller
00096 float updatePID(float current, float target, float dt)
00097 {
00098     // calculate difference between actual and goal values
00099     pidError = target - current;
00100     if (pidError < -270) pidError += 360;
00101     if (pidError >  270) pidError -= 360;
00102 
00103     // accumulates error over time
00104     steadyError += pidError*dt;
00105 
00106     // integrator windup compensation (saturates to actuator's output)
00107     if (steadyError < -10.0) steadyError = -10.0;
00108     if (steadyError > 10.0) steadyError = 10.0;
00109 
00110     P = kp*pidError; // proportional error
00111     I = ki*steadyError; // integral error
00112     D = kd*(pidError - previousError)/dt;  // derivative error
00113 
00114     // save current error
00115     previousError = pidError;
00116 
00117     float pidOutput = P + I + D;  // calculate the PID output
00118     pidOutput /= 100.0;  // scale it down to get it within the range of the actuator - probably needs tuning
00119     if(pidOutput < -0.1) pidOutput = -0.1;
00120     if(pidOutput > 0.1) pidOutput = 0.1;
00121 
00122     wait(dt);
00123 
00124     //pc.printf("P = %f | I = %f | D = %f | Output = %f\n",P,I,D,pidOutput);
00125     return pidOutput;
00126 }
00127 
00128 int main()
00129 {
00130     // initialization functions
00131     setup();
00132     initIMU();
00133 
00134     // variables
00135     dt = RATE;
00136     kp = Kp;
00137     ki = Ki;
00138     kd = Kd;
00139     motorSpeed1 = 0.4;
00140     motorSpeed2 = 0.4;
00141 
00142     while(1) {
00143         if(ir_c > 0.25f) { // Turn around if about to hit an obstacle
00144             f = 0;
00145             b = 1;
00146             l = 1;
00147             r = 0;
00148             while(count < 50) {
00149                 left.speed(0.5);
00150                 right.speed(-0.5);
00151                 wait(0.02);
00152                 count++;
00153             }
00154             count = 0;
00155             heading[0] = getHeading(); // updates new goal heading
00156         } else if(ir_l > 0.25f) { // Turn right if about to hit an obstacle
00157             f = 0;
00158             b = 0;
00159             l = 1;
00160             r = 0;
00161             while(count < 20) {
00162                 left.speed(0.5);
00163                 right.speed(-0.5);
00164                 wait(0.02);
00165                 count++;
00166             }
00167             count = 0;
00168             heading[0] = getHeading(); // updates new goal heading
00169         } else if(ir_r > 0.25f) { // Turn left if about to hit an obstacle
00170             f = 0;
00171             b = 0;
00172             l = 0;
00173             r = 1;
00174             while(count < 20) {
00175                 left.speed(-0.5);
00176                 right.speed(0.5);
00177                 wait(0.02);
00178                 count++;
00179             }
00180             count = 0;
00181             heading[0] = getHeading(); // updates new goal heading
00182         } else { // Moves forward indefinitely
00183             f = 1;
00184             b = 0;
00185             l = 0;
00186             r = 0;
00187             
00188             // average the headings to elliminate noise (LPF)
00189             // possibly needed to help smooth out sensor noise
00190             /*
00191             for(int x = 0; x < 100; x++) {
00192                 headingAvg[x] = getHeading();
00193                 sum += headingAvg[x];
00194                 }
00195             heading[1] = sum/100;
00196             */
00197             
00198             // updates new pid values
00199             heading[1] = getHeading();
00200             output = updatePID(heading[1],heading[0],dt);
00201             
00202             // updates new motor speeds
00203             motorSpeed1 += output;
00204             motorSpeed2 -= output;
00205 
00206             // set motor speeds
00207             left.speed(motorSpeed1);
00208             right.speed(motorSpeed2);
00209         }
00210     }
00211 }