Revision 1:87b28e8b9941, committed 2014-06-24

Comitter:

redxeth

Date:

Tue Jun 24 06:38:55 2014 +0000

Parent:

0:98e98e01a6ce

Child:

2:999ca57934bf

Commit message:

Updated to support Mentoring Matters 2014

Changed in this revision

--- a/Spices/Spices.cpp	Sun Apr 20 03:33:03 2014 +0000
+++ b/Spices/Spices.cpp	Tue Jun 24 06:38:55 2014 +0000
@@ -15,30 +15,26 @@
 #define DER_RATIO 0.5                              // ratio for der threshold level (was 0.5 initially, may put back)
 
 // steer/servo params
-#define MAX_STEER_LEFT -0.51                       // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach)
-#define MAX_STEER_RIGHT 0.39                       // value determined by demo mode 1 measure
+#define MAX_STEER_LEFT -0.42                       // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach)
+#define MAX_STEER_RIGHT 0.43                       // value determined by demo mode 1 measure
 #define DT 0.02                                    // # MS of time between intervals (doesn't really matter)
 
 // logging parameters
 #define NUM_LOG_FRAMES 700                         // # of frames to log (when logging active) ~14 sec worth!
 
 // ******  for debug tuning   ******
-#define TUNE_SPEED 0.7
 #define TUNE_KP 0.008
 #define TUNE_KI 0
 #define TUNE_KD 0
 #define MIN_POWER 60                               // percent min power (estimating for a 2-ft turn => 24" / (24" + 6" car) = 4/5; speed of inner wheel is 20% lower worst case
-#define SPEED_ADJUST 4
+#define SPEED_ADJUST 4                             // do not change
 #define ABS_ERROR_THRESH 10                        // number of pixels line position offset before changing KP value
 #define CONTROL_METHOD 2                           // which control method to use
 
 
 // Drive/motor params
-// 0.4 way too slow!!  need to charge battery
-#define SUB_LIGHT_SPEED 0.5                        // moderate speed (value 0 to 1 sent to motors)
-#define LIGHT_SPEED 0.6                            // fast...
-#define RIDICULOUS_SPEED 0.7                       // faster...
-#define LUDICROUS_SPEED 0.9                        // faster still!
+#define LIGHT_SPEED 0.4                            // easy speed
+#define LUDICROUS_SPEED 0.6                        // faster speed
 #define MAX_POWER 100                              // percent max power (for speed adjustments)
 
 // algo params
@@ -46,79 +42,6 @@
 #define STARTGATEFOUNDMAX  0                       // max value to allow for finding starting gate before killing engine
 #define STARTGATEDELAY     50                      // Delay before searching for starting gate to kill engine
 
