![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Use the IMU to correct steering on the Sparkfun magician robot. Moreover, IR sensors will be used to avoid and navigate around obstacles.
Dependencies: LSM9DS0 Motordriver mbed
main.cpp
- Committer:
- awinata
- Date:
- 2015-03-11
- Revision:
- 0:98baffd99476
File content as of revision 0:98baffd99476:
#include "mbed.h" #include "motordriver.h" #include "LSM9DS0.h" /*/////// PID control gains / These values need to be adjusted in accordance with the individual / actuators (motors) either by trial and error or computation. The information / here should help: http://developer.mbed.org/cookbook/PID *//////// #define RATE 0.1 #define Kp 0.0 #define Ki 0.0 #define Kd 0.0 // SDO_XM and SDO_G are pulled up, so our addresses are: #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW // peripheral objects DigitalOut f(LED1); // forward DigitalOut b(LED2); // backward DigitalOut l(LED3); // left DigitalOut r(LED4); // right //See http://mbed.org/cookbook/Motor //Connections to dual H-brdige driver for the two drive motors Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature Motor right(p26, p25, p24, 1); AnalogIn ir_c(p20); // center IR sensor AnalogIn ir_l(p19); // left IR sensor AnalogIn ir_r(p18); // right IR sensor LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // imu sensor Serial pc(USBTX, USBRX); // usb serial for debugging // function prototypes void setup(); void initIMU(); float getHeading(); float updatePID(); // global variables int count; // counter for IR float initHeading; // initial heading float calcHeading; // calculated heading float heading[1]; // index 0 = goal, index 1 = current float headingAvg[100]; float sum; float previousError; // previous error float pidError; // error float steadyError; // steady-state error float P; // proportional error float I; // integral error float D; // derivative error float dt; // update frequency float kp; // proportional constant float ki; // integral constant float kd; // derivative constant float output; // PID controller output float motorSpeed1; // motor speed for left motor float motorSpeed2; // motor speed for right motor // function implementations void setup() { // Use the begin() function to initialize the LSM9DS0 library. // You can either call it with no parameters (the easy way): uint16_t status = imu.begin(); //Make sure communication is working pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); pc.printf("Should be 0x49D4\n\n"); } // obtains the initial heading upon bootup void initIMU() { imu.readMag(); initHeading = imu.calcHeading(); heading[0] = initHeading; heading[1] = initHeading; } // obtains an updated heading float getHeading() { imu.readMag(); // reads current value calcHeading = imu.calcHeading(); // sets current value //pc.printf("Compass Heading (in degrees): "); //pc.printf("%2f\r\n",calcHeading); //pc.printf("%2f\r\n",heading[0]); return calcHeading; } // pid controller float updatePID(float current, float target, float dt) { // calculate difference between actual and goal values pidError = target - current; if (pidError < -270) pidError += 360; if (pidError > 270) pidError -= 360; // accumulates error over time steadyError += pidError*dt; // integrator windup compensation (saturates to actuator's output) if (steadyError < -10.0) steadyError = -10.0; if (steadyError > 10.0) steadyError = 10.0; P = kp*pidError; // proportional error I = ki*steadyError; // integral error D = kd*(pidError - previousError)/dt; // derivative error // save current error previousError = pidError; float pidOutput = P + I + D; // calculate the PID output pidOutput /= 100.0; // scale it down to get it within the range of the actuator - probably needs tuning if(pidOutput < -0.1) pidOutput = -0.1; if(pidOutput > 0.1) pidOutput = 0.1; wait(dt); //pc.printf("P = %f | I = %f | D = %f | Output = %f\n",P,I,D,pidOutput); return pidOutput; } int main() { // initialization functions setup(); initIMU(); // variables dt = RATE; kp = Kp; ki = Ki; kd = Kd; motorSpeed1 = 0.4; motorSpeed2 = 0.4; while(1) { if(ir_c > 0.25f) { // Turn around if about to hit an obstacle f = 0; b = 1; l = 1; r = 0; while(count < 50) { left.speed(0.5); right.speed(-0.5); wait(0.02); count++; } count = 0; heading[0] = getHeading(); // updates new goal heading } else if(ir_l > 0.25f) { // Turn right if about to hit an obstacle f = 0; b = 0; l = 1; r = 0; while(count < 20) { left.speed(0.5); right.speed(-0.5); wait(0.02); count++; } count = 0; heading[0] = getHeading(); // updates new goal heading } else if(ir_r > 0.25f) { // Turn left if about to hit an obstacle f = 0; b = 0; l = 0; r = 1; while(count < 20) { left.speed(-0.5); right.speed(0.5); wait(0.02); count++; } count = 0; heading[0] = getHeading(); // updates new goal heading } else { // Moves forward indefinitely f = 1; b = 0; l = 0; r = 0; // average the headings to elliminate noise (LPF) // possibly needed to help smooth out sensor noise /* for(int x = 0; x < 100; x++) { headingAvg[x] = getHeading(); sum += headingAvg[x]; } heading[1] = sum/100; */ // updates new pid values heading[1] = getHeading(); output = updatePID(heading[1],heading[0],dt); // updates new motor speeds motorSpeed1 += output; motorSpeed2 -= output; // set motor speeds left.speed(motorSpeed1); right.speed(motorSpeed2); } } }