Diff: main.cpp

Revision:

2:0c9c3c1f3b18

Parent:

0:f8fd381a5b8a

Child:

3:9b6db94aa2b3

--- a/main.cpp	Tue Aug 16 13:49:47 2016 +0000
+++ b/main.cpp	Thu Aug 25 15:47:50 2016 +0000
@@ -10,6 +10,8 @@
 #include "ServoIn.h"
 #include "ServoOut.h"
 #include "NeoStrip.h"
+#include "MODSERIAL.h"
+
 
 #define MAX_MESSAGE_SIZE 64
 #define IMU_RATE 100.0
@@ -37,9 +39,7 @@
 
 // Function Prototypes
 float saturateCmd(float cmd);
-void menuFunction(Serial *port);
 float wrapToPi(float ang);
-int readMessage(Serial *port, char * buffer);
 void parseMessage(char * msg);
 void setLED(int *colors, float brightness);
 
@@ -52,8 +52,9 @@
 
 NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
 
+// Comms and control object definitions
 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
-Serial xbee(p13, p14); // tx, rx for Xbee
+MODSERIAL xbee(p13, p14); // tx, rx for Xbee
 ServoIn CH1(p15);
 ServoIn CH2(p16);
 
@@ -64,28 +65,43 @@
 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;
-float psi_err,spd_err, psi_err_i,spd_err_i;
+float t_imu,t_gps,t_cmd,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;
-float wheel_spd;
-float arm_clock,auto_clock;
-bool str_cond,thr_cond,run_ctrl,log_data;
-bool log_imu,log_bno,log_odo,log_mag = false;
-int cmd_mode;
-float Kp_psi, Kp_spd, Ki_psi, Ki_spd;
-int led1,led2,led3,led4;
+bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
+float wheel_spd; // wheel speed variable
+float arm_clock,auto_clock; // timing for arming procedures
+bool str_cond,thr_cond,run_ctrl,log_data; // data saving variables?
+bool log_imu,log_bno,log_odo,log_mag = false; // data saving variables?
+int cmd_mode; // integer to set command mode of controller
+float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
+int led1,led2,led3,led4; // neo-strip variables?
+volatile bool newpacket = false; // boolean identifier of new odroid packet
+
+void rxCallback(MODSERIAL_IRQ_INFO *q)
+{
+    MODSERIAL *serial = q->serial;
+    if ( serial->rxGetLastChar() == '\n') {
+        newpacket = true;
+    }
+}
+
 
 int main()
 {
-    nav_EKF ekf;
+    nav_EKF ekf; // initialize ekf states
 
-    pc.baud(115200);
-    xbee.baud(115200);
+    pc.baud(115200); // set baud rate of serial comm to pc
+    xbee.baud(115200); // 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
-    
-    led1=led2=led3=led4 =WHITE;
+
+    xbee.attach(&rxCallback, MODSERIAL::RxIrq);
+
+
+    led1=led2=led3=led4 =WHITE; // set color of neo strip lights?
+
+    // initialize necessary float and boolean variables
     left = 0;
     str_cmd = 0;
     str=0;
@@ -110,16 +126,19 @@
     run_ctrl = false;
     log_data = false;
 
+    // timer and timing initializations
     t.start();
     t_imu = t.read();
     t_gps = t.read();
     t_cmd = 0;
 
-    leds.setBrightness(0.5);
-    
-    rc_LED = 0;
-    imu_LED = 0;
-    gps.setRefPoint(REF_LAT,REF_LON,REF_ALT);
+    leds.setBrightness(0.5); // set brightness of leds
+
+    rc_LED = 0; // turn off LED 1 to indicate no RC connection
+    imu_LED = 0; // turn off IMU indicator (LED 4)
+    gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion
+
+    // procedure to ensure IMU is operating correctly
     if(imu.check()) {
         pc.printf("BNO055 connected\r\n");
         imu.setmode(OPERATION_MODE_CONFIG);
@@ -130,119 +149,148 @@
         imu.setoutputformat(WINDOWS);
         imu.set_mapping(2);
 
+        // Blinks light if IMU is not calibrated, stops when calibration is complete
         while(int(imu.calib) < 0xCF) {
             pc.printf("Calibration = %x.\n\r",imu.calib);
             imu.get_calib();
             wait(0.5);
             imu_LED = !imu_LED;
-        }
-        imu_LED = 1;
+        } // end while(imu.calib)
+        imu_LED = 1; // turns on IMU light when calibration is successful
 
     } else {
-        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
+        pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); // catch statement if IMU is not connected correctly
+        // turn on all lights is IMU is not connected correctly
         rc_LED = 1;
         armed_LED = 1;
         imu_LED = 1;
         auto_LED = 1;
         while(1) {
+            // blink all lights if IMU is not connected correctly
             rc_LED = !rc_LED;
             armed_LED = !armed_LED;
             imu_LED = !imu_LED;
             auto_LED = !auto_LED;
-            
+
             wait(0.5);
-        }
-    }
-  //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
+        } // end while(1) {blink if IMU is not connected}
+    } // end if(imu.check)
 
