Diff: main.cpp

Revision:

4:ec2978ff7a1e

Parent:

3:ec0efa222dfa

Child:

5:03074e90ef7a

--- a/main.cpp	Tue Nov 15 14:33:49 2016 +0000
+++ b/main.cpp	Thu Nov 17 19:41:38 2016 +0000
@@ -1,8 +1,13 @@
-//Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
+// =========================================================================
+// ES456 Autonomous Vehicles
+// Template for MadPulse Vehicle Control
+// Dawkins, Piper - Nov 2016
+// =========================================================================
 
 #define LOG_RATE 25.0
 #define LOOP_RATE 200.0
 #define CMD_TIMEOUT 1.0
+#define WHL_RADIUS  0.7188    // Wheel radius (inches)
 #define GEAR_RATIO (1/2.75)
 #define PI 3.14159
 #include "mbed.h"
@@ -11,12 +16,10 @@
 #include "ServoIn.h"
 #include "ServoOut.h"
 
-
 //I2C    i2c(p9, p10); // SDA, SCL
 BNO055 imu(p9, p10);
 
-
-int left;
+void he_callback();
 float saturateCmd(float cmd);
 void menuFunction(Serial *port);
 DigitalOut status_LED(LED1);
@@ -45,23 +48,24 @@
 float arm_clock;
 bool str_cond,thr_cond,run_ctrl,log_data;
 
-void he_callback()
-{
-    hall_LED = !hall_LED;
+// User defined variables
+float distance;
+float steering_offset;
+float hdg_comp, hdg_gyro; 
+float hdg_target;
+float K;
 
-    dt_hall = t.read()-t_hall;
-    t_hall = t.read();
-}
-
+// ============================================
+// Main Program
+// ============================================
 int main()
 {
 
     pc.baud(115200);
     xbee.baud(115200);
 
-    he_sensor.rise(&he_callback);
+    he_sensor.rise(&he_callback); // Set hall effect speed sensor interrupt
 
-    left = 0;
     str_cmd = 0;
     str=0;
     thr=0;
@@ -108,27 +112,34 @@
             auto_LED = !auto_LED;
             wait(0.5);
         }
-    }
+    } // imu.check
 
-    pc.printf("ES456 Vehcile Program\r\n");
+    
+    // Initialize user defined variables  
+    distance = 0;  // Initialize distance
+    hdg_gyro = 0;  // Initialize gyro heading 
+    steering_offset = -.12;
+    K = 0.1;  // Heading control gain
 
+    
+    // ============================================
+    // Control Loop
+    // ============================================
     while(1) {
         
-        //menuFunction(&pc);
-        menuFunction(&xbee);
-        
-
-        if(CH1.servoPulse == 0 || CH2.servoPulse ==0) { //RC Reciever must be connected For Car to be armed
+        // *******************************************
+        // Check failsafe conditions
+        // *******************************************
+        // Arm car when RC Reciever is connected
+        if(CH1.servoPulse == 0 || CH2.servoPulse ==0) {
             armed = false;
         } else {
             armed = true;
         }
-
+        // -------------------------------------------
+        // Enable auto control when throttle is zero
         str_cond = (CH1.servoPulse > 1800); // If steering is full right
         thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero
-        //pc.printf("Cond 1: %d Cond 2: %d\r\n",str_cond,thr_cond);
-
-        // pc.printf("Timeer: %f \r\n",t.read()-arm_clock);
         if(str_cond&thr_cond) {     // Both of the above conditions must be met
             if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds
                 Steer.write((int)(1500.0));
@@ -136,31 +147,76 @@
             }
         } else {
             arm_clock = t.read();
+        }       
+        // ------------------------------------------
+        // Check PC terminal for run command
+        //menuFunction(&pc);
+        menuFunction(&xbee);
+        
+        // *******************************************
+        // Get Sensor Data
+        // *******************************************
+        // Compute speed from hall effect sensor
+        if(t.read()-t_hall < 0.2) {
+            //speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall);  // meters/sec ?
+            speed = WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall);   // inches/sec
+        } else {
+            speed = 0;
         }
+        // ------------------------------------------
+        // IMU data
+        //imu.get_angles();
+        imu.get_accel();
+        imu.get_gyro();
+        imu.get_mag();
 
