Diff: main.cpp

Revision:

7:a8c2e9d049e8

Parent:

6:f64b1eba4d5e

Child:

8:9817993e5df7

--- a/main.cpp	Wed Dec 14 15:47:49 2016 +0000
+++ b/main.cpp	Fri Dec 16 14:06:42 2016 +0000
@@ -1,9 +1,5 @@
-
-
-
-
 #include "mbed.h"
-
+#include "utilityFunctions.h"
 #include "GPSINT.h"
 #include "BNO055.h"
 #include "nav_ekf.h"
@@ -11,10 +7,12 @@
 #include "ServoOut.h"
 #include "NeoStrip.h"
 #include "MODSERIAL.h"
+#include "vehicleModel.h"
 
 
 #define MAX_MESSAGE_SIZE 64
 #define IMU_RATE 100.0
+#define LOG_RATE 20.0
 #define GPS_RATE 1.0
 #define LOOP_RATE 300.0
 #define CMD_TIMEOUT 1.0
@@ -29,20 +27,22 @@
 
 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
 #define COURSE_MODE 1 //Commands map to heading and speed
-#define HIL_MODE 0 // commands map to hardware active simulation
-#define SIL_MODE 1 // commands map to software only simulation
+
+#define HARDWARE_MODE 0 
+#define HIL_MODE 1 // commands map to hardware active simulation
+#define SIL_MODE 2 // commands map to software only simulation
 
 //I2C    i2c(p9, p10); // SDA, SCL
 BNO055 imu(p9, p10);
-GPSINT gps(p28,p27);
+GPSINT gps(p13,p14);
 
 vector <int> str_buf;
 int left;
 
 // Function Prototypes
-float saturateCmd(float cmd);
-float wrapToPi(float ang);
-void parseMessage(char * msg);
+//float saturateCmd(float cmd);
+//float wrapToPi(float ang);
+int parseMessage(char * msg);
 void setLED(int *colors, float brightness);
 
 
@@ -55,11 +55,11 @@
 NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
 
 // Comms and control object definitions
-Serial pc(p13, p14); // tx, rx for serial USB interface to pc
-MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee
+//MODSERIAL pc(p13, p14); // tx, rx for serial USB interface to pc
+//MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee
 
-// Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
-// MODSERIAL xbee(p13, p14); // tx, rx for Xbee
+MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
+MODSERIAL xbee(p28, p27); // tx, rx for Xbee
 ServoIn CH1(p15);
 ServoIn CH2(p16);
 
@@ -70,7 +70,8 @@
 Timer t; // create timer instance
 Ticker log_tick;
 Ticker heartbeat;
-float t_imu,t_gps,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
+float t_imu,t_gps,t_cmd,t_sim,t_ctrl;
+float str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
 float psi_err,spd_err, psi_err_i,spd_err_i; // control variables
 float t_hall, dt_hall,t_run,t_stop,t_log;
 bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
@@ -90,6 +91,9 @@
 float psi = 0.0; // simulation variables
 float spd = 0.0; // simulation speed
 
+nav_EKF ekf; // initialize ekf states
+vehicleModel vm; //create vehicle model object
+
 void rxCallback(MODSERIAL_IRQ_INFO *q)
 {
     MODSERIAL *serial = q->serial;
@@ -101,17 +105,17 @@
 
 int main()
 {
-    nav_EKF ekf; // initialize ekf states
+
 
     pc.baud(115200); // set baud rate of serial comm to pc
-    xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
+    xbee.baud(57600); // set baud rate of serial comm of wireless xbee comms
     Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
     Throttle.write(1500);   //Set Throttle to Low
 
     xbee.attach(&rxCallback, MODSERIAL::RxIrq);
 
 
-    led1=led2=led3=led4 =WHITE; // set color of neo strip lights?
+    led1=led2=led3=led4=WHITE; // set color of neo strip lights?
 
     // initialize necessary float and boolean variables
     left = 0;
@@ -126,7 +130,7 @@
     spd_err_i = 0;
     arm_clock = 0;
     auto_clock = 0;
-    Kp_psi = 1;
+    Kp_psi = 1/1.57;
     Kp_spd = 0.3;
     Ki_spd = 0.05;
     str_cond = false;
@@ -137,11 +141,15 @@
     auto_ctrl_old = false;
     run_ctrl = false;
     log_data = false;
+    cmd_mode = 1;
 
     // timer and timing initializations
     t.start();
     t_imu = t.read();
     t_gps = t.read();
+    t_sim = t.read();
+    t_ctrl = t.read();
+    t_log = t.read();
     t_cmd = 0;
 
     leds.setBrightness(0.5); // set brightness of leds
@@ -226,7 +234,6 @@
 
 
 
-
         if(newpacket) { // if xbee port receives a complete message, parse it
             char buf[MAX_MESSAGE_SIZE]; // create buffer for message
 
@@ -248,7 +255,7 @@
                 }
             } // end if xbee.rxBufferGetCount
             xbee.rxBufferFlush();// empty receive buffer
-            pc.printf("%s",buf); // print message to PC
+            //pc.printf("%s",buf); // print message to PC
             parseMessage(buf);
             newpacket = false; // reset packet flag
         } // end if(newpacket)
