![](/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:
- 5:d6d8ecd418cf
- Parent:
- 4:e27ca0c82814
- Child:
- 6:f64b1eba4d5e
--- a/main.cpp Thu Dec 08 20:38:24 2016 +0000 +++ b/main.cpp Sat Dec 10 01:44:58 2016 +0000 @@ -85,7 +85,7 @@ float x = 0.0; // simulation variables float y = 0.0; // simulation variables float psi = 0.0; // simulation variables -float spd = 0.005; // simulation speed +float spd = 0.0; // simulation speed void rxCallback(MODSERIAL_IRQ_INFO *q) { @@ -225,7 +225,7 @@ if(newpacket) { // if xbee port receives a complete message, parse it - char buf[MAX_MESSAGE_SIZE]; + char buf[MAX_MESSAGE_SIZE]; // create buffer for message // reads from modserial port buffer, stores characters into string "buf" int i = 0; @@ -243,15 +243,15 @@ i++; } } - } + } // end if xbee.rxBufferGetCount xbee.rxBufferFlush();// empty receive buffer - pc.printf("%s",buf); + pc.printf("%s",buf); // print message to PC parseMessage(buf); - newpacket = false; + newpacket = false; // reset packet flag } // end if(newpacket) - if(!rc_conn) { // Is System Armed + 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) { @@ -265,10 +265,21 @@ } // 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/ekf.getSpd(); + 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(); + } else { // otherwise design control using simulated variables, bypass ekf states + psi_err = wrapToPi(des_psi-psi); + spd_err = des_spd - spd; + spd_err_i += spd_err; + if(spd>0.05) { + str = Kp_psi*psi_err/spd; + } else { + str = Kp_psi*psi_err/0.05; + } + } // end if sim_mode thr = Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE; @@ -276,8 +287,8 @@ thr = 1.0; spd_err_i = 0; // Reset Integral If Saturated } // end if thr>=1.0 - if (thr < 0) { - thr = 0; + if (thr < 0.0) { + thr = 0.0; spd_err_i = 0; // Reset Integral If Saturated } // end iff thr<0 @@ -308,12 +319,16 @@ } // end else } else { // goes with if !rc_conn + + // for manual driving 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 + 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 + } }/// end else armed @@ -359,20 +374,21 @@ if(dt > (1/IMU_RATE)) { float tic = t.read(); - - psi_err = wrapToPi(des_psi-psi); - spd_err = des_spd - spd; - spd_err_i += spd_err; - str = Kp_psi*psi_err/(spd+0.05); - thr = Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE; - + + x = x + spd*cos(psi)*dt; y = y + spd*sin(psi)*dt; psi = psi + str*dt; - spd = spd + thr*dt; + 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\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(); }