-#define MMA8451_I2C_ADDRESS (0x1d<<1)              // address for accelerometer?
-
-
-/* CAR INTERFACE
-
-  DIP SWITCH:
-  -----------------------------------------------------------------
-  1 - ON: Run MCP below; OFF: Run Demo program (see main.cpp)
-  2 - ON: Log frame data to array
-  3 - ON: Risky race option; OFF: Conservative race option
-  4 - ON: Start Gate Kill Switch Active
-  
-  POTS
-  -----------------------------------------------------------------
-  0 - controls nothing at the moment
-  1 - controls nothing at the moment
-
-  PUSHBUTTONS  
-  -----------------------------------------------------------------
-  A -  START car race!
-  B -  END CAR RACE / (while holding down when 'log frame data' active will also output terminal data)
-  
-*/
-
-// LEARNING CAR CLUB 9/10/13: 
-// IP need to test-- get around U turns -- more aggressive proportional control?  Or derivative?  Have camera look farther ahead but not too far ahead
-// IP need to test -- fix lighting for tunnels (if good algo doesn't need lighting!!)
-// -- increase power up to get up hills, brake down hills (accel: http://mbed.org/users/SomeRandomBloke/code/MMA8451Q/#)
-// -- add speed control? (hall effect sensor)
-// DONE -- make sure steering won't go past limits!!
-// 9/12/13 - adjust camera exposure time based on maximum light intensity!!
-// 9/14/13 - DONE -- make derivative threshold related to maximum light intensity!
-// 9/16/13 - crash at car club blew out resistor R8, replaced it and getty twitching when powering servo from USB only.  Ok when powering from battery.
-// 9/17/13 - experiments show that derivative control just doesn't work very well-- the tinest error delta causes huge drastic changes, need to use non-linear proportional
-//           control instead... parabolic??
-//           DONE -- Also need to slow down on curves
-//           speed up hills and slow down on downhill...
-//           measure speed?
-// 9/18/13 - Test track work: doesn't appear to be sampling camera fast enough--  not able to handle the wiggly track and sometimes not able to handle the curves!
-//           TODO: Need to increase rate at which camera sampled and decisions are made!!  Look to codewarrior code
-//           Need to cut off left/right ends of camera data-- seems to not read line properly in well lit rooms
-//           DONE Speed adjust as you go round; Need to have it slow down "into" curve, speed up again "out of curve"
-// 9/23/13 - Definitely not processing camera fast enough-- seems to not react well when speed up the car.  Goes slow just fine all the way 'round.
-//           TODO: Need to see how often TFC_LineScanImageReady gets updated. If update camera sample freq how will that impact exposure? Light adjustment algos should
-//           be able to handle it. Need to be able to measure the 'processing time' required.  Also wondering if 20mS is fast enough for updating the servo?? Need
-//           to calculate how fast servo needs to really go based on track curves and speed.
-//           TODO: Need a way to measure speed I think as well.
-//           TODO: Need to control speed differentially across the different motors!! See Eli video!!
-//           TODO: Use this to get excel feedback quicker: http://strokescribe.com/en/serial-port-download.html  (Doesn't work too well-- prefer my own excel method)
-//           TODO: Use PID control for steering!
-//           TODO: Use Speed control (PID?) --- add speed sensor!
-// 10/8/13   Reduced speed control to 90% (was 85%)-- seems to go off track and lose line at high speed-- mainly on the U turns
-//           Need to figure out why derivative control in steering doesn't work well.  Add integral control.
-//           Latest track times (no hill, no bumps, no tunnel, only squiggles) = 11.8sec at high speed with speed control
-//           Losing time on the curves-- need to optimize!!
-//           Worry about hill later-- need to get track times down around 8sec first.
-//        *****************************
-//        ** Implement some method to acount for U-TURNS on track-- and to help even go beyond what camera can see as far as estimating line position
-//        ** (U-TURNS take the longest time out of track)
-//        ** Method to help find the line even when not really visible
-//        *****************************
-// 10/17/13
-//        NEED CURVE AND WIGGLE DETECT!!
-// 10/20/13  Added logging capability and 'algo time' detect
-//           It is not finding the line on the edges when on a curve-- likely because my line detect algo requires both edges
-//           TODO-- need method that can use only one edge!
-
-// TODO LIST
-// - Speed Control via Sensor WAITING ON SENSOR
-// - Differential drive around curves DONE -- still trying to figure out what %age to drop on turns
-// - Full PID steering control IN PROGRESS
-// - Starting gate kill engine debug IN PROGRESS -- need to add delay
-// - Off track kill engine debug -- IN PROGRESS-- seems to kill car prematurely
 
 // image processing vars
 uint16_t   GrabLineScanImage0[NUM_LINE_SCAN];      // snapshot of camera data for this 'frame'
@@ -149,7 +72,7 @@
 float      CurrentRightDriveSetting = 0;           // Drive setting (right wheel)
 
 // Speed control vars
-float      MaxSpeed;                                    // maximum speed allowed
+float      MaxSpeed;                               // maximum speed allowed
 
 uint16_t   startRaceTicker;                        // ticker at start of race1
 
@@ -191,48 +114,27 @@
 
 // EXTRA CONTROL PARAMETERS
 bool debugFakeMode = false;         // if true, ignores real camera and uses fake camera input instead; used for data processing debug
-int terminalOutput = 0;             // set debug level for terminal output
-                                    //    0 : no terminal output, race!
-                                    //    1 : output just to measure frame rate
-                                    //    2 : output for measuring time of operations
-                                    //    3 : output with delay
+bool terminalOutput = false;        // whether output terminal data
 bool doLogData = false;             // whether to capture log data to output later on
-bool killSwitch = false;             // whether to enable Kill Switch (allow engine to stop after not finding track)
+bool killSwitch = false;            // whether to enable Kill Switch (allow engine to stop after not finding track)
 bool startGateStop = false;         // whether to stop or not depending on starting gate reading
 bool doRisky = false;               // race style-- whether conservative or risky
 