@@ -256,8 +263,10 @@
 
         if(!rc_conn) { // Is System Armed, system armed if RC not connected
             // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
+
             if(auto_ctrl) {
-
+                float cdt = t.read()-t_ctrl;
+                t_ctrl = t.read();
                 switch (cmd_mode) {
                     case DIRECT_MODE: {
                         str = str_cmd;
@@ -269,34 +278,41 @@
                     case COURSE_MODE: {
 
                         if(sim_mode==0) { // if hardware is enabled use gyro and ekf
-                            psi_err = wrapToPi(des_psi-imu.euler.yaw);
+                            psi_err = wrapToPi(des_psi-imu.euler.yaw); 
+                            
                             spd_err = des_spd - ekf.getSpd();
                             spd_err_i += spd_err;
                             str = -Kp_psi*psi_err/ekf.getSpd();
                         } else { // otherwise design control using simulated variables, bypass ekf states
-                            psi_err = wrapToPi(des_psi-psi);
-                            spd_err = des_spd - spd;
+ 
+                            psi_err = wrapToPi(des_psi-vm.getYaw());
+                            str = Kp_psi*psi_err;         
+                                                    
+                            spd_err = des_spd - vm.getSpd();
                             spd_err_i += spd_err;
-                            if(spd>0.05) {
-                                str = Kp_psi*psi_err/spd;
+                              
+                             
+                           /* if(spd>0.05) {
+                                str = Kp_psi*psi_err/vm.getSpd();
                             } else {
                                 str = Kp_psi*psi_err/0.05;
-                            }
+                            }*/
                         } // end if sim_mode
 
-                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
+                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;
 
                         if (thr >= 1.0) {
                             thr = 1.0;
-                            spd_err_i = 0; // Reset Integral If Saturated
+                            spd_err_i = 0.0; // Reset Integral If Saturated
                         } // end if thr>=1.0
                         if (thr < 0.0) {
                             thr = 0.0;
-                            spd_err_i = 0;  // Reset Integral If Saturated
+                            spd_err_i = 0.0;  // Reset Integral If Saturated
                         } // end iff thr<0
 
-                        //pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
-                        //xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
+                        
+                        pc.printf("Psi Err: %.3f,  Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f\r\n",psi_err,str,spd_err,Kp_spd,thr);
+                     
                         break;
 
                     } // end COURSE_MODE case
@@ -305,7 +321,8 @@
                     } // end default status in switch
 
                 } // end switch(cmd_mode)
-
+                str = saturateCmd(str);
+                //xbee.printf("Psi: %.3f Psi Err: %.3f Str Cmd %.3f\r\n",vm.getYaw(),psi_err,str);
                 if(sim_mode<2) { // only actuates if in experiment or HIL modes
                     Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                     Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
@@ -313,7 +330,7 @@
                     // won't send command to motor and servo if in SIL mode
                 }
 
-
+                
             } else { // goes with if auto_ctrl, manual control mode
 
                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
@@ -328,6 +345,7 @@
             armed_LED = 0;          //Turn off armed LED indicator
             str = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
             thr = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
+            
             if(sim_mode<2) { // if hardware is active send command to servo and throttle
                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
@@ -336,42 +354,54 @@
         }/// end  else armed
 
         if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off
-            imu.get_angles();
-            imu.get_accel();
-            imu.get_gyro();
-            imu.get_lia();
             float dt = t.read()-t_imu;
             if(dt > (1/IMU_RATE)) {
-
+                imu.get_angles();
+                imu.get_accel();
+                imu.get_gyro();
+                imu.get_lia();
+                
                 float tic = t.read();
                 ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
                 ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
+                t_imu = t.read();
 
+            }
+        }else{ // Else we are running a simulation
+            float T = t.read()-t_sim;
+            vm.stepModel(str,thr,T);            
+            t_sim = t.read();
+        }      
+            
+        if(t.read()-t_log > (1/LOG_RATE)) {
+                             
+            if(sim_mode > 0) {
+               xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw())); 
+               xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+               
+              // pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));                
+                              
+              //  pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+              //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+            } else {
                 xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
                 xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
-                pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
-                pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
-
-                t_imu = t.read();
+              //  pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+              //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
             }
+            t_log = t.read();
+        }
 
             if(t.read()-t_gps >(1/GPS_RATE)) {
-                //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd);
-
-
                 float tic2 = t.read();
                 ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
-                // xbee.printf("$GPS,%.8f,%.8f,%.3f,%.3f,%.3f\r\n",gps.dec_latitude,gps.dec_longitude,gps.msl_altitude,gps.course_d,KNOTS_2_MPS*gps.speed_k);
-                // xbee.printf("$STA,%d,%d,%d,%d\r\n",);
-
                 t_gps = t.read();
-
             }
             wait(1/LOOP_RATE);
             // status_LED=!status_LED;
             auto_ctrl_old = auto_ctrl;
 
