Commit
Dependencies: BNO055_fusion GPSINT Navigation_EKF NeoStrip ServoIn ServoOut mbed MODSERIAL
Revision 6:f64b1eba4d5e, committed 2016-12-14
- Comitter:
- lddevrie
- Date:
- Wed Dec 14 15:47:49 2016 +0000
- Parent:
- 5:d6d8ecd418cf
- Commit message:
- Updates to SIL kinematics and general commenting of code
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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)