USNA WSE ES456
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 
Emaxx Navigation Group
Revision 9:3aa9b689bda5, committed 2018-04-03
- Comitter:
- jdawkins
- Date:
- Tue Apr 03 12:52:52 2018 +0000
- Parent:
- 8:9817993e5df7
- Commit message:
- Publish Code for MIDS
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Emaxx_Navigation_madpulse.lib Tue Apr 03 12:52:52 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/Emaxx_Navigation_Dynamic_HIL/#9817993e5df7
--- 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)) {
--- a/motor_driver.h Fri Apr 07 17:12:29 2017 +0000
+++ b/motor_driver.h Tue Apr 03 12:52:52 2018 +0000
@@ -1,7 +1,7 @@
-DigitalOut mot_ph1(p29, 0);
-DigitalOut mot_ph2(p30, 0);
-PwmOut mot_en(p26);
+DigitalOut mot_ph1(p23, 0);
+DigitalOut mot_ph2(p24, 0);
+PwmOut mot_en(p25);
void mot_control(float dc){
if(dc>1.0)