-        } else { // else condition implies a simulation mode is enabled
+       /* } else { // else condition implies a simulation mode is enabled
 
             float dt = t.read()-t_imu;
             if(dt > (1/IMU_RATE)) {
@@ -396,14 +426,14 @@
             } // end if dt
 
 
-        } // end if sim_mode
+        } // end if sim_mode */
 
 
     } // end while(1)
 
 } // end main
 
-void parseMessage(char * msg)
+int parseMessage(char * msg)
 {
 
     if(!strncmp(msg, "$CMD", 4)) {
@@ -427,6 +457,7 @@
             default: {
             }
         }
+     return 1;
     }   //emd of $CMD
 
     if(!strncmp(msg, "$PRM", 4)) {
@@ -463,9 +494,9 @@
             }
             default: {
             }
-
+            
         }
-
+        return 1;
     } // end of $PRM
 
     if(!strncmp(msg, "$LED", 4)) {
@@ -476,37 +507,37 @@
         int colors[4]= {arg1,arg1,arg1,arg1};
         float brightness = arg2;
         setLED(colors,brightness);
+        return 1;
     } // end of $LED
 
 
     if(!strncmp(msg, "$SIM", 4)) {
         int arg1;
         float arg2,arg3,arg4;
-
         sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
         sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
-        auto_clock = t.read();
-        switch (cmd_mode) {
+        switch (sim_mode) {
             case HIL_MODE: {
                 auto_ctrl = true;
                 x = arg2;
                 y = arg3;
                 psi = arg4;
+                vm.initialize(x,y,psi);
             }
             case SIL_MODE: {
                 auto_ctrl = true;
                 x = arg2;
                 y = arg3;
                 psi = arg4;
+                vm.initialize(x,y,psi);
+                
             }
             default: {
             }
         }
+        return 1;
     }   //emd of $SIM
-
-
-
-
+    return 0;
 
 }
 void setLED(int  *colors,float brightness)
@@ -527,37 +558,4 @@
     }
     leds.write();
 }
-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;
-}
 
-float wrapToPi(float ang)
-{
-
-    if(ang > PI) {
-
-        ang = ang - 2*PI;
-    }
-    if (ang < -PI) {
-        ang = ang + 2*PI;
-    }
-
-    return ang;
-}
\ No newline at end of file