Diff: main.cpp

Revision:

0:f8fd381a5b8a

Child:

2:0c9c3c1f3b18

diff -r 000000000000 -r f8fd381a5b8a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 16 13:43:26 2016 +0000
@@ -0,0 +1,428 @@
+
+
+
+
+#include "mbed.h"
+
+#include "GPSINT.h"
+#include "BNO055.h"
+#include "nav_ekf.h"
+#include "ServoIn.h"
+#include "ServoOut.h"
+#include "NeoStrip.h"
+
+#define MAX_MESSAGE_SIZE 64
+#define IMU_RATE 100.0
+#define GPS_RATE 1.0
+#define LOOP_RATE 300.0
+#define CMD_TIMEOUT 1.0
+#define GEAR_RATIO (1/2.75)
+// Reference origin is at entrance to hospital point monument
+#define REF_LAT 38.986534
+#define REF_LON -76.489914
+#define REF_ALT 1.8
+#define NUM_LED 28
+#define LED_CLUSTERS 4
+#define LED_PER_CLUSTER 12
+
+#define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
+#define COURSE_MODE 1 //Commands map to heading and speed
+
+//I2C    i2c(p9, p10); // SDA, SCL
+BNO055 imu(p9, p10);
+GPSINT gps(p28,p27);
+
+vector <int> str_buf;
+int left;
+
+// 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);
+
+
+// LED Definitions
+DigitalOut rc_LED(LED1);
+DigitalOut armed_LED(LED2);
+DigitalOut auto_LED(LED3);
+DigitalOut imu_LED(LED4);
+
+NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
+
+Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
+Serial xbee(p13, p14); // tx, rx for Xbee
+ServoIn CH1(p15);
+ServoIn CH2(p16);
+
+//InterruptIn he_sensor(p11);
+
+ServoOut Steer(p22);
+ServoOut Throttle(p21);
+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_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;
+
+int main()
+{
+    nav_EKF ekf;
+
+    pc.baud(115200);
+    xbee.baud(115200);
+    Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
+    Throttle.write(1500);   //Set Throttle to Low
+    
+    led1=led2=led3=led4 =WHITE;
+    left = 0;
+    str_cmd = 0;
+    str=0;
+    thr=0;
+    thr_cmd = 0;
+    des_psi = 0;
+    des_spd = 0;
+    psi_err = 0;
+    spd_err = 0;
+    spd_err_i = 0;
+    arm_clock = 0;
+    auto_clock = 0;
+    Kp_psi = 1;
+    Kp_spd = 0.3;
+    Ki_spd = 0.05;
+    str_cond = false;
+    thr_cond = false;
+    armed = false;
+    rc_conn = false;
+    auto_ctrl = false;
+    auto_ctrl_old = false;
+    run_ctrl = false;
+    log_data = false;
+
+    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);
+    if(imu.check()) {
+        pc.printf("BNO055 connected\r\n");
+        imu.setmode(OPERATION_MODE_CONFIG);
+        imu.SetExternalCrystal(1);
+        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
+        imu.set_angle_units(RADIANS);
+        imu.set_accel_units(MPERSPERS);
+        imu.setoutputformat(WINDOWS);
+        imu.set_mapping(2);
+
+        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;
+
+    } else {
+        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
+        rc_LED = 1;
+        armed_LED = 1;
+        imu_LED = 1;
+        auto_LED = 1;
+        while(1) {
+            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};
+
+    pc.printf("Emaxx Navigation Program\r\n");
+        
+    while(1) {
+
+        if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
+            rc_conn = false;
+        } else {
+            rc_conn = true;
+        }
+        
+        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(xbee.readable()) {
+            char buf[MAX_MESSAGE_SIZE];
+            int n = readMessage(&xbee,buf); 
+            parseMessage(buf);   
+
+        }
+
+        if(!rc_conn) { // Is System Armed
+           // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);                
+            if(auto_ctrl) {
+                 
+                 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); 
+
+                        break;
+                    }
+                    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 = 1.0;
+                            spd_err_i = 0; // Reset Integral If Saturated
+                        }
+                        if (thr < 0){
+                            thr = 0;
+                            spd_err_i = 0;  // Reset Integral If Saturated                          
+                        }
+
+                        //printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr); 
+                        break;
+
+                    }
+                    default:{
+                        break;
+                    }                 
+                
+                }
+                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
+                Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+                 
+            } else {
+                
+                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
+                Throttle.write(1500); //Write Throttle Pulse
+                
+            }
+
+        } else {
+            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();
+        imu.get_accel();
+        imu.get_gyro();
+        imu.get_lia();
+        float dt = t.read()-t_imu;
+        if(dt > (1/IMU_RATE)) {
+
+            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);
+
+            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));
+
+            t_imu = 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;
+    }
+
+}
+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){
+
+    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){
+            case DIRECT_MODE: {
+                auto_ctrl = true;
+                str_cmd = arg2;
+                thr_cmd = arg3;
+            }
+            case COURSE_MODE: {
+                auto_ctrl = true;
+                des_psi = arg2;
+                des_spd = arg3;
+            }
+            default:{
+            }    
+        }
+    }   //emd of $CMD
+    
+    /*if(!strncmp(msg, "$PRM", 4)){
+        float arg1,arg2,arg3;
+        int type;
+       // __disable_irq();        // disable interrupts
+        sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
+       // __enable_irq();         // enable interrupts
+        
+        switch(type){
+            case 1:
+            {
+                Kp_psi = arg1;
+                Kp_spd = arg2;
+                Ki_spd = arg3; 
+                printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
+                break;
+            }
+            case 2:
+            {
+                printf("%s",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);                  
+                break;
+            } 
+            default:
+            {
+            }  
+                
+        }
+
+    } */
+    
+    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};
+        float brightness = arg2;
+        setLED(colors,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++){
+            
+            if(ctr >11){
+                ctr = 0;
+                cidx++;                   
+            }
+            leds.setPixel(i,colors[cidx]);
+            ctr++;
+        }
+        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;
+}