-// timer stuff
-Timer timer;
-int after_time, before_time, start_time, last_start_time;
-bool run_once = false;
-
-void MasterControlProgram()
-{
-
-  // put here all things want to run only once after reset
-  if (!run_once){
-    if ((terminalOutput == 1) || (terminalOutput == 2)){
-      timer.start();
-    }
-    run_once = true;
-  }
+void TrackMode()
+{  
+  // set mode based on DIP switches
+  useMode();
   
-  // read DIP switches and Pots for data
-  readSwitches();
+  // grab Terminal Mode based on DIP switch 4
+  terminalOutput = terminalMode();
                                                           
-  // Every 4s (or Terminal Output is off, i.e. race mode!)
+  // Every 2s (or Terminal Output is off, i.e. race mode!)
   //    AND line scan image ready (or fake mode where image is always ready)
-  //   (ticker updates every 2ms) (Line scan image ready very 20mS?)
-  if((TFC_Ticker[0]>2000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode))
+  //   (ticker updates every 10ms) (Line scan image ready every 10ms)
+  if((TFC_Ticker[0]>2000 || (!terminalOutput)) && (TFC_LineScanImageReady>0 || debugFakeMode))
    {
 
      // stuff that needs to be reset with each image frame
-     if (terminalOutput == 1) {
-       last_start_time = start_time;
-       start_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:Between frames:%d:uSec\r\n", start_time - last_start_time);
-       before_time = timer.read_us();
-     }
      TFC_Ticker[0] = 0;
      TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero
      MaxLightIntensity = 0;                  // reset
@@ -240,177 +142,90 @@
 
      // grab camera frame
      grabCameraFrame();
-     
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER grabCameraFrame:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     }
   
      // calcalate derivative of linescandata, filter starttime data
      derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]);
 
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER derivativeLineScan:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     }
             
      // adjust deriv threshold based on max lighting value
      // has to be called before find edges
      adjustLights();
      
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER adjustLights:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     } 
-         
      //find edges from derivative data
      findEdges_v2(&DerivLineScanImage0[0]);
-
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER findEdges_v2:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     } 
-     
-     // turn on terminal output if line not found -- FOR DEBUG
-     //if (CurrentTrackStatus == Unknown)
-     //  terminalOutput = 1;
-
+    
      //review edge data and set position or starting flag appropriately
      reviewEdges();
-     
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER reviewEdges:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     }      
-    
-     if (terminalOutput == 3) {
-       // print data to Terminal for camera 0
+        
+     // print terminal data if desired (slows down things)
+     if (terminalOutput) {
        printLineScanData(&GrabLineScanImage0[0]);
-
-     // print deriviative of linescandata, filter starttime data
        printDerivLineScanData(&DerivLineScanImage0[0]);
-       
        printAdjustLightsData();
-       
        printEdgesFound();
-
      }
      
      // ** Track Status available at this point **  
-      
      
-     // test out accelerometer
-    // accelTest();
-      
      // Update things based on latest track status
      // e.g. change steering setting, stop car, ...
      ActOnTrackStatus();
      
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER ActOnTrackStatus:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     }      
-
-
      //give LED feedback as to track status
      feedbackLights();
-     
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER feedbackLights:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();       
-     }  
-     
+    
      // control max power (speed)
      SpeedControl();
      
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER SpeedControl:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     }       
-
      // Drive!!     
      Drive();
      
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME:TO AFTER Drive:%d:uSec\r\n", after_time - before_time);
-       before_time = timer.read_us();
-     }         
-     
-     // wait_ms(1);
-
-     // Capture Log data while driving
-     if (go && doLogData) {
-       captureData();
-     }
-     
-     // Dump Log data to Terminal while stopped and holding B button
-     if (!go && doLogData && TFC_PUSH_BUTTON_1_PRESSED) {
-       dumpData();
-     }
-     
-     if (terminalOutput == 2) {
-       after_time = timer.read_us();
-       TERMINAL_PRINTF("TIME: ENTIRE FRAME (include prints):%d:uSec\r\n", after_time - start_time);
-       before_time = timer.read_us();
-     }
-
-     if (terminalOutput == 3) {
+     if (terminalOutput) {
        TERMINAL_PRINTF("\r\n**************************END********************************\r\n");
      }
 
    } 
 }
 
