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)