![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Emaxx Navigation code ported for MBED
Dependencies: BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed
Fork of Emaxx_Navigation_Dynamic_HIL by
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; +}