-    pc.printf("Emaxx Navigation Program\r\n");
-        
+    //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
+
+    pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running
+
+
     while(1) {
 
+        // check for servo pulse from either channel of receiver module
         if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
             rc_conn = false;
         } else {
             rc_conn = true;
-        }
-        
+        } // end if(Channels connected)
+
+        // turn on RC led if transmitter is connected
         rc_LED = rc_conn;
         auto_LED = auto_ctrl;
         armed_LED = armed;
-        
+
         str_cond = (CH1.servoPulse > 1800); // If steering is full right
         thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
-        
-        if(t.read()-auto_clock > 3){ //Auto control timeout if no commands recevied after 5 seconds
-            auto_ctrl = false;     
 
-        }
-        
+        if(t.read()-auto_clock > 3) { //Auto control timeout if no commands recevied after 3 seconds
+            auto_ctrl = false;
+        } // end if(t.read()-autoclock>3) timeout procedure
+
+
+
+
 
-   
-        if(xbee.readable()) {
+        if(newpacket) { // if xbee port receives a complete message, parse it
             char buf[MAX_MESSAGE_SIZE];
-            int n = readMessage(&xbee,buf); 
-            parseMessage(buf);   
+            
+            // reads from modserial port buffer, stores characters into string "buf"
+            int i = 0;
+            if(xbee.rxBufferGetCount()>0) {
+                char c = xbee.getc();
+                //pc.printf("%s",c);
+                if(c=='$') {
+                    buf[i] = c;
+                    i++;
+                    while(1) {
+                        buf[i] = xbee.getc();
+                        if(buf[i]=='\n') {
+                            break;
+                        }
+                        i++;
+                    }
+                }
+            }
+            xbee.rxBufferFlush();// empty receive buffer
+            pc.printf("%s",buf);
+            parseMessage(buf);
+            newpacket = false;
+        } // end if(newpacket)
 
-        }
 
         if(!rc_conn) { // Is System Armed
-           // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);                
+            // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
             if(auto_ctrl) {
-                 
-                 switch (cmd_mode){
+
+                switch (cmd_mode) {
                     case DIRECT_MODE: {
                         str = str_cmd;
                         thr = thr_cmd;
-                      //  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 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);
                         break;
-                    }
+                    } // end direct mode case
                     case COURSE_MODE: {
-                        
+
                         psi_err = wrapToPi(des_psi-imu.euler.yaw);
                         spd_err = des_spd - ekf.getSpd();
                         spd_err_i += spd_err;
                         str = -Kp_psi*psi_err;
-                        
-                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i;
-                        
-                        if (thr >= 1.0){
+
+                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
+
+                        if (thr >= 1.0) {
                             thr = 1.0;
                             spd_err_i = 0; // Reset Integral If Saturated
-                        }
-                        if (thr < 0){
+                        } // end if thr>=1.0
+                        if (thr < 0) {
                             thr = 0;
-                            spd_err_i = 0;  // Reset Integral If Saturated                          
-                        }
+                            spd_err_i = 0;  // Reset Integral If Saturated
+                        } // end iff thr<0
 
-                        //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 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);
                         break;
 
-                    }
-                    default:{
+                    } // end COURSE_MODE case
+                    default: {
                         break;
-                    }                 
-                
-                }
+                    } // end default status in switch
+
+                } // end switch(cmd_mode)
                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
-                 
-            } else {
-                
+
+            } else { // goes with if auto_ctrl
+
                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                 Throttle.write(1500); //Write Throttle Pulse
-                
-            }
 
-        } else {
+            } // end else
+
+        } else { // goes with if !rc_conn
             auto_ctrl = false;
             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
             Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
             Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
-                     
+
         }/// end  else armed
 
         imu.get_angles();
