![](/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:
- 9:3aa9b689bda5
- Parent:
- 8:9817993e5df7
diff -r 9817993e5df7 -r 3aa9b689bda5 main.cpp --- a/main.cpp Fri Apr 07 17:12:29 2017 +0000 +++ b/main.cpp Tue Apr 03 12:52:52 2018 +0000 @@ -57,14 +57,11 @@ ServoIn CH1(p15); ServoIn CH2(p16); -InterruptIn he_sensor(p11); -ServoOut Steer(p22); +InterruptIn he_sensor(p15); +ServoOut Steer(p20); ServoOut Throttle(p21); - - - Timer t; // create timer instance Ticker log_tick; Ticker heartbeat; @@ -119,7 +116,7 @@ xbee.attach(&rxCallback, MODSERIAL::RxIrq); - he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt + // he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt // initialize necessary float and boolean variables left = 0; @@ -223,6 +220,7 @@ rc_conn = true; } // end if(Channels connected) + // pc.printf("Str %d Thr %d\r\n",CH1.servoPulse,CH2.servoPulse); // turn on RC led if transmitter is connected rc_LED = rc_conn; auto_LED = auto_ctrl; @@ -245,7 +243,6 @@ int i = 0; if(xbee.rxBufferGetCount()>0) { char c = xbee.getc(); - //pc.printf("%s",c); if(c=='$') { buf[i] = c; i++; @@ -259,10 +256,11 @@ } } // end if xbee.rxBufferGetCount xbee.rxBufferFlush();// empty receive buffer - //pc.printf("%s",buf); // print message to PC + pc.printf("%s",buf); // print message to PC parseMessage(buf); newpacket = false; // reset packet flag } // end if(newpacket) + float cdt = t.read()-t_ctrl; t_ctrl = t.read(); @@ -277,7 +275,6 @@ str = -str_cmd; - des_spd = thr_cmd; spd_err = des_spd - speed; spd_err_i += spd_err*cdt; @@ -310,30 +307,6 @@ } // 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(); - } 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; - }*/ - } // end if sim_mode - - - thr = Kp_spd*spd_err + Ki_spd*spd_err_i*cdt; if (thr >= 1.0) { @@ -357,14 +330,6 @@ } // end switch(cmd_mode) 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 - } else { - // won't send command to motor and servo if in SIL mode - } - } else { // goes with if auto_ctrl, manual control mode @@ -390,15 +355,14 @@ 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 + thr_out_old = thr_out;// + 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); - } + pc.printf("crrl cmd: %f %f\r\n",str,thr); - 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)) { imu.get_angles(); @@ -427,40 +391,16 @@ } } - - } // end if simmode ==0 - - - - - - } else { // Else we are running a simulation - float T = t.read()-t_sim; - 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())); - 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()); - 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)); // 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(); } @@ -468,7 +408,7 @@ // 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)) {