Diff: main.cpp

Revision:

5:03074e90ef7a

Parent:

4:ec2978ff7a1e

Child:

6:d133964667f3

--- a/main.cpp	Thu Nov 17 19:41:38 2016 +0000
+++ b/main.cpp	Sun Nov 20 19:29:37 2016 +0000
@@ -16,11 +16,9 @@
 #include "ServoIn.h"
 #include "ServoOut.h"
 
-//I2C    i2c(p9, p10); // SDA, SCL
 BNO055 imu(p9, p10);
 
 void he_callback();
-float saturateCmd(float cmd);
 void menuFunction(Serial *port);
 DigitalOut status_LED(LED1);
 DigitalOut armed_LED(LED2);
@@ -38,22 +36,19 @@
 Timer t; // create timer instance
 Ticker log_tick;
 Ticker heartbeat;
-float t_imu,t_cmd,str_cmd,thr_cmd,str,thr,dt;
+float t_imu,t_cmd,str_cmd,thr_cmd,dt;
 float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop;
-float psi_err,spd_err, psi_err_i,spd_err_i, psi_est, psi_est_dot, psi_comp;
-float L;
 
 bool armed, auto_ctrl;
 float speed;
 float arm_clock;
 bool str_cond,thr_cond,run_ctrl,log_data;
 
-// User defined variables
+// USER DEFINED VARIABLES
 float distance;
 float steering_offset;
 float hdg_comp, hdg_gyro; 
-float hdg_target;
-float K;
+
 
 // ============================================
 // Main Program
@@ -67,13 +62,8 @@
     he_sensor.rise(&he_callback); // Set hall effect speed sensor interrupt
 
     str_cmd = 0;
-    str=0;
-    thr=0;
-    L =0;
     thr_cmd = 0;
-    arm_clock =0;
-    psi_est=0;
-    psi_est_dot=0;    
+    arm_clock =0; 
     str_cond = false;
     thr_cond = false;
     armed = false;
@@ -115,12 +105,12 @@
     } // imu.check
 
     
-    // Initialize user defined variables  
+    // INITIALIZE USER DEFINED VARIABLES  
     distance = 0;  // Initialize distance
-    hdg_gyro = 0;  // Initialize gyro heading 
-    steering_offset = -.12;
-    K = 0.1;  // Heading control gain
-
+    steering_offset = 0; // Steering command offset
+    hdg_gyro = 0; // Initialize integrated gyro heading
+    hdg_est = 0; // Initialize heading estimate
+   
     
     // ============================================
     // Control Loop
@@ -158,17 +148,15 @@
         // *******************************************
         // 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();
+        imu.get_accel();    // imu.accel.x, imu.accel.y, imu.accel.z
+        imu.get_gyro();     // imu.gyro.x, imu.gyro.y, imu.gyro.z
+        imu.get_mag();      // imu.mag.x, imu.mag.y, imu.mag.z
 
  
         // *******************************************
@@ -188,21 +176,25 @@
                     //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);
+                    // ***COMPUTE HEADING HERE***
+                    // Compute Heading via magnetometer (deg)
+                    //hdg_comp = ... ;
                     
                     // 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); 
+                    //hdg_gyro = ... ; 
+                     
+                   // Heading estimate
+                    //hdg_est = ... ;  
+                    
+                   
+                   // ***COMPUTE CONTROL LAW HERE *** 
+                   // For now it is just a step input steering angle
+                   if(distance <= 72 ) {
+                        thr_cmd = 0.2;
+                        str_cmd = 0.0;
+                    } else if(distance > 72 && distance <= 150) {
+                        thr_cmd = 0.2;
+                        str_cmd = 0.5;
                     } else {  // Run completed
                         thr_cmd = 0;
                         str_cmd = 0;
@@ -210,7 +202,7 @@
                         run_ctrl = false; // Turn off run control
                         log_data = false;
                     }
-                    
+                
                     // Compensate for steering offset
                     str_cmd = str_cmd + steering_offset;
 
@@ -249,26 +241,12 @@
         }/// end  else armed
 
         
-        
-        
-        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;
-        
         // ***************************************
         // 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, %.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);
+              xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z);
              }
              t_log = t.read();
          } // End transmit data
@@ -293,6 +271,7 @@
     t_hall = t.read();
 }
 
+
 void menuFunction(Serial *port){
         if(port->readable()) {
             char c = port->getc();
@@ -326,27 +305,6 @@
 
 }
 
-float saturateCmd(float cmd)
-{
-    if(cmd>1.0) {
-        cmd = 1.0;
-    }
-    if(cmd < -1.0) {
-        cmd = -1.0;
-    }
-    return cmd;
-}
 
 
-float saturateCmd(float cmd, float max,float min)
-{
-    if(cmd>max) {
-        cmd = max;
-    }
-    if(cmd < min) {
-        cmd = min;
-    }
-    return cmd;
-}
 
-