Diff: main.cpp

Revision:

1:cb84b477886c

Parent:

0:a6a169de725f

Child:

2:fbc6e3cf3ed8

--- a/main.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/main.cpp	Tue May 28 13:58:35 2013 +0000
@@ -1,4 +1,4 @@
-/** Code for "Data Bus" UGV entry for Sparkfun AVC 2012
+/** Code for "Data Bus" UGV entry for Sparkfun AVC 2013
  *  http://www.bot-thoughts.com/
  */
 
@@ -188,12 +188,12 @@
 
     // Send data back to the PC
     pc.baud(115200);
-    fprintf(stdout, "Data Bus mAGV Control System\n");
+    fprintf(stdout, "Data Bus 2013\n");
 
     display.init();
-    display.status("Data Bus mAGV V3");
+    display.status("Data Bus 2013");
    
-    fprintf(stdout, "Initialization...\n");
+    fprintf(stdout, "Initializing...\n");
     display.status("Initializing");
     wait(0.2);
     
@@ -210,18 +210,21 @@
     wait(0.2);
     if (config.load())                          // Load various configurable parameters, e.g., waypoints, declination, etc.
         confStatus = 1;
-        
+
     // Something here is jacking up the I2C stuff
+    // Also when initializing with ESC powered, it causes motor to run which
+    // totally jacks up everything (noise?)
     initSteering();
     initThrottle();
     // initFlasher();                                   // Initialize autonomous mode flasher
         
     sensors.Compass_Calibrate(config.magOffset, config.magScale);
-    pc.printf("Declination: %.1f\n", config.declination);
+    //pc.printf("Declination: %.1f\n", config.declination);
     pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", 
         config.escZero, config.escMax, config.topSpeed, config.turnSpeed, 
         config.speedKp, config.speedKi, config.speedKd);
-    pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n", config.steerZero, config.steerGain, config.steerGainAngle);
+    pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n",
+        config.steerZero, config.steerGain, config.steerGainAngle);
 
     // Convert lat/lon waypoints to cartesian
     mapper.init(config.wptCount, config.wpt);
@@ -231,9 +234,9 @@
                     w, config.cwpt[w]._x, config.cwpt[w]._y, config.wpt[w].latitude(), config.wpt[w].longitude());
     }
 
-    // TODO: 3 print mag and gyro calibrations
+    // TODO 3 print mag and gyro calibrations
 
-    // TODO 3: remove GPS configuration, all config will be in object itself I think
+    // TODO 3 remove GPS configuration, all config will be in object itself I think
 
     display.status("Nav configuration   ");
     steerCalc.setIntercept(config.interceptDist);               // Setup steering calculator based on intercept distance
@@ -245,8 +248,7 @@
     fprintf(stdout, "Calculating offsets...\n");
     display.status("Offset calculation  ");
     wait(0.2);
-    // TODO: 3 Really need to give the gyro more time to settle
-    // 5/20 - for some reason it is hanging here
+    // TODO 3 Really need to give the gyro more time to settle
     sensors.gps.disable();
     sensors.Calculate_Offsets();
 
