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

Files at this revision

API Documentation at this revision

Comitter:
awinata
Date:
Wed Mar 11 21:57:57 2015 +0000
Commit message:
Initial release.

Changed in this revision

LSM9DS0.lib Show annotated file Show diff for this revision Revisions of this file
Motordriver.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 98baffd99476 LSM9DS0.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS0.lib	Wed Mar 11 21:57:57 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aswild/code/LSM9DS0/#5556e6fb99f5
diff -r 000000000000 -r 98baffd99476 Motordriver.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motordriver.lib	Wed Mar 11 21:57:57 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/littlexc/code/Motordriver/#3110b9209d3c
diff -r 000000000000 -r 98baffd99476 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 11 21:57:57 2015 +0000
@@ -0,0 +1,211 @@
+#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);
+        }
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r 98baffd99476 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 11 21:57:57 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7e07b6fb45cf
\ No newline at end of file