+ 
+        // *******************************************
+        // Begin Control
+        // *******************************************
+ 
         if(armed) { // Is System Armed
             armed_LED = 1;
 
-            if(auto_ctrl) { // Armed and in Auto Control
+            if(auto_ctrl) { // Armed and Auto Control enabled
                 auto_LED = 1; // Turn on LED to indicate Auto Control
 
-                if(run_ctrl) {
+                if(run_ctrl) { // Armed, in Auto Control enabled, and Run command recieved
 
                     float run_time = t.read()-t_run;
-                    if(run_time > 0 && run_time <= 1) {
-                        thr_cmd = 0.2;
-                        str_cmd = 0.0;
-                    } else if(run_time >1 && run_time< 3) {
-                        thr_cmd = 0.2;
-                        str_cmd = -1.0;
-                    } else {
+ 
+                    //Integrate speed to determine distance (inches)
+                    distance = distance + speed *dt;
+                    
+                    // Heading via magnetometer (deg)
+                    hdg_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x);
+                    
+                    // Heading via integrated gyro
+                    hdg_gyro = hdg_gyro + imu.gyro.z * dt;  
+                                       
+                    if(distance <= 60 ) {
+                        thr_cmd = 0.3;
+                        //str_cmd = 0;
+                        hdg_target = 0;
+                        str_cmd = K * (hdg_target - hdg_gyro);
+                    } else if(distance > 60 && distance <= 120) {
+                        thr_cmd = 0.3;
+                        hdg_target = 90;
+                        str_cmd = K * (hdg_target - hdg_gyro); 
+                    } else {  // Run completed
                         thr_cmd = 0;
                         str_cmd = 0;
+                        distance = 0;  // Reset distance for next run
+                        run_ctrl = false; // Turn off run control
                         log_data = false;
                     }
+                    
+                    // Compensate for steering offset
+                    str_cmd = str_cmd + steering_offset;
 
                 } // End if run_ctrl
 
+                // Write steering and throttle commands to vehicle
                 Steer.write((int)((str_cmd*500.0)+1500.0));
                 Throttle.write((int)((thr_cmd*500.0)+1500.0));
 
@@ -184,7 +240,7 @@
                 auto_LED = 0;
             } // end if autocontrol
 
-        } else {
+        } else {  // System is not aArmed
             armed_LED = 0;          //Turn off armed LED indicator
             str_cmd = 0;
             thr_cmd = 0;
@@ -192,43 +248,49 @@
             Throttle.write(1500);   //Set Throttle to Low
         }/// end  else armed
 
-        if(t.read()-t_hall < 0.2) {
-            speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall);
-        } else {
-            speed = 0;
-        }
-
-
-// Read Angles
-        imu.get_angles();
-        imu.get_accel();
-        imu.get_gyro();
-        imu.get_mag();
+        
         
-        dt = t.read()-t_loop;
-        t_loop = t.read();
+        
         psi_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x);
         psi_est_dot = imu.gyro.z + L*(psi_comp - psi_est);
         
         psi_est = psi_est + psi_est_dot*dt;
         
-         if(t.read()-t_log > (1/LOG_RATE)) {
-
+        // ***************************************
+        // Transmit data to ground station
+        // ***************************************
+        if(t.read()-t_log > (1/LOG_RATE)) {
             if(log_data) {
               /* Data Output Options
                  imu.accel.x imu.accel.y imu.accel.z
                  imu.gyro.x imu.gyro.y imu.gyro.z
                  wheel_spd thr str*/
-              xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, thr_cmd, str_cmd, imu.gyro.z);
+              xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z, hdg_gyro);
+ //             xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, thr_cmd, str_cmd, imu.gyro.z);
              // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
              // xbee.printf("$ODO,%.3f, %.3f, %.3f\r\n",wheel_spd,thr,str);
              }
              t_log = t.read();
-         }
+         } // End transmit data
+        
+        dt = t.read()-t_loop;  // Sample period (sec)
+        t_loop = t.read();
+  
         wait(1/LOOP_RATE);
         status_LED=!status_LED;
-    }
+        
+    } // End control loop
+
+} // End main
+
 
+void he_callback()
+{
+    // Hall effect speed sensor callback
+    hall_LED = !hall_LED;
+
+    dt_hall = t.read()-t_hall;
+    t_hall = t.read();
 }
 
 void menuFunction(Serial *port){
@@ -263,6 +325,7 @@
         }
 
 }
+
 float saturateCmd(float cmd)
 {
     if(cmd>1.0) {
@@ -273,6 +336,8 @@
     }
     return cmd;
 }
+
+
 float saturateCmd(float cmd, float max,float min)
 {
     if(cmd>max) {