Diff: main.cpp

Revision:

1:e71cba217586

Parent:

0:98baffd99476

--- a/main.cpp	Wed Mar 11 21:57:57 2015 +0000
+++ b/main.cpp	Tue Oct 20 19:13:05 2015 +0000
@@ -1,17 +1,56 @@
 #include "mbed.h"
 #include "motordriver.h"
 #include "LSM9DS0.h"
+#include "IMUfilter.h"
+
+#define RATE  0.1
+
+// Callback to update the PID controller
+Ticker pid_updater;
+// Set a PID update rate of 10 Hz
+const float PID_RATE = 0.1;
+// Call back to update the IMU samples
+Ticker imu_updater;
+Ticker heading_reseter;
+// Set a sample rate of 200 Hz for the IMU
+const float IMU_RATE = 0.025;
+const int NUM_IMU_SAMPLES = 4;
+// Count the number of times the IMU is sampled to average every 100 samples.
+int imu_count = 0;
+Ticker dist_updater;
+const float DIST_RATE = 0.1;
+const float DIST_LIMIT = 0.5;
+
+const float INITIAL_SPEED_LEFT = 0.7;
+const float INITIAL_SPEED_RIGHT = 0.609;
+
+const float TURN_TIME = 0.55;
 
 /*/////// 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
+ / here should help: 
 *////////
-#define RATE  0.1
-#define Kp    0.0
-#define Ki    0.0
-#define Kd    0.0
+/*
+ * Done with Ziegler-Nichols method
+ 
+#define Kp    0.002
+#define Ki    0.00002
+#define Kd    0.075
+ */
+/*
+ * Done with random method from stack overflow which is more upvoted than Ziegler-Nichols
+ 
+#define Kp 0.192
+#define Ki 0.0
+#define Kd 0.048
+ */
 
+#define Kp 0.0001
+#define Ki 0.0
+#define Kd 0.00001
+/*
+ */
 // 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
@@ -25,9 +64,6 @@
 //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
 
@@ -35,28 +71,49 @@
 void setup();
 void initIMU();
 float getHeading();
-float updatePID();
+void updatePID();
+void updateIMU();
+float GetAvgHeading();
+void UpdateMotorSpeed(float pid_output);
+void updateDist();
+void headingReset();
+void turnRight();
+void turnLeft();
+void calibrateAccel();
+void turnToAngle(float angle);
 
 // 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 heading_avg[NUM_IMU_SAMPLES]; // moving average array
 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 dt; // update frequency
+
 float motorSpeed1; // motor speed for left motor
 float motorSpeed2; // motor speed for right motor
+float dist[2]; // 0 = x, 1 = y
+float vel[2]; // 0 = x, 1 = y
+float accel[2]; // 0 = x, 1 = y
+float cur_dist; // resultant distance
+
+
 
 // function implementations
 void setup()
@@ -65,16 +122,39 @@
     // You can either call it with no parameters (the easy way):
     uint16_t status = imu.begin();
 
-    //Make sure communication is working
+    // Make sure communication is working
     pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
     pc.printf("Should be 0x49D4\n\n");
+    initIMU();
+    
+    // Initialize motor speeds
+    motorSpeed1 = INITIAL_SPEED_LEFT;
+    motorSpeed2 = INITIAL_SPEED_RIGHT;
+    // Attach the PID Update routine to a callback
+    pid_updater.attach(&updatePID, PID_RATE);
+    // Attach the IMU update routine to a callback to computer a moving average
+    imu_updater.attach(&updateIMU, IMU_RATE);
+    dist_updater.attach(&updateDist, DIST_RATE);
+    // set motor speeds
+    left.speed(motorSpeed1);
+    right.speed(motorSpeed2);
 }
 
 // obtains the initial heading upon bootup
 void initIMU()
 {
     imu.readMag();
-    initHeading = imu.calcHeading();
+    imu.readAccel();
+    imu.calcBias();
+    imu.readMag();
+    wait(0.1);
+    // initial heading
+    float sum = 0;
+    for( int i = 0; i < NUM_IMU_SAMPLES; ++i) {
+        sum += getHeading();
+    }
+    float initHeading = sum/NUM_IMU_SAMPLES;
+        
     heading[0] = initHeading;
     heading[1] = initHeading;
 }
@@ -85,16 +165,20 @@
     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]);
