![](/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:
- 6:f64b1eba4d5e
- Parent:
- 5:d6d8ecd418cf
- Child:
- 7:a8c2e9d049e8
diff -r d6d8ecd418cf -r f64b1eba4d5e main.cpp --- a/main.cpp Sat Dec 10 01:44:58 2016 +0000 +++ b/main.cpp Wed Dec 14 15:47:49 2016 +0000 @@ -55,8 +55,11 @@ NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER); // Comms and control object definitions -Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc -MODSERIAL xbee(p13, p14); // tx, rx for Xbee +Serial pc(p13, p14); // tx, rx for serial USB interface to pc +MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee + +// Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc +// MODSERIAL xbee(p13, p14); // tx, rx for Xbee ServoIn CH1(p15); ServoIn CH2(p16); @@ -332,7 +335,7 @@ }/// end else armed - if(sim_mode==0) { + if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off imu.get_angles(); imu.get_accel(); imu.get_gyro(); @@ -376,9 +379,9 @@ float tic = t.read(); - x = x + spd*cos(psi)*dt; - y = y + spd*sin(psi)*dt; - psi = psi + str*dt; + 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 @@ -390,10 +393,10 @@ 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 while(1)