@@ -263,53 +311,35 @@
         }
 
         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); 
- 
+            //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",);
+            // 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;
+        // status_LED=!status_LED;
+        auto_ctrl_old = auto_ctrl;
     }
 
 }
-int readMessage(Serial *port, char * buffer){
-int i = 0;
 
-    if(port->readable()){
-        char c = port->getc();
-        if(c=='$'){
-            buffer[i] = c;
-            i++;
-            while(1){
-            buffer[i] = port->getc();
-                if(buffer[i]=='\n' || buffer[i]=='\r'){
-                    break;
-                }
-            i++;          
-            }
-        }
-    }
-    return i;
-}
+void parseMessage(char * msg)
+{
 
-void parseMessage(char * msg){
-
-    if(!strncmp(msg, "$CMD", 4)){
+    if(!strncmp(msg, "$CMD", 4)) {
         int arg1;
         float arg2,arg3;
 
         sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
         cmd_mode = arg1;
         auto_clock = t.read();
-         switch (cmd_mode){
+        switch (cmd_mode) {
             case DIRECT_MODE: {
                 auto_ctrl = true;
                 str_cmd = arg2;
@@ -320,77 +350,81 @@
                 des_psi = arg2;
                 des_spd = arg3;
             }
-            default:{
-            }    
+            default: {
+            }
         }
     }   //emd of $CMD
-    
-    /*if(!strncmp(msg, "$PRM", 4)){
+
+    if(!strncmp(msg, "$PRM", 4)) {
         float arg1,arg2,arg3;
         int type;
-       // __disable_irq();        // disable interrupts
+        // __disable_irq();        // disable interrupts
         sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
-       // __enable_irq();         // enable interrupts
-        
-        switch(type){
-            case 1:
-            {
+        // __enable_irq();         // enable interrupts
+
+        switch(type) {
+            case 1: { // sets PID gains on heading and speed controller
+                //pc.printf("%s\n",msg);
                 Kp_psi = arg1;
                 Kp_spd = arg2;
-                Ki_spd = arg3; 
-                printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
+                Ki_spd = arg3;
+                
+                pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
+                //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
                 break;
             }
-            case 2:
-            {
-                printf("%s",msg);
+            case 2: { // sets origin of local reference frame
+                
+                //pc.printf("%s\n",msg);
 
                 float ref_lat = arg1;
                 float ref_lon = arg2;
                 float ref_alt = arg3;
-                printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
-
-              //  gps.setRefPoint(ref_lat,ref_lon,ref_alt);                  
+                gps.setRefPoint(ref_lat,ref_lon,ref_alt);
+                
+                pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
+                //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
+                
                 break;
-            } 
-            default:
-            {
-            }  
-                
+            }
+            default: {
+            }
+
         }
 
-    } */
-    
-    if(!strncmp(msg, "$LED", 4)){
+    } // end of $PRM
+
+    if(!strncmp(msg, "$LED", 4)) {
         int arg1;
         float arg2;
         sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
-        printf("%s\r\n",msg);
-        int colors[4]={arg1,arg1,arg1,arg1};
+        //pc.printf("%s\n",msg);
+        int colors[4]= {arg1,arg1,arg1,arg1};
         float brightness = arg2;
-        setLED(colors,brightness);        
+        setLED(colors,brightness);
     }
 
 
- 
+
 
 }
-void setLED(int  *colors,float brightness){
-        
-        leds.setBrightness(brightness);
+void setLED(int  *colors,float brightness)
+{
+
+    leds.setBrightness(brightness);
+
+    int cidx = 0;
+    int ctr = 0;
+    for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
 
-        int cidx = 0;
-        int ctr = 0;
-        for (int i=0;i<LED_PER_CLUSTER*LED_CLUSTERS;i++){
-            
-            if(ctr >11){
-                ctr = 0;
-                cidx++;                   
-            }
-            leds.setPixel(i,colors[cidx]);
-            ctr++;
+        if(ctr >11) {
+            ctr = 0;
+            cidx++;
         }
-        leds.write();
+        leds.setPixel(i,colors[cidx]);
+        ctr++;
+    }
+    leds.write();
 }
 float saturateCmd(float cmd)
 {
@@ -425,4 +459,4 @@
     }
 
     return ang;
-}
+}
\ No newline at end of file