Carden Bagwell / Mbed 2 deprecated Robot localization using an IMU with PID control

Code used to localize a robot using IMU with PID control

Dependencies:   IMUfilter LSM9DS0 Motordriver mbed

Fork of Robot_feedback_ir by Adrian Winata

main.cpp@0:98baffd99476, 2015-03-11 (annotated)

Committer:
awinata
Date:
Wed Mar 11 21:57:57 2015 +0000
Revision:
0:98baffd99476
Child:
1:e71cba217586
Initial release.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
awinata 0:98baffd99476 1 #include "mbed.h"
awinata 0:98baffd99476 2 #include "motordriver.h"
awinata 0:98baffd99476 3 #include "LSM9DS0.h"
awinata 0:98baffd99476 4
awinata 0:98baffd99476 5 /*/////// PID control gains
awinata 0:98baffd99476 6 / These values need to be adjusted in accordance with the individual
awinata 0:98baffd99476 7 / actuators (motors) either by trial and error or computation. The information
awinata 0:98baffd99476 8 / here should help: http://developer.mbed.org/cookbook/PID
awinata 0:98baffd99476 9 *////////
awinata 0:98baffd99476 10 #define RATE 0.1
awinata 0:98baffd99476 11 #define Kp 0.0
awinata 0:98baffd99476 12 #define Ki 0.0
awinata 0:98baffd99476 13 #define Kd 0.0
awinata 0:98baffd99476 14
awinata 0:98baffd99476 15 // SDO_XM and SDO_G are pulled up, so our addresses are:
awinata 0:98baffd99476 16 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
awinata 0:98baffd99476 17 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
awinata 0:98baffd99476 18
awinata 0:98baffd99476 19 // peripheral objects
awinata 0:98baffd99476 20 DigitalOut f(LED1); // forward
awinata 0:98baffd99476 21 DigitalOut b(LED2); // backward
awinata 0:98baffd99476 22 DigitalOut l(LED3); // left
awinata 0:98baffd99476 23 DigitalOut r(LED4); // right
awinata 0:98baffd99476 24 //See http://mbed.org/cookbook/Motor
awinata 0:98baffd99476 25 //Connections to dual H-brdige driver for the two drive motors
awinata 0:98baffd99476 26 Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
awinata 0:98baffd99476 27 Motor right(p26, p25, p24, 1);
awinata 0:98baffd99476 28 AnalogIn ir_c(p20); // center IR sensor
awinata 0:98baffd99476 29 AnalogIn ir_l(p19); // left IR sensor
awinata 0:98baffd99476 30 AnalogIn ir_r(p18); // right IR sensor
awinata 0:98baffd99476 31 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // imu sensor
awinata 0:98baffd99476 32 Serial pc(USBTX, USBRX); // usb serial for debugging
awinata 0:98baffd99476 33
awinata 0:98baffd99476 34 // function prototypes
awinata 0:98baffd99476 35 void setup();
awinata 0:98baffd99476 36 void initIMU();
awinata 0:98baffd99476 37 float getHeading();
awinata 0:98baffd99476 38 float updatePID();
awinata 0:98baffd99476 39
awinata 0:98baffd99476 40 // global variables
awinata 0:98baffd99476 41 int count; // counter for IR
awinata 0:98baffd99476 42 float initHeading; // initial heading
awinata 0:98baffd99476 43 float calcHeading; // calculated heading
awinata 0:98baffd99476 44 float heading[1]; // index 0 = goal, index 1 = current
awinata 0:98baffd99476 45 float headingAvg[100];
awinata 0:98baffd99476 46 float sum;
awinata 0:98baffd99476 47 float previousError; // previous error
awinata 0:98baffd99476 48 float pidError; // error
awinata 0:98baffd99476 49 float steadyError; // steady-state error
awinata 0:98baffd99476 50 float P; // proportional error
awinata 0:98baffd99476 51 float I; // integral error
awinata 0:98baffd99476 52 float D; // derivative error
awinata 0:98baffd99476 53 float dt; // update frequency
awinata 0:98baffd99476 54 float kp; // proportional constant
awinata 0:98baffd99476 55 float ki; // integral constant
awinata 0:98baffd99476 56 float kd; // derivative constant
awinata 0:98baffd99476 57 float output; // PID controller output
awinata 0:98baffd99476 58 float motorSpeed1; // motor speed for left motor
awinata 0:98baffd99476 59 float motorSpeed2; // motor speed for right motor
awinata 0:98baffd99476 60
awinata 0:98baffd99476 61 // function implementations
awinata 0:98baffd99476 62 void setup()
awinata 0:98baffd99476 63 {
awinata 0:98baffd99476 64 // Use the begin() function to initialize the LSM9DS0 library.
awinata 0:98baffd99476 65 // You can either call it with no parameters (the easy way):
awinata 0:98baffd99476 66 uint16_t status = imu.begin();
awinata 0:98baffd99476 67
awinata 0:98baffd99476 68 //Make sure communication is working
awinata 0:98baffd99476 69 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
awinata 0:98baffd99476 70 pc.printf("Should be 0x49D4\n\n");
awinata 0:98baffd99476 71 }
awinata 0:98baffd99476 72
awinata 0:98baffd99476 73 // obtains the initial heading upon bootup
awinata 0:98baffd99476 74 void initIMU()
awinata 0:98baffd99476 75 {
awinata 0:98baffd99476 76 imu.readMag();
awinata 0:98baffd99476 77 initHeading = imu.calcHeading();
awinata 0:98baffd99476 78 heading[0] = initHeading;
awinata 0:98baffd99476 79 heading[1] = initHeading;
awinata 0:98baffd99476 80 }
awinata 0:98baffd99476 81
awinata 0:98baffd99476 82 // obtains an updated heading
awinata 0:98baffd99476 83 float getHeading()
awinata 0:98baffd99476 84 {
awinata 0:98baffd99476 85 imu.readMag(); // reads current value
awinata 0:98baffd99476 86 calcHeading = imu.calcHeading(); // sets current value
awinata 0:98baffd99476 87
awinata 0:98baffd99476 88 //pc.printf("Compass Heading (in degrees): ");
awinata 0:98baffd99476 89 //pc.printf("%2f\r\n",calcHeading);
awinata 0:98baffd99476 90 //pc.printf("%2f\r\n",heading[0]);
awinata 0:98baffd99476 91
awinata 0:98baffd99476 92 return calcHeading;
awinata 0:98baffd99476 93 }
awinata 0:98baffd99476 94
awinata 0:98baffd99476 95 // pid controller
awinata 0:98baffd99476 96 float updatePID(float current, float target, float dt)
awinata 0:98baffd99476 97 {
awinata 0:98baffd99476 98 // calculate difference between actual and goal values
awinata 0:98baffd99476 99 pidError = target - current;
awinata 0:98baffd99476 100 if (pidError < -270) pidError += 360;
awinata 0:98baffd99476 101 if (pidError > 270) pidError -= 360;
awinata 0:98baffd99476 102
awinata 0:98baffd99476 103 // accumulates error over time
awinata 0:98baffd99476 104 steadyError += pidError*dt;
awinata 0:98baffd99476 105
awinata 0:98baffd99476 106 // integrator windup compensation (saturates to actuator's output)
awinata 0:98baffd99476 107 if (steadyError < -10.0) steadyError = -10.0;
awinata 0:98baffd99476 108 if (steadyError > 10.0) steadyError = 10.0;
awinata 0:98baffd99476 109
awinata 0:98baffd99476 110 P = kp*pidError; // proportional error
awinata 0:98baffd99476 111 I = ki*steadyError; // integral error
awinata 0:98baffd99476 112 D = kd*(pidError - previousError)/dt; // derivative error
awinata 0:98baffd99476 113
awinata 0:98baffd99476 114 // save current error
awinata 0:98baffd99476 115 previousError = pidError;
awinata 0:98baffd99476 116
awinata 0:98baffd99476 117 float pidOutput = P + I + D; // calculate the PID output
awinata 0:98baffd99476 118 pidOutput /= 100.0; // scale it down to get it within the range of the actuator - probably needs tuning
awinata 0:98baffd99476 119 if(pidOutput < -0.1) pidOutput = -0.1;
awinata 0:98baffd99476 120 if(pidOutput > 0.1) pidOutput = 0.1;
awinata 0:98baffd99476 121
awinata 0:98baffd99476 122 wait(dt);
awinata 0:98baffd99476 123
awinata 0:98baffd99476 124 //pc.printf("P = %f | I = %f | D = %f | Output = %f\n",P,I,D,pidOutput);
awinata 0:98baffd99476 125 return pidOutput;
awinata 0:98baffd99476 126 }
awinata 0:98baffd99476 127
awinata 0:98baffd99476 128 int main()
awinata 0:98baffd99476 129 {
awinata 0:98baffd99476 130 // initialization functions
awinata 0:98baffd99476 131 setup();
awinata 0:98baffd99476 132 initIMU();
awinata 0:98baffd99476 133
awinata 0:98baffd99476 134 // variables
awinata 0:98baffd99476 135 dt = RATE;
awinata 0:98baffd99476 136 kp = Kp;
awinata 0:98baffd99476 137 ki = Ki;
awinata 0:98baffd99476 138 kd = Kd;
awinata 0:98baffd99476 139 motorSpeed1 = 0.4;
awinata 0:98baffd99476 140 motorSpeed2 = 0.4;
awinata 0:98baffd99476 141
awinata 0:98baffd99476 142 while(1) {
awinata 0:98baffd99476 143 if(ir_c > 0.25f) { // Turn around if about to hit an obstacle
awinata 0:98baffd99476 144 f = 0;
awinata 0:98baffd99476 145 b = 1;
awinata 0:98baffd99476 146 l = 1;
awinata 0:98baffd99476 147 r = 0;
awinata 0:98baffd99476 148 while(count < 50) {
awinata 0:98baffd99476 149 left.speed(0.5);
awinata 0:98baffd99476 150 right.speed(-0.5);
awinata 0:98baffd99476 151 wait(0.02);
awinata 0:98baffd99476 152 count++;
awinata 0:98baffd99476 153 }
awinata 0:98baffd99476 154 count = 0;
awinata 0:98baffd99476 155 heading[0] = getHeading(); // updates new goal heading
awinata 0:98baffd99476 156 } else if(ir_l > 0.25f) { // Turn right if about to hit an obstacle
awinata 0:98baffd99476 157 f = 0;
awinata 0:98baffd99476 158 b = 0;
awinata 0:98baffd99476 159 l = 1;
awinata 0:98baffd99476 160 r = 0;
awinata 0:98baffd99476 161 while(count < 20) {
awinata 0:98baffd99476 162 left.speed(0.5);
awinata 0:98baffd99476 163 right.speed(-0.5);
awinata 0:98baffd99476 164 wait(0.02);
awinata 0:98baffd99476 165 count++;
awinata 0:98baffd99476 166 }
awinata 0:98baffd99476 167 count = 0;
awinata 0:98baffd99476 168 heading[0] = getHeading(); // updates new goal heading
awinata 0:98baffd99476 169 } else if(ir_r > 0.25f) { // Turn left if about to hit an obstacle
awinata 0:98baffd99476 170 f = 0;
awinata 0:98baffd99476 171 b = 0;
awinata 0:98baffd99476 172 l = 0;
awinata 0:98baffd99476 173 r = 1;
awinata 0:98baffd99476 174 while(count < 20) {
awinata 0:98baffd99476 175 left.speed(-0.5);
awinata 0:98baffd99476 176 right.speed(0.5);
awinata 0:98baffd99476 177 wait(0.02);
awinata 0:98baffd99476 178 count++;
awinata 0:98baffd99476 179 }
awinata 0:98baffd99476 180 count = 0;
awinata 0:98baffd99476 181 heading[0] = getHeading(); // updates new goal heading
awinata 0:98baffd99476 182 } else { // Moves forward indefinitely
awinata 0:98baffd99476 183 f = 1;
awinata 0:98baffd99476 184 b = 0;
awinata 0:98baffd99476 185 l = 0;
awinata 0:98baffd99476 186 r = 0;
awinata 0:98baffd99476 187
awinata 0:98baffd99476 188 // average the headings to elliminate noise (LPF)
awinata 0:98baffd99476 189 // possibly needed to help smooth out sensor noise
awinata 0:98baffd99476 190 /*
awinata 0:98baffd99476 191 for(int x = 0; x < 100; x++) {
awinata 0:98baffd99476 192 headingAvg[x] = getHeading();
awinata 0:98baffd99476 193 sum += headingAvg[x];
awinata 0:98baffd99476 194 }
awinata 0:98baffd99476 195 heading[1] = sum/100;
awinata 0:98baffd99476 196 */
awinata 0:98baffd99476 197
awinata 0:98baffd99476 198 // updates new pid values
awinata 0:98baffd99476 199 heading[1] = getHeading();
awinata 0:98baffd99476 200 output = updatePID(heading[1],heading[0],dt);
awinata 0:98baffd99476 201
awinata 0:98baffd99476 202 // updates new motor speeds
awinata 0:98baffd99476 203 motorSpeed1 += output;
awinata 0:98baffd99476 204 motorSpeed2 -= output;
awinata 0:98baffd99476 205
awinata 0:98baffd99476 206 // set motor speeds
awinata 0:98baffd99476 207 left.speed(motorSpeed1);
awinata 0:98baffd99476 208 right.speed(motorSpeed2);
awinata 0:98baffd99476 209 }
awinata 0:98baffd99476 210 }
awinata 0:98baffd99476 211 }