+    // 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)
+// PID update attached to a ticker.
+void updatePID()
 {
+    float current = heading[1];
+    float target = heading[0];
+    float dt = PID_RATE;
+    // pc.printf("current=%0.2f, target=%0.2f, dt=%0.2f\r\n", current, target, dt);
     // calculate difference between actual and goal values
     pidError = target - current;
     if (pidError < -270) pidError += 360;
@@ -115,97 +199,171 @@
     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
+    pidOutput /= 300.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\r\n",P,I,D,pidOutput);
+    UpdateMotorSpeed(pidOutput);
+}
+
+void UpdateMotorSpeed(float pid_output) {
+    // updates new motor speeds
+    motorSpeed1 -= pid_output;
+    motorSpeed2 += pid_output;
+
+    // pc.printf("Wheel speed difference = %0.2f\r\n", motorSpeed1-motorSpeed2);
+    // set motor speeds
+    left.speed(motorSpeed1);
+    right.speed(motorSpeed2);
+}
+
+float GetAvgHeading() {
+    float sum = 0;
+    for( int i = 0; i < NUM_IMU_SAMPLES; ++i ) {
+        sum += heading_avg[i];
+    }
+    return sum/NUM_IMU_SAMPLES;
+}
+        
+
+// Get reach the minimum number of samples and then use a moving average filter
+// Keep the count below 2*num_samples to prevent overflow, but keep at a minimum of 1*num_samples
+void updateIMU() {
+    // Get current index into the ring buffer (heading_avg)
+    int moving_idx = imu_count % NUM_IMU_SAMPLES;
+    // Add the current heading into the buffer
+    heading_avg[moving_idx] = getHeading();
+    
+    // Keep the IMU count between num_samples & 2*num_samples
+    // Get the average heading as long as the minimum number of samples have been reached.
+    if( imu_count >= NUM_IMU_SAMPLES && imu_count <= 2*NUM_IMU_SAMPLES ) {
+        if( imu_count == 2*NUM_IMU_SAMPLES ) {
+            imu_count = NUM_IMU_SAMPLES;
+        }
+        heading[1] = GetAvgHeading();
+        // pc.printf("av_heading=%0.2f\r\n", heading[1]);
+    }
+    imu_count += 1;
+}
 
-    //pc.printf("P = %f | I = %f | D = %f | Output = %f\n",P,I,D,pidOutput);
-    return pidOutput;
+// Update acceleration, velocity, and distance vectors.
+void updateDist() {
+    // Get latest acceleration reading.
+    imu.readAccel();
+    
+    // Get acceleration vector
+    accel[0] = imu.ax - imu.abias[0];
+    accel[1] = imu.ay - imu.abias[1];
+    
+    // Get velocity vector
+    vel[0] += accel[0]*RATE;
+    vel[1] += accel[1]*RATE;
+    
+    // Get distance vector
+    dist[0] += vel[0]*RATE;
+    dist[1] += vel[1]*RATE;
+    
+    // Get resultant distance
+    // d = (dx^2 + dy^2)^(1/2)
+    cur_dist = sqrt(dist[0]*dist[0] + dist[1]*dist[1]);
+    if( cur_dist > DIST_LIMIT ) {
+        turnRight();
+        // Reset vel and dist to get accurate distance measurement to next move
+        vel[0] = 0;
+        vel[1] = 0;
+        dist[0] = 0;
+        dist[1] = 0;
+        cur_dist = 0;
+        pidError = 0;
+        previousError = 0;
+    }
+    // pc.printf("ax=%0.2f, ay=%0.2f\r\nvx=%0.2f, vy=%0.2f\r\ndx=%0.2f, dy=%0.2f, d=%0.2f\r\n", accel[0], accel[1], vel[0], vel[1], dist[0], dist[1], cur_dist);
+}
+
+void headingReset() {
+    motorSpeed1 = INITIAL_SPEED_LEFT;
+    motorSpeed2 = INITIAL_SPEED_RIGHT;
+    
+    // set motor speeds
+    left.speed(motorSpeed1);
+    right.speed(motorSpeed2);
+
+    heading[0] = getHeading();
+    heading[1] = getHeading();
+}
+
+void turnRight() {
+    
+    left.stop(0);
+    right.stop(0);
+    left.speed(-1);
+    right.speed(1);
+    
+    /*
+    int turn_ang = heading[0] + 90;
+    turn_ang = turn_ang% 360;
+    turnToAngle(turn_ang);
+    */
+    
+    wait(TURN_TIME);
+    headingReset();
+}
+
+void turnToAngle(float angle) {
+    bool hasTurned = false;
+    left.stop(0);
+    right.stop(0);
+    left.speed(-1);
+    right.speed(1);
+    while(!hasTurned) {
+        float cur_head = getHeading();
+        pc.printf("%0.2f, %0.2f\r\n", cur_head, angle);
+        if( cur_head > angle - 20 && cur_head < angle + 20 ) {
+            hasTurned = true;
+            heading[0] = cur_head;
+            left.stop(0);
+            right.stop(0);
+        }
+        
+    }
+}
+void turnLeft() {
+    left.stop(0);
+    right.stop(0);
+    left.speed(1);
+    right.speed(-1);
+    
+    /*
+    int turn_ang = heading[0] - 90;
+    turn_ang = turn_ang% 360;
+    turnToAngle(turn_ang);
+    */
+    
+    wait(TURN_TIME);
+    headingReset();
 }
 
 int main()
 {
     // initialization functions
     setup();
-    initIMU();
 
     // variables
     dt = RATE;
     kp = Kp;
     ki = Ki;
     kd = Kd;
-    motorSpeed1 = 0.4;
-    motorSpeed2 = 0.4;
+    
+    /*
+    // Attach the PID Update routine to a callback
+    pid_updater.attach(&updatePID, PID_RATE);
+    // Attach the IMU update routine to a callback to computer a moving average
+    imu_updater.attach(&updateIMU, IMU_RATE);
+    dist_updater.attach(&updateDist, DIST_RATE);
 
+    */
+    // spin in the main loop. Updates will happen asychronously.
     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