-void dumpData()
-{
-   TERMINAL_PRINTF("INDEX,LINEPOS,STEERSETTING,LEFTDRIVESETTING,RIGHTDRIVESETTING\r\n");
-       for(logDataIndex=0;logDataIndex<NUM_LOG_FRAMES;logDataIndex++) {
-         TERMINAL_PRINTF("%d,%6.2f,%6.2f,%6.2f,%6.2f\r\n",logDataIndex,frameLogs[logDataIndex].linepos,frameLogs[logDataIndex].steersetting,frameLogs[logDataIndex].leftdrivesetting,frameLogs[logDataIndex].rightdrivesetting);
-       }
+uint16_t getMode(){
+  // get mode based on first 3 DIP switches (1, 2, 3)
+  return (TFC_GetDIP_Switch()&0x07);
 }
 
-void captureData()
-{
-  frameLogs[logDataIndex].linepos = CurrentLinePosition;
-  frameLogs[logDataIndex].steersetting = CurrentSteerSetting;
-  frameLogs[logDataIndex].leftdrivesetting = CurrentLeftDriveSetting;
-  frameLogs[logDataIndex].rightdrivesetting = CurrentRightDriveSetting;
-
-  // increment index                
-  logDataIndex++;
-  if (logDataIndex > NUM_LOG_FRAMES) 
-    logDataIndex = 0;
-}
-
-void readSwitches()
+void useMode()
 {
 
-  // ********* GATHER DIP SWITCH INPUTS *********
-  if(TFC_GetDIP_Switch()&0x02)     // SWITCH 2 
-    doLogData = true;              // Log data to array
-  else
-    doLogData = false;             // normal operation
+  switch(getMode())
+  {
+  default:
+  case 4 :  // Track Mode, Auto Steer, safe settings
+      doRisky = false;
+      break;
+  
+  case 5 :  // Track Mode, Auto Steer, fast settings
+      doRisky = true;
+      break;
   
-  if(TFC_GetDIP_Switch()&0x04)     // SWITCH 3
-    doRisky = true;     
-  else
-    doRisky = false;          
-    
-  if(TFC_GetDIP_Switch()&0x08)     // SWITCH 4 control start stop gate
+  case 6 :
+      // NOT USED YET
+      break;
+  
+  case 7 :
+      // NOT USED YET
+      break;
+  
+  } // end case
+
+  if(TFC_GetDIP_Switch()&0x08)     // SWITCH 4 Terminal Output on/off
     startGateStop = true;
   else
     startGateStop = false;
 
+}
 
+bool terminalMode()
+{
+    return ((TFC_GetDIP_Switch()>>3)&0x01 == 1);
 }
 
 void grabCameraFrame()
@@ -595,7 +410,7 @@
   {  
      if (derivLineScanData[i] <= NegDerivThreshold)     // NEGATIVE EDGE FOUND!
      {
-       if (terminalOutput == 3) {
+       if (terminalOutput) {
          TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
        }
        
@@ -603,12 +418,12 @@
        {  // edge actually across multiple pixels
          multiNegEdgeCnt++;
          multiNegEdgeSum = multiNegEdgeSum + i;
-         if (terminalOutput == 3) {
+         if (terminalOutput) {
            TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum);
          }
        } else {  // not a multi-pixel edge known at this time, store negative edge index value
          numNegEdges++;
-         if (terminalOutput == 3) {
+         if (terminalOutput) {
            TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d.  NUM NEG EDGES = %d\r\n", i, numNegEdges);
          }
          NegEdges[numNegEdges - 1] = (float) i;
@@ -618,7 +433,7 @@
  
      } else if (derivLineScanData[i] > PosDerivThreshold) {    // POSITIVE EDGE FOUND!
      
-       if (terminalOutput == 3) {
+       if (terminalOutput) {
          TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
        }
        
@@ -626,11 +441,11 @@
        {  // edge actually across multiple pixels
          multiPosEdgeCnt++;
          multiPosEdgeSum = multiPosEdgeSum + i;
-         if (terminalOutput == 3) {
+         if (terminalOutput) {
            TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum);
          }
        } else {  // not a multi-pixel edge known at this time, store Posative edge index value
-         if (terminalOutput == 3) {
+         if (terminalOutput) {
            TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d.  NUM POS EDGES = %d\r\n", i, numPosEdges);
          }
          numPosEdges++;
@@ -681,7 +496,7 @@
      if (derivLineScanData[i] <= NegDerivThreshold)          // NEGATIVE EDGE FOUND!
      {
        
-       if (terminalOutput == 3) {
+       if (terminalOutput) {
          TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
        }
 
@@ -690,7 +505,7 @@
        
      } else if (derivLineScanData[i] > PosDerivThreshold) {  // POSITIVE EDGE FOUND!
        
-       if (terminalOutput == 3) {
+       if (terminalOutput) {
          TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
        }
 
@@ -804,7 +619,7 @@
   
    // remove edges that aren't close to center TBD DDHH
    
-      if (terminalOutput == 3) {
+      if (terminalOutput) {
          TERMINAL_PRINTF("***************************************** \r\n");
          TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
          TERMINAL_PRINTF("***************************************** \r\n");
@@ -813,7 +628,7 @@
   
   } else {  // no track or starting gate found
   
-    if (terminalOutput == 3) {
+    if (terminalOutput) {
       TERMINAL_PRINTF("***************************************** \r\n");
       TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition);
       TERMINAL_PRINTF("***************************************** \r\n");
@@ -834,7 +649,7 @@
 
   if (CurrentTrackStatus == LineFound)   {             // LINE FOUND!
   
-    if (terminalOutput == 3) {
+    if (terminalOutput) {
       TERMINAL_PRINTF("***************************************** \r\n");
       TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
       TERMINAL_PRINTF("***************************************** \r\n");
@@ -848,7 +663,7 @@
     
   } else if (CurrentTrackStatus == StartGateFound) {   // STARTING GATE FOUND
   
-    if (terminalOutput == 3) {
+    if (terminalOutput) {
       TERMINAL_PRINTF("***************************************** \r\n");
       TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n");
       TERMINAL_PRINTF("**********     count = %d      ********** \r\n", StartGateFoundCount);
@@ -938,7 +753,7 @@
   */
   
   
-  if (terminalOutput == 3) {
+  if (terminalOutput) {
     TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
     TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition);
   }
@@ -952,7 +767,7 @@
 
     DerivError = (CurrentLinePosError - LastLinePosError) / DT;
     
-    if (terminalOutput == 3) {
+    if (terminalOutput) {
       TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition);
       TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError);
     }
@@ -968,7 +783,7 @@
     // Derivative control term
     Dout = KD * DerivError;
 
-    if (terminalOutput == 3) {
+    if (terminalOutput) {
       TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
       TERMINAL_PRINTF("KI = %6.4f\r\n", KI);
       TERMINAL_PRINTF("KD = %6.4f\r\n", KD);
@@ -1011,7 +826,7 @@
        CurrentSteerSetting = MAX_STEER_LEFT;
     }
 
-    if (terminalOutput == 3) {
+    if (terminalOutput) {
       TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting);
     }
     TFC_SetServo(0,CurrentSteerSetting);  
@@ -1027,34 +842,11 @@
   float ErrLimit;
   float LeftDriveRatio, RightDriveRatio;
   
-  // set maximum speed allowed
-  switch (1)
-    {
-      case 0:
-        // read value off pot0
-        MaxSpeed = TFC_ReadPot(0);
-        break;
-      case 1:
-        if (doRisky)
-          MaxSpeed = TUNE_SPEED + 0.1;
-        else 
-          MaxSpeed = TUNE_SPEED;
-        break;
-      case 2:
-        MaxSpeed = SUB_LIGHT_SPEED;
-        break;
-      case 3:
-        MaxSpeed = LIGHT_SPEED;
-        break;
-      case 4:
-        MaxSpeed = RIDICULOUS_SPEED;      
-        break;
-      case 5:
-        MaxSpeed = LUDICROUS_SPEED;      
-        break;        
-      default:
-        break;
-    }
+  // set maximum speed
+  if (doRisky)
+    MaxSpeed = LUDICROUS_SPEED; // faster
+  else
+    MaxSpeed = LIGHT_SPEED;     // slower
     
   switch (SPEED_ADJUST)
    {
@@ -1085,13 +877,12 @@
      case 3:
        // SPEED ADJUST METHOD 3
        // have wheel relative speed proportional to absolute value of line error
-       ErrLimit = ((float) RANGE )  * 0.5 * ERR_RATIO;
-       if (CurrentLinePosError > 0) {           // heading right
+       if (CurrentLinePosError > 0) {           // heading right, slow right wheel down
          LeftDriveRatio = MAX_POWER;
-         RightDriveRatio = (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER;
-       } else if (CurrentLinePosError < 0) {    // heading left
+         RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
+       } else if (CurrentLinePosError < 0) {    // heading left, slow left wheel down
+         LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative
          RightDriveRatio = MAX_POWER;
-         LeftDriveRatio = (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER;
        } else {
          LeftDriveRatio = MAX_POWER;
          RightDriveRatio = MAX_POWER;
@@ -1102,13 +893,13 @@
        // have wheel relative speed proportional to absolute value of line error
        // only when above a certain error
        ErrLimit = ((float) RANGE )  * 0.5 * ERR_RATIO * 0.1;
-       if (CurrentLinePosError > ErrLimit) {           // heading right
-         LeftDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
-         RightDriveRatio = MIN_POWER;
-       } else if (CurrentLinePosError < (-1 * ErrLimit)) {    // heading left
+       if (CurrentLinePosError > ErrLimit) {                  // right turn-- slow right wheel down a bit
+         LeftDriveRatio = MAX_POWER;
          RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
-         LeftDriveRatio = MIN_POWER;
-       } else {
+       } else if (CurrentLinePosError < (-1 * ErrLimit)) {    // left turn-- slow left wheel down a bit
+         LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative
+         RightDriveRatio = MAX_POWER;
+       } else { // when in center drive full speed
          LeftDriveRatio = MAX_POWER;
          RightDriveRatio = MAX_POWER;
        }
@@ -1148,17 +939,13 @@
   CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed;
 
   
-  if (terminalOutput == 3) {
+  if (terminalOutput) {
     TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError);
     TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit);
-    TERMINAL_PRINTF("MAX SPEED = %5.2f\n", MaxSpeed);
+    TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed);
     TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
     TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
   }
-  if (1 == 0) {
-   TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
-   TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
-  }
 
 }
 
@@ -1235,7 +1022,7 @@
 
 void printAdjustLightsData()
 {
-  if (terminalOutput == 3) {
+  if (terminalOutput) {
     TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity);
     TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity);
     TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold);

--- a/Spices/Spices.h	Sun Apr 20 03:33:03 2014 +0000
+++ b/Spices/Spices.h	Tue Jun 24 06:38:55 2014 +0000
@@ -67,8 +67,10 @@
 //
 // *******************************************
 //
-void MasterControlProgram();
+void TrackMode();
 
+uint16_t getMode();
+bool terminalMode();
 
 // prints out line scan data for you
 void printLineScanData(uint16_t *LineScanData);
@@ -121,8 +123,7 @@
 // Give user feedback as to detection state via LEDs
 void feedbackLights();
 
-// read DIP switches and potentiometers for setting changes
-void readSwitches();
+void useMode();
      
 // capture log data
 void captureData();

--- a/main.cpp	Sun Apr 20 03:33:03 2014 +0000
+++ b/main.cpp	Tue Jun 24 06:38:55 2014 +0000
@@ -4,6 +4,76 @@
 #include "common.h"
 #include "Spices.h"
 
+// Exercises designed for Mentoring Matters Car Summer Camp
+//   for Summer 2014
+//   at Freescale, Inc.
+//   in Austin, Texas
+//
+// 5 MODES OR EXERCISES THAT WILL ALIGN WITH DIP SWITCH SETTINGS IN BINARY FASHION
+//  e.g. Mode 1 = 001 = switch 1 is on, switch 2 is off, switch 3 is off
+//
+// Modes:
+//  0 = 000 = Garage Mode, button light test to see if car alive!!
+//             PUSHBUTTON A - Light LEDs 0 and 1
+//             PUSHBUTTON B - Light LEDs 2 and 3
+//            -switch 4 does nothing-
+//
+//  1 = 001 = Garage Mode, forward/reverse adjust, no auto steer, terminal output
+//            -switch 4 does nothing-
+//
+//  2 = 010 = Garage Mode, steering adjust, no auto steer, terminal output
+//             POT 1 - Controls Servo:
+//                CCW = turn left
+//                CW = turn right
+//            -switch 4 does nothing-
+//
+//  3 = 011 = Garage Mode, Camera test, some auto steer, terminal output
+//            switch 4:
+//               OFF = normal dec data
+//                ON = o-scope mode
+//
+//  4 = 100 = Track Mode, Auto Steer, safe settings
+//            switch 4 = terminal output on/off
+//
+//  5 = 101 = Track Mode, Auto Steer, fast settings
+//            switch 4 = terminal output on/off
+//
+//  6 = 110 = future upgrades
+//
+//  7 = 111 = future upgrades
+
+/* NOTES
+Camera unmounted
+motors unhooked
+only servo / steering hooked up 
+
+exercise 1 - garage drive
+- manual motors forward / reverse
+- ensure motors hooked up properly - forward/reverse/left/right
+- show terminal value
+
+exercise 2 - garage steer
+- manual steering - left /right 
+- calibrate steering
+- show terminal to get feedback
+
+exercise 3 - garage see
+- focus camera
+- mount camera
+- camera + servo line tracking (race mode)
+garage mode use paper show following
+- bad Kp value
+- min speed = 0.5
+
+exercise 4 - track - slow
+- fine tuning max speed
+- fine tune proportional control - Kp
+
+exercise 5 - track - fast
+- ratio of differential motor speed on curves
+- dead zone with high speed on straights
+
+*/
 
 void TFC_TickerUpdate()
 {
@@ -18,147 +88,113 @@
     }
 }
 
-void DemoProgram()
+// Garage Mode
+//   Car not meant to run on track
+//   Use this to test out car features
+//   and calibrate car
+//
+void GarageMode()
 {
-  uint32_t i,j,t = 0;
+  uint32_t i,j = 0;
   float ReadPot0, ReadPot1;
 
 
-  //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
-  //Let's look at the middle 2 switches
-  switch((TFC_GetDIP_Switch()>>1)&0x03)
+  //This Demo program will look at the first 2 switches to select one of 4 demo / garage modes
+  switch(TFC_GetDIP_Switch()&0x03)
   {
   default:
-  case 0 :
-      //Demo mode 0 just tests the switches and LED's
+  case 0 : // Mode 0
+      TFC_HBRIDGE_DISABLE;
+
+      TERMINAL_PRINTF("MODE 0\r\n");  
+      
       if(TFC_PUSH_BUTTON_0_PRESSED)
+      {
           TFC_BAT_LED0_ON;
-      else
+          TFC_BAT_LED1_ON;
+      } else {
           TFC_BAT_LED0_OFF;
+          TFC_BAT_LED1_OFF;
+      }
       
       if(TFC_PUSH_BUTTON_1_PRESSED)
+      {
+          TFC_BAT_LED2_ON;
           TFC_BAT_LED3_ON;
-      else
+      } else {
+          TFC_BAT_LED2_OFF;
           TFC_BAT_LED3_OFF;
-      
-      
-/*      if(TFC_GetDIP_Switch()&0x01)
-          TFC_BAT_LED1_ON;
-      else
-          TFC_BAT_LED1_OFF;
-      
-      if(TFC_GetDIP_Switch()&0x08)
-          TFC_BAT_LED2_ON;
-      else
-          TFC_BAT_LED2_OFF; */
-      
+      }     
       break;
           
-  case 1:
-     TFC_HBRIDGE_DISABLE;
-                    
-     // if (TFC_HBRIDGE_ENABLED) {
-        
-    //    TFC_HBRIDGE_ENABLED = false;
-    //  }
-
-      //Demo mode 1 will just move the servos with the on-board potentiometers
-      if(TFC_Ticker[0]>=20) // every 40mS...
-      {
-          TFC_Ticker[0] = 0; //reset the Ticker
-          //update the Servos
-          ReadPot0 = TFC_ReadPot(0);
-          ReadPot1 = TFC_ReadPot(1);
-          TFC_SetServo(0,ReadPot0);
-          TFC_SetServo(1,ReadPot1);
-          TERMINAL_PRINTF("Pot0 = %1.2f\r\n", ReadPot0);
-      //    TERMINAL_PRINTF("Pot1 = %1.2f\r\n", ReadPot1);
-      }
-      //Let's put a pattern on the LEDs
-      if(TFC_Ticker[1] >= 125) // every 250mS... cycle through LEDs
-      {
-          TFC_Ticker[1] = 0;
-          t++;
-          if(t>4)
-          {
-              t=0;
-          }           
-          TFC_SetBatteryLED_Level(t);
-      }
-      
-      TFC_SetMotorPWM(0,0); //Make sure motors are off 
-      TFC_HBRIDGE_DISABLE;
-
-      break;
-      
-  case 2 :
+  case 1 : // Mode 1
       TFC_HBRIDGE_ENABLE;
      
       ReadPot0 = TFC_ReadPot(0);
       ReadPot1 = TFC_ReadPot(1);
- //     TERMINAL_PRINTF("Pot0 = %1.2f\n", ReadPot0);
- //     TERMINAL_PRINTF("Pot1 = %1.2f\n", ReadPot1);
+      TERMINAL_PRINTF("MODE 1: ");   
+      TERMINAL_PRINTF("Left drive setting = %1.2f\t\t", ReadPot1);
+      TERMINAL_PRINTF("Right drive setting = %1.2f\r\n", ReadPot0);
       TFC_SetMotorPWM(ReadPot0,ReadPot1);
               
-      //Let's put a pattern on the LEDs
-      if(TFC_Ticker[1] >= 125)
-          {
-              TFC_Ticker[1] = 0;
-                  t++;
-                  if(t>4)
-                  {
-                      t=0;
-                  }           
-              TFC_SetBatteryLED_Level(t);
-          }
       break;
   
-  case 3 :
-      //Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the 
-      //Labview Application
-      //note that there are some issues 
-      if(TFC_Ticker[0]>1000 && TFC_LineScanImageReady>0) // every 2s ...
+   case 2:                  
+      //Make sure motors are off 
+      TFC_SetMotorPWM(0,0);
+      TFC_HBRIDGE_DISABLE;
+
+      if(TFC_Ticker[0]>=20) // every 40mS output data to terminal
+      {
+          TFC_Ticker[0] = 0; //reset the Ticker
+          //update the Servos
+          TERMINAL_PRINTF("MODE 2: ");   
+          ReadPot0 = TFC_ReadPot(0);
+//          ReadPot1 = TFC_ReadPot(1);
+          TFC_SetServo(0,ReadPot0);
+//          TFC_SetServo(1,ReadPot1);
+          TERMINAL_PRINTF("Steer1 setting = %1.2f\r\n", ReadPot0);
+//          TERMINAL_PRINTF("Steer2 setting = %1.2f\r\n", ReadPot0);
+      }    
+      break;
+      
+   case 3 :
+      TFC_HBRIDGE_DISABLE;
+
+      if(TFC_Ticker[0]>1000 && TFC_LineScanImageReady>0) // every 1000 ticks (1s if 10ms)
           {
+           // TERMINAL_PRINTF("MODE 3:\r\n");
            TFC_Ticker[0] = 0;
            TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero
-          
-              if(t==0)
-                  t=4;
-              else
-                  t--;
-              
-               TFC_SetBatteryLED_Level(t);
+
+           if (terminalMode()) {
+             // print values to terminal as if were o-scope...
+             
+             for(j=20;j>0;j--) {
+               for(i=0;i<128;i++) {
+                 if ((TFC_LineScanImage0[i]<=(4096*j/20)) && (TFC_LineScanImage0[i]>=(4096*(j-1)/20)))
+                   TERMINAL_PRINTF("*");
+                 else
+                   TERMINAL_PRINTF(" ");
+               }
+               TERMINAL_PRINTF("\r\n");
+             }
+   
+           } else { 
+             for(i=0;i<128;i++) // print one line worth of data (128) from Camera 0
+             {
+               TERMINAL_PRINTF("%d",TFC_LineScanImage0[i]);
               
-               for(i=0;i<8;i++) // print one line worth of data (128) from Camera 0
-               {
-                  for(j=0;j<16;j++)
-                  {
-                       
-                       TERMINAL_PRINTF("0x%X",TFC_LineScanImage0[(i*16)+j]);
-                      
-                       if((i==7) && (j==15))  // when last data reached put in line return
-                           TERMINAL_PRINTF("\r\n",TFC_LineScanImage0[(i*16)+j]);
-                       else
-                           TERMINAL_PRINTF(",",TFC_LineScanImage0[(i*16)+j]);
-                      
-                  }
-                  wait_ms(10);
-               }
+               if(i==127)  // when last data reached put in line return
+                 TERMINAL_PRINTF("\r\n",TFC_LineScanImage0[i]);
+               else
+                 TERMINAL_PRINTF(",",TFC_LineScanImage0[i]);               
+             }
+             TERMINAL_PRINTF("============================================================================================\r\n");
+             wait_ms(10);
+           }
               
-     /*          for(i=0;i<8;i++) // print one line worth of data (128) from Camera 1 ??
-               {
-                    for(j=0;j<16;j++)
-                     {
-                       TERMINAL_PRINTF("0x%X",TFC_LineScanImage1[(i*16)+j]);
-                      
-                       if((i==7) && (j==15))  // when last data reached put in line return
-                           TERMINAL_PRINTF("\r\n",TFC_LineScanImage1[(i*16)+j]);
-                       else
-                           TERMINAL_PRINTF(",",TFC_LineScanImage1[(i*16)+j]);
-                      }   
-                      
-                      wait_ms(10);  
-              }      */                                 
+                               
                   
           }
           
@@ -180,18 +216,13 @@
     TFC_Init();
     
     for(;;)
-    {      
-        //TFC_Task must be called in your main loop.  This keeps certain processing happy (I.E. Serial port queue check)
-        //   TFC_Task();
-
-
-        // If DIP switch 1 is high, then run MCP, else Demo program
-        if(TFC_GetDIP_Switch()&0x01)
-          // Run MCP
-          MasterControlProgram();
+    {   
+        if(getMode() < 4)
+          // Run Garage Mode
+          GarageMode();
         else      
-          // Run Demo Program
-          DemoProgram();
+          // Run Track Mode
+          TrackMode();
  
     } // end of infinite for loop