@@ -303,20 +305,20 @@
         }*/
 
         if ((thisUpdate = timer.read_ms()) > nextUpdate) {
-            // TODO 1: wtf is this?!?!  why not pull out of state[outstate]
+            // Pulling out current state so we get the most current
             SystemState s = state[inState];
+            // Now populate in the current GPS data
             s.gpsHDOP = sensors.gps.hdop();
             s.gpsSats = sensors.gps.sat_count();
             display.update(s);
             nextUpdate = thisUpdate + 2000;
+            // TODO move this statistic into display class
             fprintf(stdout, "update time: %d\n", getUpdateTime());
         }
         
         if (keypad.pressed) {
             keypad.pressed = false;
             printLCDMenu = true;
-            //causes issue with servo library
-            //speaker.beep(3000.0, 0.1); // non-blocking
             switch (keypad.which) {
                 case NEXT_BUTTON:
                     menu.next();
@@ -338,7 +340,6 @@
 
             
         if (printLCDMenu) {
-        
             display.menu( menu.getItemName() );
             display.status("Ready.");
             display.redraw();
@@ -506,13 +507,9 @@
     timer.start();
     wait(0.1);
     
-    // Initialize logging buffer
-    // Needs to happen after we've reset the millisecond timer and after
-    // the schedHandler() fires off at least once more with the new time
-    inState = outState = 0;    
-    
     // Tell the navigation / position estimation stuff to reset to starting waypoint
-    restartNav();
+    // Disable 05/27/2013 to try and fix initial heading estimate
+    //restartNav();
                 
     // Main loop
     //
@@ -529,14 +526,6 @@
         //
         // set throttle only if goGoGo set
         if (goGoGo) {
-            /** acceleration curve */
-            /*
-            if (go.ticked(timer.read_ms())) {
-                throttle = go.get() / 1000.0;
-                //fprintf(stdout, "throttle: %0.3f\n", throttle.read());
-            }
-            */
-            
             // TODO: 1 Add additional condition of travel for N meters before
             // the HALT button is armed
             
@@ -554,23 +543,15 @@
                 display.status("GO GO GO!");
                 goGoGo = true;
                 keypad.pressed = false;
-                //restartNav();
                 beginRun();
-                // Doing this for collecting step response, hopefully an S curve... we'll see.
-                //throttle = config.escMax;
-                // TODO: 2 Improve encapsulation of the scheduler
-                // TODO: 2 can we do something clever with GPS position estimate since we know where we're starting?
-                // E.g. if dist to wpt0 < x then initialize to wpt0 else use gps
             }
         }        
 
         // Are we at the last waypoint?
         // 
         if (state[inState].nextWaypoint == config.wptCount) {
-            fprintf(stdout, "Arrived at final destination. Done.\n");
-            //causes issue with servo library
-            //speaker.beep(3000.0, 1.0); // non-blocking
-            display.status("Arrived. Done.");
+            fprintf(stdout, "Arrived at final destination.\n");
+            display.status("Arrived at end.");
             navDone = true;
             endRun();
         }
@@ -579,14 +560,15 @@
         // LOGGING
         //////////////////////////////////////////////////////////////////////////////
         // sensor reads are happening in the schedHandler();
-        // Are there more items to come out of the log buffer?
+        // Are there more items to come out of the log fifo?
         // Since this could take anywhere from a few hundred usec to
         // 150ms, we run it opportunistically and use a buffer. That way
         // the sensor updates, calculation, and control can continue to happen
         if (outState != inState) {
             logStatus = !logStatus;         // log indicator LED
             //fprintf(stdout, "FIFO: in=%d out=%d\n", inState, outState);
-            if (ssBufOverrun) {
+ 
+            if (ssBufOverrun) { // set in update()
                 fprintf(stdout, ">>> SystemState Buffer Overrun Condition\n");
                 ssBufOverrun = false;
             }
@@ -603,13 +585,11 @@
         }
 
     } // while
-    
     closeLogfile();
+    wait(2.0);
     logStatus = 0;
-    fprintf(stdout, "Completed, file saved.\n");
-    wait(2); // wait from last printout
-    display.status("Done. Saved.");
-    wait(2);        
+    display.status("Completed. Saved.");
+    wait(2.0);
 
     ahrsStatus = 0;
     gpsStatus = 0;
@@ -942,13 +922,13 @@
                 fprintf(stdout, "Rangers: L=%.2f R=%.2f C=%.2f", sensors.leftRanger, sensors.rightRanger, sensors.centerRanger);
                 fprintf(stdout, "\n");
                 //fprintf(stdout, "ahrs.MAG_Heading=%4.1f\n",  ahrs.MAG_Heading*180/PI);
-                fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]);
-                fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2],
-                        sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] ));
+                //fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]);
+                //fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2],
+                //        sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] ));
                 fprintf(stdout, "g=(%4d, %4d, %4d) %d\n", sensors.g[0], sensors.g[1], sensors.g[2], sensors.gTemp);
                 fprintf(stdout, "gc=(%.1f, %.1f, %.1f)\n", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]);
                 fprintf(stdout, "a=(%5d, %5d, %5d)\n", sensors.a[0], sensors.a[1], sensors.a[2]);
-                fprintf(stdout, "estHdg=%.2f\n", state[inState].estHeading);
+                fprintf(stdout, "estHdg=%.2f lagHdg=%.2f\n", state[inState].estHeading, state[inState].estLagHeading);
                 //fprintf(stdout, "roll=%.2f pitch=%.2f yaw=%.2f\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw));
                 fprintf(stdout, "speed: left=%.3f  right=%.3f\n", sensors.lrEncSpeed, sensors.rrEncSpeed);
                 fprintf(stdout, "gps=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n",