main.cpp

00001 
00002 
00003 
00004 
00005 #include "mbed.h"
00006 
00007 #include "GPSINT.h"
00008 #include "BNO055.h"
00009 #include "nav_ekf.h"
00010 #include "ServoIn.h"
00011 #include "ServoOut.h"
00012 #include "NeoStrip.h"
00013 #include "MODSERIAL.h"
00014 
00015 
00016 #define MAX_MESSAGE_SIZE 64
00017 #define IMU_RATE 100.0
00018 #define GPS_RATE 1.0
00019 #define LOOP_RATE 300.0
00020 #define CMD_TIMEOUT 1.0
00021 #define GEAR_RATIO (1/2.75)
00022 // Reference origin is at entrance to hospital point monument
00023 #define REF_LAT 38.986534
00024 #define REF_LON -76.489914
00025 #define REF_ALT 1.8
00026 #define NUM_LED 28
00027 #define LED_CLUSTERS 4
00028 #define LED_PER_CLUSTER 12
00029 
00030 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
00031 #define COURSE_MODE 1 //Commands map to heading and speed
00032 #define HIL_MODE 0 // commands map to hardware active simulation
00033 #define SIL_MODE 1 // commands map to software only simulation
00034 
00035 //I2C    i2c(p9, p10); // SDA, SCL
00036 BNO055 imu(p9, p10);
00037 GPSINT gps(p28,p27);
00038 
00039 vector <int> str_buf;
00040 int left;
00041 
00042 // Function Prototypes
00043 float saturateCmd(float cmd);
00044 float wrapToPi(float ang);
00045 void parseMessage(char * msg);
00046 void setLED(int *colors, float brightness);
00047 
00048 
00049 // LED Definitions
00050 DigitalOut rc_LED(LED1);
00051 DigitalOut armed_LED(LED2);
00052 DigitalOut auto_LED(LED3);
00053 DigitalOut imu_LED(LED4);
00054 
00055 NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
00056 
00057 // Comms and control object definitions
00058 Serial pc(p13, p14); // tx, rx for serial USB interface to pc
00059 MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee
00060 
00061 // Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
00062 // MODSERIAL xbee(p13, p14); // tx, rx for Xbee
00063 ServoIn CH1(p15);
00064 ServoIn CH2(p16);
00065 
00066 //InterruptIn he_sensor(p11);
00067 
00068 ServoOut Steer(p22);
00069 ServoOut Throttle(p21);
00070 Timer t; // create timer instance
00071 Ticker log_tick;
00072 Ticker heartbeat;
00073 float t_imu,t_gps,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
00074 float psi_err,spd_err, psi_err_i,spd_err_i; // control variables
00075 float t_hall, dt_hall,t_run,t_stop,t_log;
00076 bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
00077 float wheel_spd; // wheel speed variable
00078 float arm_clock,auto_clock; // timing for arming procedures
00079 bool str_cond,thr_cond,run_ctrl,log_data; // data saving variables?
00080 bool log_imu,log_bno,log_odo,log_mag = false; // data saving variables?
00081 int cmd_mode; // integer to set command mode of controller
00082 float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
00083 int led1,led2,led3,led4; // neo-strip variables?
00084 volatile bool newpacket = false; // boolean identifier of new odroid packet
00085 float x0; // initial x-position when in software or hardware in the loop simulation
00086 float y0; // initial y-position when in software or hardware in the loop simulation
00087 int sim_mode = 0; // sets simulation mode, zero by default, 1 for HIL, 2 for SIL
00088 float x = 0.0; // simulation variables
00089 float y = 0.0; // simulation variables
00090 float psi = 0.0; // simulation variables
00091 float spd = 0.0; // simulation speed
00092 
00093 void rxCallback(MODSERIAL_IRQ_INFO *q)
00094 {
00095     MODSERIAL *serial = q->serial;
00096     if ( serial->rxGetLastChar() == '\n') {
00097         newpacket = true;
00098     }
00099 }
00100 
00101 
00102 int main()
00103 {
00104     nav_EKF ekf; // initialize ekf states
00105 
00106     pc.baud(115200); // set baud rate of serial comm to pc
00107     xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
00108     Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
00109     Throttle.write(1500);   //Set Throttle to Low
00110 
00111     xbee.attach(&rxCallback, MODSERIAL::RxIrq);
00112 
00113 
00114     led1=led2=led3=led4 =WHITE; // set color of neo strip lights?
00115 
00116     // initialize necessary float and boolean variables
00117     left = 0;
00118     str_cmd = 0;
00119     str=0;
00120     thr=0;
00121     thr_cmd = 0;
00122     des_psi = 0;
00123     des_spd = 0;
00124     psi_err = 0;
00125     spd_err = 0;
00126     spd_err_i = 0;
00127     arm_clock = 0;
00128     auto_clock = 0;
00129     Kp_psi = 1;
00130     Kp_spd = 0.3;
00131     Ki_spd = 0.05;
00132     str_cond = false;
00133     thr_cond = false;
00134     armed = false;
00135     rc_conn = false;
00136     auto_ctrl = false;
00137     auto_ctrl_old = false;
00138     run_ctrl = false;
00139     log_data = false;
00140 
00141     // timer and timing initializations
00142     t.start();
00143     t_imu = t.read();
00144     t_gps = t.read();
00145     t_cmd = 0;
00146 
00147     leds.setBrightness(0.5); // set brightness of leds
00148 
00149     rc_LED = 0; // turn off LED 1 to indicate no RC connection
00150     imu_LED = 0; // turn off IMU indicator (LED 4)
00151     gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion
00152 
00153 
00154     // procedure to ensure IMU is operating correctly
00155     if(imu.check()) {
00156         pc.printf("BNO055 connected\r\n");
00157         imu.setmode(OPERATION_MODE_CONFIG);
00158         imu.SetExternalCrystal(1);
00159         imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
00160         imu.set_angle_units(RADIANS);
00161         imu.set_accel_units(MPERSPERS);
00162         imu.setoutputformat(WINDOWS);
00163         imu.set_mapping(2);
00164 
00165         // Blinks light if IMU is not calibrated, stops when calibration is complete
00166         /*while(int(imu.calib) < 0xCF) {
00167             pc.printf("Calibration = %x.\n\r",imu.calib);
00168             imu.get_calib();
00169             wait(0.5);
00170             imu_LED = !imu_LED;
00171         } // end while(imu.calib)
00172         */
00173         imu_LED = 1; // turns on IMU light when calibration is successful
00174 
00175     } else {
00176         pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly
00177 
00178         sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
00179 
00180 
00181         /* // turn on all lights is IMU is not connected correctly
00182         rc_LED = 1;
00183         armed_LED = 1;
00184         imu_LED = 1;
00185         auto_LED = 1;
00186         while(1) {
00187             // blink all lights if IMU is not connected correctly
00188             rc_LED = !rc_LED;
00189             armed_LED = !armed_LED;
00190             imu_LED = !imu_LED;
00191             auto_LED = !auto_LED;
00192 
00193             wait(0.5);
00194         } // end while(1) {blink if IMU is not connected}
00195         */
00196 
00197     } // end if(imu.check)
00198 
00199 
00200     //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
00201 
00202     pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running
00203 
00204 
00205     while(1) {
00206 
00207         // check for servo pulse from either channel of receiver module
00208         if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
00209             rc_conn = false;
00210         } else {
00211             rc_conn = true;
00212         } // end if(Channels connected)
00213 
00214         // turn on RC led if transmitter is connected
00215         rc_LED = rc_conn;
00216         auto_LED = auto_ctrl;
00217         armed_LED = armed;
00218 
00219         str_cond = (CH1.servoPulse > 1800); // If steering is full right
00220         thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
00221 
00222         if(t.read()-auto_clock > 3) { //Auto control timeout if no commands recevied after 3 seconds
00223             auto_ctrl = false;
00224         } // end if(t.read()-autoclock>3) timeout procedure
00225 
00226 
00227 
00228 
00229 
00230         if(newpacket) { // if xbee port receives a complete message, parse it
00231             char buf[MAX_MESSAGE_SIZE]; // create buffer for message
00232 
00233             // reads from modserial port buffer, stores characters into string "buf"
00234             int i = 0;
00235             if(xbee.rxBufferGetCount()>0) {
00236                 char c = xbee.getc();
00237                 //pc.printf("%s",c);
00238                 if(c=='$') {
00239                     buf[i] = c;
00240                     i++;
00241                     while(1) {
00242                         buf[i] = xbee.getc();
00243                         if(buf[i]=='\n') {
00244                             break;
00245                         }
00246                         i++;
00247                     }
00248                 }
00249             } // end if xbee.rxBufferGetCount
00250             xbee.rxBufferFlush();// empty receive buffer
00251             pc.printf("%s",buf); // print message to PC
00252             parseMessage(buf);
00253             newpacket = false; // reset packet flag
00254         } // end if(newpacket)
00255 
00256 
00257         if(!rc_conn) { // Is System Armed, system armed if RC not connected
00258             // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
00259             if(auto_ctrl) {
00260 
00261                 switch (cmd_mode) {
00262                     case DIRECT_MODE: {
00263                         str = str_cmd;
00264                         thr = thr_cmd;
00265                         //  pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
00266                         //  xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
00267                         break;
00268                     } // end direct mode case
00269                     case COURSE_MODE: {
00270 
00271                         if(sim_mode==0) { // if hardware is enabled use gyro and ekf
00272                             psi_err = wrapToPi(des_psi-imu.euler.yaw);
00273                             spd_err = des_spd - ekf.getSpd();
00274                             spd_err_i += spd_err;
00275                             str = -Kp_psi*psi_err/ekf.getSpd();
00276                         } else { // otherwise design control using simulated variables, bypass ekf states
00277                             psi_err = wrapToPi(des_psi-psi);
00278                             spd_err = des_spd - spd;
00279                             spd_err_i += spd_err;
00280                             if(spd>0.05) {
00281                                 str = Kp_psi*psi_err/spd;
00282                             } else {
00283                                 str = Kp_psi*psi_err/0.05;
00284                             }
00285                         } // end if sim_mode
00286 
00287                         thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
00288 
00289                         if (thr >= 1.0) {
00290                             thr = 1.0;
00291                             spd_err_i = 0; // Reset Integral If Saturated
00292                         } // end if thr>=1.0
00293                         if (thr < 0.0) {
00294                             thr = 0.0;
00295                             spd_err_i = 0;  // Reset Integral If Saturated
00296                         } // end iff thr<0
00297 
00298                         //pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
00299                         //xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
00300                         break;
00301 
00302                     } // end COURSE_MODE case
00303                     default: {
00304                         break;
00305                     } // end default status in switch
00306 
00307                 } // end switch(cmd_mode)
00308 
00309                 if(sim_mode<2) { // only actuates if in experiment or HIL modes
00310                     Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
00311                     Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
00312                 } else {
00313                     // won't send command to motor and servo if in SIL mode
00314                 }
00315 
00316 
00317             } else { // goes with if auto_ctrl, manual control mode
00318 
00319                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
00320                 Throttle.write(1500); //Write Throttle Pulse
00321 
00322             } // end else
00323 
00324         } else { // goes with if !rc_conn
00325 
00326             // for manual driving
00327             auto_ctrl = false;
00328             armed_LED = 0;          //Turn off armed LED indicator
00329             str = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
00330             thr = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
00331             if(sim_mode<2) { // if hardware is active send command to servo and throttle
00332                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
00333                 Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
00334             }
00335 
00336         }/// end  else armed
00337 
00338         if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off
00339             imu.get_angles();
00340             imu.get_accel();
00341             imu.get_gyro();
00342             imu.get_lia();
00343             float dt = t.read()-t_imu;
00344             if(dt > (1/IMU_RATE)) {
00345 
00346                 float tic = t.read();
00347                 ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
00348                 ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
00349 
00350                 xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
00351                 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));
00352                 pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
00353                 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));
00354 
00355                 t_imu = t.read();
00356             }
00357 
00358             if(t.read()-t_gps >(1/GPS_RATE)) {
00359                 //printf("Kp_psi: %.2f Kp_spd: %.2f Ki_spd: %.2f\r\n", Kp_psi,Kp_spd,Ki_spd);
00360 
00361 
00362                 float tic2 = t.read();
00363                 ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
00364                 // 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);
00365                 // xbee.printf("$STA,%d,%d,%d,%d\r\n",);
00366 
00367                 t_gps = t.read();
00368 
00369             }
00370             wait(1/LOOP_RATE);
00371             // status_LED=!status_LED;
00372             auto_ctrl_old = auto_ctrl;
00373 
00374         } else { // else condition implies a simulation mode is enabled
00375 
00376             float dt = t.read()-t_imu;
00377             if(dt > (1/IMU_RATE)) {
00378 
00379                 float tic = t.read();
00380 
00381 
00382                 x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
00383                 y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
00384                 psi = psi + str*dt; // turn rate kinematics
00385                 float drag = 0.0;
00386                 if(spd>1) {
00387                     drag = 0.0059*spd*spd; // based on drag on a cat sized object
00388                 } else {
00389                     drag = 0.0059*spd;
00390                 }
00391                 spd = spd + (thr-drag)*dt; // give throttle offset
00392 
00393                 xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
00394                 pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
00395                 t_imu = t.read();
00396             } // end if dt
00397 
00398 
00399         } // end if sim_mode
00400 
00401 
00402     } // end while(1)
00403 
00404 } // end main
00405 
00406 void parseMessage(char * msg)
00407 {
00408 
00409     if(!strncmp(msg, "$CMD", 4)) {
00410         int arg1;
00411         float arg2,arg3;
00412 
00413         sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
00414         cmd_mode = arg1;
00415         auto_clock = t.read();
00416         switch (cmd_mode) {
00417             case DIRECT_MODE: {
00418                 auto_ctrl = true;
00419                 str_cmd = arg2;
00420                 thr_cmd = arg3;
00421             }
00422             case COURSE_MODE: {
00423                 auto_ctrl = true;
00424                 des_psi = arg2;
00425                 des_spd = arg3;
00426             }
00427             default: {
00428             }
00429         }
00430     }   //emd of $CMD
00431 
00432     if(!strncmp(msg, "$PRM", 4)) {
00433         float arg1,arg2,arg3;
00434         int type;
00435         // __disable_irq();        // disable interrupts
00436         sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
00437         // __enable_irq();         // enable interrupts
00438 
00439         switch(type) {
00440             case 1: { // sets PID gains on heading and speed controller
00441                 //pc.printf("%s\n",msg);
00442                 Kp_psi = arg1;
00443                 Kp_spd = arg2;
00444                 Ki_spd = arg3;
00445 
00446                 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
00447                 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
00448                 break;
00449             }
00450             case 2: { // sets origin of local reference frame
00451 
00452                 //pc.printf("%s\n",msg);
00453 
00454                 float ref_lat = arg1;
00455                 float ref_lon = arg2;
00456                 float ref_alt = arg3;
00457                 gps.setRefPoint(ref_lat,ref_lon,ref_alt);
00458 
00459                 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
00460                 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
00461 
00462                 break;
00463             }
00464             default: {
00465             }
00466 
00467         }
00468 
00469     } // end of $PRM
00470 
00471     if(!strncmp(msg, "$LED", 4)) {
00472         int arg1;
00473         float arg2;
00474         sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
00475         //pc.printf("%s\n",msg);
00476         int colors[4]= {arg1,arg1,arg1,arg1};
00477         float brightness = arg2;
00478         setLED(colors,brightness);
00479     } // end of $LED
00480 
00481 
00482     if(!strncmp(msg, "$SIM", 4)) {
00483         int arg1;
00484         float arg2,arg3,arg4;
00485 
00486         sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
00487         sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
00488         auto_clock = t.read();
00489         switch (cmd_mode) {
00490             case HIL_MODE: {
00491                 auto_ctrl = true;
00492                 x = arg2;
00493                 y = arg3;
00494                 psi = arg4;
00495             }
00496             case SIL_MODE: {
00497                 auto_ctrl = true;
00498                 x = arg2;
00499                 y = arg3;
00500                 psi = arg4;
00501             }
00502             default: {
00503             }
00504         }
00505     }   //emd of $SIM
00506 
00507 
00508 
00509 
00510 
00511 }
00512 void setLED(int  *colors,float brightness)
00513 {
00514 
00515     leds.setBrightness(brightness);
00516 
00517     int cidx = 0;
00518     int ctr = 0;
00519     for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
00520 
00521         if(ctr >11) {
00522             ctr = 0;
00523             cidx++;
00524         }
00525         leds.setPixel(i,colors[cidx]);
00526         ctr++;
00527     }
00528     leds.write();
00529 }
00530 float saturateCmd(float cmd)
00531 {
00532     if(cmd>1.0) {
00533         cmd = 1.0;
00534     }
00535     if(cmd < -1.0) {
00536         cmd = -1.0;
00537     }
00538     return cmd;
00539 }
00540 float saturateCmd(float cmd, float max,float min)
00541 {
00542     if(cmd>max) {
00543         cmd = max;
00544     }
00545     if(cmd < min) {
00546         cmd = min;
00547     }
00548     return cmd;
00549 }
00550 
00551 float wrapToPi(float ang)
00552 {
00553 
00554     if(ang > PI) {
00555 
00556         ang = ang - 2*PI;
00557     }
00558     if (ang < -PI) {
00559         ang = ang + 2*PI;
00560     }
00561 
00562     return ang;
00563 }