![](/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:
- 8:9817993e5df7
- Parent:
- 7:a8c2e9d049e8
- Child:
- 9:3aa9b689bda5
diff -r a8c2e9d049e8 -r 9817993e5df7 main.cpp --- a/main.cpp Fri Dec 16 14:06:42 2016 +0000 +++ b/main.cpp Fri Apr 07 17:12:29 2017 +0000 @@ -1,22 +1,21 @@ #include "mbed.h" #include "utilityFunctions.h" -#include "GPSINT.h" #include "BNO055.h" -#include "nav_ekf.h" #include "ServoIn.h" #include "ServoOut.h" -#include "NeoStrip.h" #include "MODSERIAL.h" #include "vehicleModel.h" - +#include "motor_driver.h" #define MAX_MESSAGE_SIZE 64 #define IMU_RATE 100.0 -#define LOG_RATE 20.0 -#define GPS_RATE 1.0 +#define LOG_RATE 10.0 #define LOOP_RATE 300.0 #define CMD_TIMEOUT 1.0 -#define GEAR_RATIO (1/2.75) +#define GEAR_RATIO (1/2.75) // gear ratio +#define WHL_RADIUS 0.036 // Wheel radius (m) +#define PI 3.14159 + // Reference origin is at entrance to hospital point monument #define REF_LAT 38.986534 #define REF_LON -76.489914 @@ -28,34 +27,29 @@ #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized #define COURSE_MODE 1 //Commands map to heading and speed -#define HARDWARE_MODE 0 +#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(p13,p14); -vector <int> str_buf; int left; // Function Prototypes -//float saturateCmd(float cmd); -//float wrapToPi(float ang); +void he_callback(); int 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); +// DigitalOut imu_LED(LED4); +DigitalOut hall_LED(LED4); -NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER); // Comms and control object definitions -//MODSERIAL pc(p13, p14); // tx, rx for serial USB interface to pc +//MODSERIAL pc(p28, p27); // tx, rx for serial USB interface to pc //MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc @@ -63,16 +57,20 @@ ServoIn CH1(p15); ServoIn CH2(p16); -//InterruptIn he_sensor(p11); - +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,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 psi_err,spd_err, psi_err_i,spd_err_i,speed; // 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 float wheel_spd; // wheel speed variable @@ -90,10 +88,16 @@ float y = 0.0; // simulation variables float psi = 0.0; // simulation variables float spd = 0.0; // simulation speed +float tau = 0.25; // throttle time constant +float Kdc = 1; // throttle dc gain +float thr_out,thr_out_old; +float KpWhl = 0.6; +float dz; +int reverse; -nav_EKF ekf; // initialize ekf states vehicleModel vm; //create vehicle model object + void rxCallback(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; @@ -108,14 +112,14 @@ pc.baud(115200); // set baud rate of serial comm to pc - xbee.baud(57600); // set baud rate of serial comm of wireless xbee comms + 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 xbee.attach(&rxCallback, MODSERIAL::RxIrq); - + - led1=led2=led3=led4=WHITE; // set color of neo strip lights? + he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt // initialize necessary float and boolean variables left = 0; @@ -131,8 +135,9 @@ arm_clock = 0; auto_clock = 0; Kp_psi = 1/1.57; - Kp_spd = 0.3; - Ki_spd = 0.05; + Kp_spd = 0.4; + Ki_spd = 0.2; + dz = 0.2; str_cond = false; thr_cond = false; armed = false; @@ -142,21 +147,20 @@ run_ctrl = false; log_data = false; cmd_mode = 1; + reverse = 0; + // 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 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 +// imu_LED = 0; // turn off IMU indicator (LED 4) // procedure to ensure IMU is operating correctly @@ -178,12 +182,12 @@ imu_LED = !imu_LED; } // end while(imu.calib) */ - imu_LED = 1; // turns on IMU light when calibration is successful + //imu_LED = 1; // turns on IMU light when calibration is successful } else { pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly - sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL) + //sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL) /* // turn on all lights is IMU is not connected correctly @@ -227,7 +231,7 @@ 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 3 seconds + if(t.read()-auto_clock > 1) { //Auto control timeout if no commands recevied after 1 seconds auto_ctrl = false; } // end if(t.read()-autoclock>3) timeout procedure @@ -259,46 +263,77 @@ parseMessage(buf); newpacket = false; // reset packet flag } // end if(newpacket) + float cdt = t.read()-t_ctrl; + t_ctrl = t.read(); 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) { - if(auto_ctrl) { - float cdt = t.read()-t_ctrl; - t_ctrl = t.read(); + switch (cmd_mode) { case DIRECT_MODE: { - str = str_cmd; - thr = thr_cmd; - // pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr); + + str = -str_cmd; + + + des_spd = thr_cmd; + spd_err = des_spd - speed; + spd_err_i += spd_err*cdt; + + // saturate integral term + if(spd_err_i> 1) { + spd_err_i = 1; + } + if(spd_err<-1) { + spd_err_i = -1; + } + + thr = Kp_spd*(des_spd-speed) + Ki_spd*spd_err_i; + + if(des_spd > 0.02){ + thr = thr + dz; + } + else if(des_spd < -0.02){ + // speed has no sign figure out how to make it track direction using accel perhaps. + thr = -(dz+0.2); + //thr = 0.3*des_spd; + }else { + thr = 0; + spd_err_i = 0; + } + //thr = thr_cmd; + // pc.printf("des_spd %.3f, Spd %.3f Thr_cmd %.3f rev %d\r\n",des_spd,reverse*speed,thr,reverse); // 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: { if(sim_mode==0) { // if hardware is enabled use gyro and ekf - 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(); + // 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-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/vm.getSpd(); - } else { - str = Kp_psi*psi_err/0.05; - }*/ + + // 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/vm.getSpd(); + } else { + str = Kp_psi*psi_err/0.05; + }*/ } // end if sim_mode + + thr = Kp_spd*spd_err + Ki_spd*spd_err_i*cdt; if (thr >= 1.0) { @@ -310,9 +345,9 @@ spd_err_i = 0.0; // Reset Integral If Saturated } // end iff thr<0 - - 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); - + + // pc.printf("Psi Err: %.3f, Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f rev %d\r\n",psi_err,str,spd_err,Kp_spd,thr,reverse); + break; } // end COURSE_MODE case @@ -324,13 +359,13 @@ 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 + // Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse + // Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse } else { // 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 @@ -345,14 +380,24 @@ 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 + //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 + float thr_dot = -(1/tau)*thr_out + (Kdc/tau)*thr; // first order filter for throttle + thr_out = thr_out_old + thr_dot*cdt; + thr_out_old = thr_out; + 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_out*500.0)+1500.0)); //Write Throttle Pulse + mot_control(thr); + // pc.printf("Motcontrol: %f %f\r\n",thr,thr_out); + } + if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off float dt = t.read()-t_imu; if(dt > (1/IMU_RATE)) { @@ -360,79 +405,114 @@ 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(); + // ******************************************* + // Get Sensor Data + // ******************************************* + // Compute speed from hall effect sensor + if(t.read()-t_hall < 0.2) { + speed = 0.5*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec + } else { + speed = 0; } - }else{ // Else we are running a simulation + + if(abs(speed) <0.01){ + if(imu.accel.x < -0.5 && thr_out < -0.05){ + reverse = -1; + }else{ + reverse = 1; + } + } + + + } // end if simmode ==0 + + + + + + } else { // Else we are running a simulation float T = t.read()-t_sim; - vm.stepModel(str,thr,T); + vm.stepModel(str,thr,T); + speed = vm.getSpd(); 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())); + 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())); + printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw())); + 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("$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("$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)); 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)); + // xbee.printf("Thr in %.3f Thr_out %.3f\r\n",thr,thr_out); + xbee.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str); + //pc.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str); + + // 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)) { - float tic2 = t.read(); - ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east); - t_gps = t.read(); - } - wait(1/LOOP_RATE); - // status_LED=!status_LED; - auto_ctrl_old = auto_ctrl; + 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)) { + float dt = t.read()-t_imu; + if(dt > (1/IMU_RATE)) { - float tic = t.read(); + float tic = t.read(); - x = x + spd*cos(psi)*dt; // self-propelled particle kinematics - y = y + spd*sin(psi)*dt; // self-propelled particle kinematics - psi = psi + str*dt; // turn rate kinematics - float drag = 0.0; - if(spd>1) { - drag = 0.0059*spd*spd; // based on drag on a cat sized object - } else { - drag = 0.0059*spd; - } - spd = spd + (thr-drag)*dt; // give throttle offset + x = x + spd*cos(psi)*dt; // self-propelled particle kinematics + y = y + spd*sin(psi)*dt; // self-propelled particle kinematics + psi = psi + str*dt; // turn rate kinematics + float drag = 0.0; + if(spd>1) { + drag = 0.0059*spd*spd; // based on drag on a cat sized object + } else { + drag = 0.0059*spd; + } + spd = spd + (thr-drag)*dt; // give throttle offset - xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi)); - 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); - t_imu = t.read(); - } // end if dt + xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi)); + 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); + t_imu = t.read(); + } // end if dt - } // end if sim_mode */ + } // end if sim_mode */ } // end while(1) } // end main + + +void he_callback() +{ + // Hall effect speed sensor callback + hall_LED = !hall_LED; + + dt_hall = t.read()-t_hall; + t_hall = t.read(); +} // end void he_callback + + + int parseMessage(char * msg) { @@ -457,7 +537,7 @@ default: { } } - return 1; + return 1; } //emd of $CMD if(!strncmp(msg, "$PRM", 4)) { @@ -485,7 +565,7 @@ float ref_lat = arg1; float ref_lon = arg2; float ref_alt = 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); @@ -494,23 +574,11 @@ } default: { } - + } return 1; } // end of $PRM - if(!strncmp(msg, "$LED", 4)) { - int arg1; - float arg2; - sscanf(msg,"$LED,%x,%f",&arg1,&arg2); - //pc.printf("%s\n",msg); - 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; @@ -530,7 +598,7 @@ y = arg3; psi = arg4; vm.initialize(x,y,psi); - + } default: { } @@ -540,22 +608,3 @@ return 0; } -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(); -} -