Diff: main.cpp

Revision:

0:98baffd99476

Child:

1:e71cba217586

--- /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