TFC-Mentoring-Matters-Abstraction

Dependencies:   FRDM-TFC

Files at this revision

API Documentation at this revision

Comitter:
mrkeithcyr
Date:
Wed Jul 16 15:36:33 2014 +0000
Commit message:
First code commit.; Note known issue: in modes 1, 4, and 5 - The motor outputs are severely retarded by serial connection.

Changed in this revision

FRDM-TFC.lib Show annotated file Show diff for this revision Revisions of this file
RaceCar/Inits.cpp Show annotated file Show diff for this revision Revisions of this file
RaceCar/Inits.h Show annotated file Show diff for this revision Revisions of this file
RaceCar/Modes.cpp Show annotated file Show diff for this revision Revisions of this file
RaceCar/Modes.h Show annotated file Show diff for this revision Revisions of this file
RaceCar/TheDetails.cpp Show annotated file Show diff for this revision Revisions of this file
RaceCar/TheDetails.h Show annotated file Show diff for this revision Revisions of this file
common.cpp Show annotated file Show diff for this revision Revisions of this file
common.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM-TFC.lib	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/emh203/code/FRDM-TFC/#b34924a05d60
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RaceCar/Inits.cpp	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,42 @@
+#include "TFC.h"
+#include "common.h"
+
+/****** TFC_TickerUpdate() *****************************************************
+Purpose: This ticker code is used to maintain compatibility with the CodeWarrior
+    version of the sample. This code uses an MBED Ticker for background timing.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void TFC_TickerUpdate()
+{
+    int i;
+
+    for(i=0; i<NUM_TFC_TICKERS; i++)
+    {
+        if(TFC_Ticker[i]<0xFFFFFFFF) 
+        {
+            TFC_Ticker[i]++;
+        }
+    }
+}
+
+/****** WhatToTurnOn() *********************************************************
+Purpose: This function initializes the interfaces between the controller
+    board and the hardware.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void WhatToTurnOn()
+{
+    // set baud rate for serial output based on terminal type
+        // 115200 works with Excel, TeraTerm, and Putty
+        // 9600 works with USB Serial Monitor Lite
+            // USB Serial Monitor Lite does not work above 9600
+    PC.baud(115200);
+
+    // start counter for this hardware
+    TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
+
+    // initialize camera, servo, and motor systems
+    TFC_Init();
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RaceCar/Inits.h	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,8 @@
+/* Purpose: This ticker code is used to maintain compatibility with the
+    CodeWarrior version of the sample. This code uses an MBED Ticker for
+    background timing. */
+void TFC_TickerUpdate();
+
+/* This function initializes the interfaces between the controller board and
+    the hardware. */
+void WhatToTurnOn();
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RaceCar/Modes.cpp	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,412 @@
+#include "common.h"
+#include "TheDetails.h"
+
+/****** RunMode0() *************************************************************
+Purpose: Mode 0 is used for some very basic tests. It will test to see that the
+    car is "turned on" by printing a message to the computer telling us that it
+    is on. This mode also allows us to check that push buttons 'A' and 'B' are
+    working and that the LEDs on the board are working.
+    Most importantly, however, this quick and simple code will show us whether
+    or not our CODE is working before we spend too much time writing the rest of
+    the code.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode0()
+{
+    // print out a message to our computer so we know what mode we're in
+    TERMINAL_PRINTF("The car is currently turned on and in MODE 0\r\n");
+
+    // next we want to 
+    DISABLE_THE_MOTORS;
+
+    // this mode tests the push buttons and LEDs
+    if(BUTTON_A_IS_PRESSED)
+    { //then
+        TURN_ON_LED_0;  // and
+        TURN_ON_LED_1;
+    }
+    else
+    { // if button A is not pressed
+        TURN_OFF_LED_0;  // and
+        TURN_OFF_LED_1;
+    }
+    
+    if(BUTTON_B_IS_PRESSED)
+    { // then
+        TURN_ON_LED_2;  // and
+        TURN_ON_LED_3;
+    }
+    else
+    { // if button B is not pressed
+        TURN_OFF_LED_2;  // and
+        TURN_OFF_LED_3;
+    }
+}
+
+/****** RunMode1() *************************************************************
+Purpose: Mode 1 isolates the drive motors on the rear wheels. The potentiometers
+    on the board allow each motor to be adjusted individually. The numerical
+    value that the pot represents is output to our computer so that we can
+    gather some information about how the different values used for the drive
+    settings will affect the direction and/or speed of the wheels.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode1()
+{
+    // These variables will hold the values that the potentiometers on the board
+    // are set to.
+    float pot_1_value;
+    float pot_2_value;
+
+    // These variables will be the values that we send to the motors in order to
+    // control them and see how different values will cause the motors to 
+    // behave.
+    float right_drive;
+    float left_drive;
+
+    // Print out a message to our computer so we know what mode we're in
+    TERMINAL_PRINTF("The car is currently in MODE 1\r\n");
+
+    // Next we want to 
+    ENABLE_THE_MOTORS;
+    // because that's what Mode 1 tests.
+
+    // Then we check what the pots on the board are set to right now
+    pot_1_value = CHECK_POT_1_VALUE;
+    pot_2_value = CHECK_POT_2_VALUE;
+
+    // it turns out that we want to use these pot values as our drive settings
+    right_drive = pot_1_value;
+    left_drive = pot_2_value;
+
+    // We can print out to the computer what those values are so that we can
+    // actually see them as numbers.
+    TERMINAL_PRINTF("Left drive setting = %1.2f\t\t", left_drive);
+    TERMINAL_PRINTF("Right drive setting = %1.2f\r\n", right_drive);
+
+    // Let's give those drive settings to the motors and see what happens.
+    GiveDriveSettingsToMotors(right_drive, left_drive);
+}
+
+/****** RunMode2() *************************************************************
+Purpose: Mode 2 isolates the steering servo. We are able to use a potentiometer
+    on the board to adjust the steering settings left and right. The values
+    being given to the steering servo are printed out to the computer so that
+    they can be recorded and used later if necessary.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode2()
+{
+    // This variable will hold the value that potentiometer number 0 on the
+    // board is set to.
+    float steering_value;
+
+    // Next we want to 
+    DISABLE_THE_MOTORS;
+
+    // Use pot 1 to adjust the steering value.
+    steering_value = CHECK_POT_1_VALUE;
+
+    // Give the steering value to the steering servo.
+    GiveSteeringSettingTo(SERVO_NUMBER_1, steering_value);
+
+    // We only want the computer to give us information every 200mS so let's
+    // start a clock. Our clock counts every 2ms, so if it counts to 50 then
+    // 100 * 2ms = 200ms and the computer has some work to do.
+    if(CLOCK_NUMBER_1 >= 100)
+    {
+        // First we need to reset the clock so we get another 100ms break.
+        CLOCK_NUMBER_1 = 0;
+
+        // Print out a message to our computer so we know what mode we're in
+        TERMINAL_PRINTF("The car is currently in MODE 2 to test steering.\r\n");
+
+        // Let's display our steering setting on the computer to record it.
+        TERMINAL_PRINTF("The current steer setting is= %1.2f\r\n",
+                        steering_value);
+    }
+}
+
+/****** RunMode3() *************************************************************
+Purpose: This function runs the camera test mode for the car. It disables the
+    motors and steering servo to prevent damage to that hardware while isolating
+    the camera output. First we check to see if the camera has an image ready
+    for us to see and then we decode the camera image and translate it into a
+    picture that can be displayed on the computer screen.  The image on the
+    screen can be used to focus the camera lens. Dip switch 4 can be turned off 
+    to output the original camera code to the screen if it is needed to test the
+    camera in this mode.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode3()
+{
+    // We are going to use these variables to count a couple of things
+    uint32_t i = 0;
+    uint32_t j = 0;
+
+    // next we want to 
+    DISABLE_THE_MOTORS;
+    // since we are only working with the camera in this mode
+
+    // We only want the computer to give us information every 1s, and only if
+    // there is a picture ready. So let's start a clock. Our clock counts every
+    // 2ms, so if it counts to 500 then 500 * 2ms = 1s. Once a second has passed
+    // we'll check if the camera is ready. If so, then the computer has some
+    // work to do.
+    if(CLOCK_NUMBER_1 > 500 && CAMERA_READY > 0)
+    {
+        // First we need to reset the clock so we get another 1s break.
+        CLOCK_NUMBER_1 = 0;
+
+        // Since we are going to use the camera, we'll reset the camera to start
+        // taking the next image.
+        ResetCamera();
+
+        // Let's see which type of data we want to send to the computer.
+        if (LINE_IMAGE) // is what we want to see, then dip switch 4 must be ON
+        {
+            // These 'for loops' look at each little piece of the image that the
+            // camera sees and translates it for us. The pieces that the camera
+            // gives us are actually numbers that have a code of their own. By
+            // decoding the camera image we can decide if the piece should look
+            // like an asterisk or a blank space in the computer terminal. The
+            // result is grid printed out to the computer that is 20 rows by 128
+            // columns that tells us where the camera sees the line.
+            for(j = 20; j > 0; j--) // For each of 20 Rows...
+            {
+                for(i = 0; i < 128; i++) // ...do this for all 128 Columns.
+                {
+                    if (THE_PIECE_OF_IMAGE[i] <= LOOKS_IMPORTANT * j
+                        && (THE_PIECE_OF_IMAGE[i] >= LOOKS_IMPORTANT * (j - 1)))
+                    {   // then print an asterisk in this space
+                        TERMINAL_PRINTF("*");
+                    }   // otherwise print nothing in this space
+                    else
+                        TERMINAL_PRINTF(" ");
+                }
+
+                // At the end of the row we need to start printing out to
+                // the next row down on the computer screen.
+                TERMINAL_PRINTF("\r\n");
+            }
+        }
+
+        // if we don't want to translate the camera's coded numbers into a
+        // computer image, we can just print out the numbers themselves.
+        else    
+        {
+            // there are a lot of these numbers so we are only going to print
+            // one line worth of data (128 numbers) at a time.
+            for(i=0;i<128;i++)
+            {
+                TERMINAL_PRINTF("%d",THE_PIECE_OF_IMAGE[i]);
+                // when last data reached put in line return
+                if(i==127) 
+                    TERMINAL_PRINTF("\r\n",THE_PIECE_OF_IMAGE[i]);
+                else
+                    TERMINAL_PRINTF(",",THE_PIECE_OF_IMAGE[i]);
+            }
+            TERMINAL_PRINTF("=============================================="
+                        "==============================================\r\n");
+            
+            // We can pause the computer display here by entering how many
+            // milliseconds we want to wait.
+            wait_ms(10); // Waits for .1 second
+        }
+    }
+}
+
+/****** RunMode4() *************************************************************
+Purpose: Mode 4 is the first track mode. It takes all of the parts that we
+    tested in modes 0 thru 3 and puts them together to [hopefully] make our car
+    race around the track. Since we are still just testing the car, Mode 4 will
+    limit the top speed of the car just in case something doesn't work. We don't
+    want the car to crash into something too fast and break.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode4()
+{
+    // Let's set that slow speed right away.
+    GoSlow();
+
+    // We can still look at data some data on the computer in this mode by
+    // turning dip switch number 4 on, so let's check here if that switch is on.
+    bool if_we_want_data = DEPENDS_ON_DIP_SWITCH_4;
+
+    // Getting Data takes a little while, so if we want data we make mode 4 wait
+    // for 2 whole seconds to run each time, but if we don't want data, then
+    // mode 4 runs as fast as the camera can get each image to us! There is also
+    // a way here to create fake images that are always ready. They allow us to
+    // test the car operation based on a certain image even if we don't have the
+    // track to race on at the time.
+    if((CLOCK_NUMBER_1 > 2000 || !if_we_want_data)
+        && (CAMERA_READY || USE_FAKE_IMAGES))
+    {
+        // Just like in the other modes, we have to reset things each time we
+        // use them:
+        // Reset the clock to make sure we get long enough to use our data.
+        CLOCK_NUMBER_1 = 0;
+        // Reset the camera so that it can start working on the next image.
+        ResetCamera();
+        // This mode also looks at how bright the lights are on the track for
+        // each picture it takes. We have to reset the light intensity readings
+        // each time too, so we do that here.
+        ResetMaximumLightReading();
+        ResetMinimumLightReading();
+
+        // We also need the image that the camera told us it had ready. Let's go
+        // get it. If we are using fake images, this function will go get us the
+        // fake image.
+        GetTheImageToUse();
+
+        // Now that we have our image, let's see if the board can spot the line
+        // anywhere in the image. This will take a couple steps, first we have
+        // to get all the numbers that the camera uses to code its image
+        // (remember in mode 3?)
+        GetCameraData();
+        // Remember how we just reset the light readings, let's adjust them
+        // again for this new image before we try to find the line edges in the
+        // image.
+        adjustLights();
+        // We can now find out where the line is in the image.
+        FindTheLine();
+        //Let's look real close at that line and see exactly where the edges of
+        // the line are.
+        reviewEdges();
+
+        // We can print what we have so far on the computer it we have selected
+        // to print data.
+        if (if_we_want_data)
+        {
+            PrintOutSomeData();
+        }
+
+        // At this point we should know where the line is and what the car
+        // should do to stay on the track. We'll call that "Track Status."
+
+        // Let's update things based on latest Track Status
+        // This means we can adjust the steering and speed or even stop the car!
+        ActOnTrackStatus();
+
+        // Wouldn't it be nice if we could easily tell when the car was "seeing 
+        // the line" as it went around the track? Let's use those LEDs from
+        // mode 0 to show us:
+        // If the car sees the line, the middle 2 LEDs turn on
+        // If the car can't find the line then all the LEDs are off
+        feedbackLights();
+
+        // Let's use this function to control our speeds around corners and such
+        SpeedControl();
+
+        // Now that ALL OUR SETTINGS are ready to go, let's turn on the motors!
+        Drive();
+
+        // Some of the functions above will print more things out to the
+        // computer, so now that they are all complete let's print a line to
+        // tell ourselves that mode 4 has completed this time through.
+        if (if_we_want_data)
+        {
+            TERMINAL_PRINTF("\r\n**************************"
+                            "END********************************\r\n");
+        }
+    }
+}
+
+/****** RunMode5() *************************************************************
+Purpose: Mode 5 does exactly what mode 4 does, except that it does it FASTER.
+    This makes the code VERY easy to write for Mode 5
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode5()
+{
+    // Let's set the motors for race speed.
+    GoFast();
+
+    // We can still look at data some data on the computer in this mode by
+    // turning dip switch number 4 on, so let's check here if that switch is on.
+    bool if_we_want_data = DEPENDS_ON_DIP_SWITCH_4;
+
+    // Getting Data takes a little while, so if we want data we make mode 4 wait
+    // for 2 whole seconds to run each time, but if we don't want data, then
+    // mode 4 runs as fast as the camera can get each image to us! There is also
+    // a way here to create fake images that are always ready. They allow us to
+    // test the car operation based on a certain image even if we don't have the
+    // track to race on at the time.
+    if((CLOCK_NUMBER_1 > 2000 || !if_we_want_data)
+        && (CAMERA_READY || USE_FAKE_IMAGES))
+    {
+        // Just like in the other modes, we have to reset things each time we
+        // use them:
+        // Reset the clock to make sure we get long enough to use our data.
+        CLOCK_NUMBER_1 = 0;
+        // Reset the camera so that it can start working on the next image.
+        ResetCamera();
+        // This mode also looks at how bright the lights are on the track for
+        // each picture it takes. We have to reset the light intensity readings
+        // each time too, so we do that here.
+        ResetMaximumLightReading();
+        ResetMinimumLightReading();
+
+        // We also need the image that the camera told us it had ready. Let's go
+        // get it. If we are using fake images, this function will go get us the
+        // fake image.
+        GetTheImageToUse();
+
+        // Now that we have our image, let's see if the board can spot the line
+        // anywhere in the image. This will take a couple steps, first we have
+        // to get all the numbers that the camera uses to code its image
+        // (remember in mode 3?)
+        GetCameraData();
+        // Remember how we just reset the light readings, let's adjust them
+        // again for this new image before we try to find the line edges in the
+        // image.
+        adjustLights();
+        // We can now find out where the line is in the image.
+        FindTheLine();
+        //Let's look real close at that line and see exactly where the edges of
+        // the line are.
+        reviewEdges();
+
+        // We can print what we have so far on the computer it we have selected
+        // to print data.
+        if (if_we_want_data)
+        {
+            PrintOutSomeData();
+        }
+
+        // At this point we should know where the line is and what the car
+        // should do to stay on the track. We'll call that "Track Status."
+
+        // Let's update things based on latest Track Status
+        // This means we can adjust the steering and speed or even stop the car!
+        ActOnTrackStatus();
+
+        // Wouldn't it be nice if we could easily tell when the car was "seeing 
+        // the line" as it went around the track? Let's use those LEDs from
+        // mode 0 to show us:
+        // If the car sees the line, the middle 2 LEDs turn on
+        // If the car can't find the line then all the LEDs are off
+        feedbackLights();
+
+        // Let's use this function to control our speeds around corners and such
+        SpeedControl();
+
+        // Now that ALL OUR SETTINGS are ready to go, let's turn on the motors!
+        Drive();
+
+        // Some of the functions above will print more things out to the
+        // computer, so now that they are all complete let's print a line to
+        // tell ourselves that mode 4 has completed this time through.
+        if (if_we_want_data)
+        {
+            TERMINAL_PRINTF("\r\n**************************"
+                            "END********************************\r\n");
+        }
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RaceCar/Modes.h	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,66 @@
+/****** RunMode0() *************************************************************
+Purpose: Mode 0 is used for some very basic tests. It will test to see that the
+    car is "turned on" by printing a message to the computer telling us that it
+    is on. This mode also allows us to check that push buttons 'A' and 'B' are
+    working and that the LEDs on the board are working.
+    Most importantly, however, this quick and simple code will show us whether
+    or not our CODE is working before we spend too much time writing the rest of
+    the code.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode0();
+
+/****** RunMode1() *************************************************************
+Purpose: Mode 1 isolates the drive motors on the rear wheels. The potentiometers
+    on the board allow each motor to be adjusted individually. The numerical
+    value that the pot represents is output to our computer so that we can
+    gather some information about how the different values used for the drive
+    settings will affect the direction and/or speed of the wheels.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode1();
+
+/****** RunMode2() *************************************************************
+Purpose: Mode 2 isolates the steering servo. We are able to use a potentiometer
+    on the board to adjust the steering settings left and right. The values
+    being given to the steering servo are printed out to the computer so that
+    they can be recorded and used later if necessary.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode2();
+
+/****** RunMode3() *************************************************************
+Purpose: This function runs the camera test mode for the car. It disables the
+    motors and steering servo to prevent damage to that hardware while isolating
+    the camera output. First we check to see if the camera has an image ready
+    for us to see and then we decode the camera image and translate it into a
+    picture that can be displayed on the computer screen.  The image on the
+    screen can be used to focus the camera lens. Dip switch 4 can be turned off 
+    to output the original camera code to the screen if it is needed to test the
+    camera in this mode.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode3();
+
+/****** RunMode4() *************************************************************
+Purpose: Mode 4 is the first track mode. It takes all of the parts that we
+    tested in modes 0 thru 3 and puts them together to [hopefully] make our car
+    race around the track. Since we are still just testing the car, Mode 4 will
+    limit the top speed of the car just in case something doesn't work. We don't
+    want the car to crash into something too fast and break.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode4();
+
+/****** RunMode5() *************************************************************
+Purpose: Mode 5 does exactly what mode 4 does, except that it does it FASTER.
+    This makes the code VERY easy to write for Mode 5
+Parameters: None
+Returns: None
+*******************************************************************************/
+void RunMode5();
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RaceCar/TheDetails.cpp	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,1379 @@
+#include "TFC.h"
+#include "mbed.h"
+
+/****** INCLUDES ******/
+
+#include "mbed.h"
+#include "common.h"
+#include "TFC.h"
+
+/**********************/
+
+
+/****** CAMERA PARAMETERS ******/
+
+#define NUM_LINE_SCAN 128
+#define MAX_LINE_SCAN NUM_LINE_SCAN-1
+#define MIN_LINE_WIDTH  0
+#define MAX_LINE_WIDTH  15
+
+    // number of pixels at end of camera data to ignore
+    // set to 0 for now, later make 15
+#define FILTER_ENDS 15
+
+    // range of camera pixels to consider
+#define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS))
+
+    // ratio of max possible error to pixels (have to measure!)
+#define ERR_RATIO 0.85
+
+    // ratio for der threshold level (was 0.5 initially, may put back)
+#define DER_RATIO 0.5
+
+/*******************************/
+
+
+/****** STEERING AND SERVO PARAMETERS ******/
+
+    // value determined by garage mode 2 measure 
+    // must be adjusted for every servo horn attached
+    // must be re-adjusted (or at least checked) if 
+#define MAX_STEER_LEFT -0.38
+
+    // value determined by garage mode 2 measure
+#define MAX_STEER_RIGHT 0.49
+
+    // # MS of time between intervals (doesn't really matter)
+#define DT 0.02
+
+/*******************************************/
+
+
+/****** LOGGING PARAMETERS ******/
+
+    // number of frames to log (when logging is active) ~14 sec worth!
+#define NUM_LOG_FRAMES 700
+
+/********************************/
+
+
+/****** FOR DEBUG TUNING ******/
+
+#define TUNE_KP 0.008
+#define TUNE_KI 0
+#define TUNE_KD 0
+
+    // percent minimum power 
+        // estimating for a 2-ft turn => 24" / (24" + 6" car) = 4/5
+        // speed of inner wheel is 20% lower worst case
+#define MIN_POWER 60
+
+    // do not change
+#define SPEED_ADJUST 4
+
+    // number of pixels line position offset before changing KP value
+#define ABS_ERROR_THRESH 10
+
+    // which control method to use
+#define CONTROL_METHOD 1
+
+/******************************/
+
+
+/****** DRIVE/MOTOR PARAMETERS ******/
+
+    // maximum speed cap for slow speed setting
+#define LIGHT_SPEED 0.5
+
+    // maximum speed cap for faster speed setting
+#define LUDICROUS_SPEED 0.7
+
+    // percent max power (for speed adjustments)
+#define MAX_POWER 100
+
+/************************************/
+
+
+/****** ALGORITHM PARAMETERS ******/
+
+    // max value to allow for unknown track conditions before killing engine
+#define UNKNOWN_COUNT_MAX  50
+
+    // max value to allow for finding starting gate before killing engine
+#define STARTGATEFOUNDMAX  0
+
+    // delay before searching for starting gate to kill engine
+#define STARTGATEDELAY     50
+
+/**********************************/
+
+
+/****** IMAGE PROCESSING VARIABLES ******/
+
+    // snapshot of camera data for this 'frame'
+uint16_t   GrabLineScanImage0[NUM_LINE_SCAN];
+
+    // derivative of line scan data
+float      DerivLineScanImage0[NUM_LINE_SCAN];
+
+    // array-- set of where in line scan data negative edges found
+float      NegEdges[NUM_LINE_SCAN];
+
+    // array-- set of where in line scan data positive edges found
+float      PosEdges[NUM_LINE_SCAN];
+
+    // max value of valid neg and positive indices 
+        // also serves as a count of # edges found
+uint16_t   numNegEdges = 0, numPosEdges = 0;
+
+    // maximum measured light intensity -- to account for lighting differences
+uint16_t   MaxLightIntensity = 0;
+
+    // minimum measured light intensity -- to account for lighting differences
+uint16_t   MinLightIntensity = (1 << 12);
+
+    // maximum derivative value
+float      maxDerVal = 0;
+
+    // minimum derivative value
+float      minDerVal = (float) (1 << 12);
+
+    // average derivative value
+float      aveDerVal = 0;
+
+    // default derivative threshold
+float      DerivThreshold = (1 << 9);
+
+    // default positive edge derivative threshold
+float      PosDerivThreshold = (1 << 9);
+
+    // default negative edge derivative threshold
+float      NegDerivThreshold = (1 << 9);
+
+/****************************************/
+
+
+/****** STEERING CONTROL VARIABLES ******/
+
+    // current position of track line (in pixels -- 0 to 127)
+float      CurrentLinePosition;
+
+    // last position of track line (in pixels -- 0 to 127)
+float      LastLinePosition;
+
+    // current line position error (used for derivative calculation)
+float      CurrentLinePosError = 0;
+float      AbsError;
+
+    // last line position error (used for derivative calculation)
+float      LastLinePosError = 0;
+
+    // sum of line position error (used for integral calculation)
+float      SumLinePosError = 0;
+
+    // derivative of the error
+float      DerivError = 0;
+
+    // drive straight at first
+float      CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2;
+
+    // left wheel drive setting 
+float      CurrentLeftDriveSetting = 0;
+
+    // right wheel drive setting
+float      CurrentRightDriveSetting = 0;
+
+/****************************************/
+
+
+/****** SPEED CONTROL VARIABLES ******/
+
+    // maximum speed allowed
+float      MaxSpeed;
+
+    // ticker at start of race
+uint16_t   startRaceTicker;
+
+/*************************************/
+
+
+/****** CUSTOM DATA TYPES ******/
+
+typedef enum TrackStatusType {Unknown,
+                              LineFound,
+                              StartGateFound,
+                              LineJustLeft} TrackStatusType;
+                              
+TrackStatusType CurrentTrackStatus;                // current track status
+TrackStatusType LastTrackStatus;                   // last track status
+
+/* typedef enum TrackType {NotSure,
+                        Straight,
+                        Curve,
+                        Wiggle,
+                        Bumps,
+                        StartGate,
+                        UpHill,
+                        DownHill} TrackType;
+
+TrackType CurrentTrack; */
+
+struct LogData {
+  float linepos;
+  float steersetting;
+  float leftdrivesetting;
+  float rightdrivesetting;
+};
+
+    // array of log data to store
+LogData    frameLogs[NUM_LOG_FRAMES];
+
+    // index for log data
+int        logDataIndex;
+
+    // how many times start gate has been found
+int        StartGateFoundCount = 0;
+
+    // how many times nothing has been found
+        // help5 with kill switch implementation
+int        UnknownCount = 0;
+
+    // Car can go!  Should be set to false to start.
+bool       go = false;
+
+/*******************************/
+
+/****** EXTRA CONTROL PARAMETERS ******/
+
+    // if true, ignores real camera and uses fake camera input instead
+        // used for data processing debug
+extern bool debugFakeMode = false;
+
+    // whether output terminal data
+bool terminalOutput = false;
+
+    // whether to capture log data to output later on
+bool doLogData = false;
+
+    // whether to enable Kill Switch 
+        // allows engine to stop after not finding track if true
+bool killSwitch = false;
+
+    // whether to stop or not depending on starting gate reading
+bool startGateStop = false;
+
+    // race style -- whether conservative or risky
+bool doRisky = false;
+
+/**************************************/
+
+/****** terminalMode() *********************************************************
+Purpose: Checks the current setting of dip switch 4.
+Parameters: None
+Returns: True if dip switch 4 is set.
+*******************************************************************************/
+bool terminalMode()
+{
+    return ((TFC_GetDIP_Switch()>>3)&0x01 == 1);
+}
+
+/****** GiveDriveSettingsToMotors() ********************************************
+Purpose: Simply passes parameters to TFC_SetMotorPWM().
+Parameters: The desired settings for each of the two motors
+Returns: None
+*******************************************************************************/
+void GiveDriveSettingsToMotors(float right_drive_setting,
+                               float left_drive_setting)
+{
+    TFC_SetMotorPWM(right_drive_setting, left_drive_setting);
+}
+
+/****** GiveSteeringSettingTo() ************************************************
+Purpose: Simply passes parameters to TFC_SetServo().
+Parameters: The servo number and the steering setting.
+Returns: None
+*******************************************************************************/
+void GiveSteeringSettingTo(uint8_t ServoNumber, float Position)
+{
+    TFC_SetServo(ServoNumber, Position);
+}
+
+/****** ResetCamera() **********************************************************
+Purpose: This function resets the camera after an image has been used.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ResetCamera()
+{
+    TFC_LineScanImageReady = false;
+}
+
+/****** ResetMaximumLightReading() *********************************************
+Purpose: Resets the maximum light intensity variable
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ResetMaximumLightReading()
+{
+    MaxLightIntensity = 0;
+}
+
+/****** ResetMinimumLightReading() *********************************************
+Purpose: Resets the minimum light intensity variable
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ResetMinimumLightReading()
+{
+    MinLightIntensity = (1 << 12);
+}
+
+/****** grabCameraFrame() ******************************************************
+Purpose: This function gets a scan from the camera or fakes a scan from the
+    camera. There are different types of fake scans that can be used to test
+    other parts of the car to see how the car reacts to certain types of line
+    conditions.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void grabCameraFrame()
+{
+    uint32_t i = 0;
+    uint8_t fake_type = 4;                   // type of fake data if used
+    
+    for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data 
+                                 // (128 characters) from Camera 0
+    {
+        if (debugFakeMode)
+        {                   // use fake camera data
+            switch (fake_type)
+            {
+                // ideal track -- line in center
+                case 0:
+                    if (i<57 || i > 70)
+                        GrabLineScanImage0[i] = 0xFFF;  // no line
+                    else
+                        GrabLineScanImage0[i] = 0x4B0;  // line
+                    break;
+                // ideal track -- line to the left
+                case 1:
+                    if (i<27 || i > 40)
+                        GrabLineScanImage0[i] = 0xFFF;  // no line
+                    else
+                        GrabLineScanImage0[i] = 0x4B0;  // line
+                    break;
+                // ideal track -- line to the right
+                case 2:
+                    if (i<87 || i > 100)
+                        GrabLineScanImage0[i] = 0xFFF;  // no line
+                    else
+                        GrabLineScanImage0[i] = 0x4B0;  // line
+                    break;
+                // ideal track -- starting gate!
+                case 3:
+                    // TBD
+                    break;
+                // less than ideal track -- debug multi-edge issue!
+                case 4:
+                    if (i<54)
+                        GrabLineScanImage0[i] = 4000;   // no line
+                    if (i == 54)
+                        GrabLineScanImage0[i] = 3370;   // neg edge
+                    if (i == 55)
+                        GrabLineScanImage0[i] = 3309;   // neg edge
+                    if (i == 56)
+                        GrabLineScanImage0[i] = 2016;   // neg edge
+                    if (i == 57)
+                        GrabLineScanImage0[i] = 711;    // neg edge
+                    if (i == 58)
+                        GrabLineScanImage0[i] = 696;    // neg edge
+                    if ((i>58) && (i<69))
+                        GrabLineScanImage0[i] = 500;    // line
+                    if (i == 69)
+                        GrabLineScanImage0[i] = 1800;   // pos edge
+                    if (i > 69)
+                        GrabLineScanImage0[i] = 4000;   // no line
+                default:
+                    break;
+            }
+        } else  // use real camera data
+        {
+            GrabLineScanImage0[i] = TFC_LineScanImage0[i];
+        }
+    }
+}
+
+/****** GetTheImageToUse() *****************************************************
+Purpose: Retrieves whichever image we are supposed to use.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GetTheImageToUse()
+{
+    grabCameraFrame();
+}
+
+/****** derivativeLineScan() ***************************************************
+Purpose: Calculates derivative of line scan data.
+Parameters: Pointer to current image scan data, and a pointer to where you want
+    the derivative to be output.
+Returns: None
+*******************************************************************************/
+void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut)
+{
+    uint32_t i;
+    uint32_t minCnt = 0;
+    uint32_t maxCnt = 0;
+    float DerVal;
+    float upperDerVal;
+    float lowerDerVal = 0;
+
+    maxDerVal = 0;
+    minDerVal = (float) (1 << 12);
+    aveDerVal = 0;
+
+    minCnt = FILTER_ENDS;
+    maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
+
+    // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n");
+
+    // print one line worth of data from Camera 0
+    for(i=minCnt;i<maxCnt;i++)
+    {
+        // store max light intensity value
+        if (LineScanDataIn[i] > MaxLightIntensity)
+            MaxLightIntensity = LineScanDataIn[i];
+
+        // store min light intensity value
+        if (LineScanDataIn[i] < MinLightIntensity)
+            MinLightIntensity = LineScanDataIn[i];
+
+        // Central Derivative
+        // start point
+        if (i==minCnt)
+        {
+            upperDerVal = (float)(LineScanDataIn[i+1]);
+            // make same as start point
+            lowerDerVal = (float)(LineScanDataIn[i]);
+        }
+        // end point
+        else if (i==maxCnt - 1)
+        {
+            // make same as end point
+            upperDerVal = (float)(LineScanDataIn[i]);
+            lowerDerVal = (float)(LineScanDataIn[i-1]);
+        }
+        // any other point
+        else
+        {
+            upperDerVal = (float)(LineScanDataIn[i+1]);
+            lowerDerVal = (float)(LineScanDataIn[i-1]);
+        }
+        DerVal = (upperDerVal - lowerDerVal) / 2;
+        // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n",
+        //                 i, upperDerVal, lowerDerVal, DerVal);
+
+        if (DerVal > maxDerVal)
+        {
+            maxDerVal = DerVal;
+        }
+        if (DerVal < minDerVal)
+        {
+            minDerVal = DerVal;
+        }
+        //get sum
+        aveDerVal = aveDerVal + DerVal;
+        DerivLineScanDataOut[i] = DerVal;
+    }
+    aveDerVal = (float) aveDerVal / (maxCnt - minCnt);
+}
+
+/****** GetCameraData() *****************************************************
+Purpose: Processes an image to get the camera's raw data.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GetCameraData()
+{
+    derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]);
+}
+
+/****** printAdjustLightsData() ************************************************
+Purpose: Prints out light intensity data to the terminal.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void printAdjustLightsData()
+{
+    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);
+    }
+}
+
+/****** adjustLights() *********************************************************
+Purpose: Adjusts the line found threshold for different lighting conditions.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void adjustLights()
+{
+    // LIGHT ADJUST METHOD 1
+    // threshold is 1/5 of light intensity 'range'
+    if (1 == 0) {
+        DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5;
+        NegDerivThreshold = (float) -1 * (DerivThreshold);
+        PosDerivThreshold = (float) (DerivThreshold);
+    }
+
+    // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER
+    // pos edge threshold is half range of max derivative above ave derivative
+    // neg edge threshold is half range of min derivative above ave derivative
+    else
+    {
+        NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO;
+        PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO;
+    }
+    printAdjustLightsData();
+}
+
+/****** findEdges_v2() *********************************************************
+Purpose: This function is used to determine where the edges of the line are
+    found in the camera image. The location of the line edges is printed out to
+    the terminal if terminal output has been enabled.
+Parameters: Pointer to the array of line scan data derivatives
+Returns: None
+*******************************************************************************/
+void findEdges_v2(float* derivLineScanData)
+{
+    // search for edges in deriviative data using a threshold
+    // need to store in a hash if that's possible...
+    // combine edges that are a pixel apart
+    
+    int i;
+    
+    // serves as buffer to store neg edges found next to each other
+    int NegEdgeBufCnt = 0, NegEdgeBufSum = 0;
+    // serves as buffer to store pos edges found next to each other
+    int PosEdgeBufCnt = 0, PosEdgeBufSum = 0;
+        
+    int minCnt = FILTER_ENDS;
+    int maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
+    
+    
+    // count of neg edges found thus far
+    numNegEdges = 0;
+    // count of pos edges found thus far
+    numPosEdges = 0;
+    // print one line worth of data from Camera 0
+    for(i=minCnt;i<maxCnt;i++)
+    {
+        // NEGATIVE EDGE FOUND!
+        if (derivLineScanData[i] <= NegDerivThreshold)
+        {
+            if (terminalOutput)
+            {
+                TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX "
+                "%d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
+            }
+            // add value to neg edge buffer
+            NegEdgeBufCnt++;
+            NegEdgeBufSum = NegEdgeBufSum + i;
+        }
+        // POSITIVE EDGE FOUND!
+        else if(derivLineScanData[i] > PosDerivThreshold)
+        {
+            if (terminalOutput)
+            {
+                TERMINAL_PRINTF("POS EDGE FOUND AT INDEX "
+                "%d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
+            }
+
+            // add value to pos edge buffer
+            PosEdgeBufCnt++;                                      
+            PosEdgeBufSum = PosEdgeBufSum + i;
+
+        }
+
+        // NO EDGE FOUND
+        else
+        {
+            // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" 
+            // (i.e. edges found)
+            if (NegEdgeBufCnt > 0)
+            {
+                // store edge value
+                numNegEdges++;
+                NegEdges[numNegEdges - 1] =
+                    (float) NegEdgeBufSum / NegEdgeBufCnt;
+
+                // clear edge buffer      
+                NegEdgeBufSum = 0; NegEdgeBufCnt = 0;
+            }
+
+            if (PosEdgeBufCnt > 0)
+            {
+                // store edge value
+                numPosEdges++;
+                PosEdges[numPosEdges - 1] = 
+                    (float) PosEdgeBufSum / PosEdgeBufCnt;
+
+                // clear edge buffer
+                PosEdgeBufSum = 0; PosEdgeBufCnt = 0;
+            }
+        }
+    }
+}
+
+/****** FindTheLine() *********************************************************
+Purpose: Simply a link to whichever version of the edge-finding functions you
+    choose.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void FindTheLine()
+{
+    findEdges_v2(&DerivLineScanImage0[0]);
+}
+
+/****** reviewEdges() **********************************************************
+Purpose: This function check which edges were found and decides whether or not
+    the line has been found. Its results are printed out to the terminal if
+    terminal output has been enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void reviewEdges()
+{
+    LastTrackStatus = CurrentTrackStatus;
+
+    // only one negative and positive edge found (LINE)
+    if ((numPosEdges == 1) && (numNegEdges == 1))
+    {
+        // has proper expected width
+        if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH)
+            && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH))
+        {
+            // report line found!
+            CurrentTrackStatus = LineFound;
+            // reset unknown status count
+            UnknownCount = 0;
+            LastLinePosition = CurrentLinePosition;
+            // update line position
+            CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2;
+        }
+    }
+    
+    // 1 pos edge found (POSSIBLE LINE)
+    else if ((numPosEdges == 1) && (numNegEdges == 0))
+    {
+        // pos edge is within line width of edge of camera (LEFT)
+        if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0))
+        {
+            // report line found!
+            CurrentTrackStatus = LineFound;
+            // reset unknown status count
+            UnknownCount = 0;
+            LastLinePosition = CurrentLinePosition;
+            // update line position
+            CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2);
+            // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION "
+            //     "%9.3f *** \r\n", CurrentLinePosition);
+        }
+    }
+    
+    // 1 neg edge found (POSSIBLE LINE)
+    else if ((numNegEdges == 1) && (numPosEdges == 0))
+    {
+        // neg edge is within line width of edge of camera (RIGHT)
+        if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH))
+            && (LastLinePosError > 0))
+        {
+            // report line found!
+            CurrentTrackStatus = LineFound;
+            // reset unknown status count
+            UnknownCount = 0;
+            LastLinePosition = CurrentLinePosition;
+            // update line position
+            CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2);
+            // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION "
+            //     "%9.3f *** \r\n", CurrentLinePosition);
+        } 
+    }
+
+    // 2 negative and 2 positive edges found (STARTING/FINISH GATE)
+    else if ((numPosEdges == 2) && (numNegEdges == 2))
+    {
+        if ( // white left 'line'
+            (((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) 
+            && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH))
+            // white right 'line'
+            && (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH)
+            && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH))
+            // actual track line
+            && (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH)
+            && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)))
+        {
+            // only start counting for starting gate until after delay
+            if (startRaceTicker > STARTGATEDELAY)
+            {
+                StartGateFoundCount++;
+            }
+        }
+        CurrentTrackStatus = StartGateFound;
+        // reset unknown status count
+        UnknownCount = 0;
+    }
+
+    // more than 1 negative edge and positive edge found 
+    // (but not 2 for both) (STARTING / FINISH GATE)
+    else if ((numPosEdges > 1) && (numNegEdges > 1))
+    {
+
+    // remove edges that aren't close to center TBD DDHH
+    if (terminalOutput)
+    {
+        TERMINAL_PRINTF("***************************************** \r\n");
+        TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
+        TERMINAL_PRINTF("***************************************** \r\n");
+    }
+    CurrentTrackStatus = Unknown; 
+        
+    }
+
+    // no track or starting gate found
+    else
+    {
+        if (terminalOutput) {
+            TERMINAL_PRINTF("***************************************** \r\n");
+            TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n",
+                CurrentLinePosition);
+            TERMINAL_PRINTF("***************************************** \r\n");
+        }
+
+        CurrentTrackStatus = Unknown;
+        UnknownCount++;
+    }
+}
+
+/****** printLineScanData() ****************************************************
+Purpose: This function prints out the camera data to the terminal for use in
+    testing and debugging.
+Parameters: Takes the most recent scan returned by the camera.
+Returns: None
+*******************************************************************************/
+void printLineScanData(uint16_t* LineScanData)
+{
+    uint32_t i = 0;
+    float Val;
+
+    TERMINAL_PRINTF("LINE SCAN DATA:,");
+
+    // print one line worth of data (128) from Camera 0
+    for(i=0;i<NUM_LINE_SCAN;i++)
+    { 
+        if (1 == 1) { // use float to print
+            Val = (float) LineScanData[i];
+            TERMINAL_PRINTF("%9.3f",Val);
+            if(i==MAX_LINE_SCAN)  // when last data reached put in line return
+                TERMINAL_PRINTF("\r\n");
+            else
+                TERMINAL_PRINTF(",");
+        } else
+        {
+            TERMINAL_PRINTF("0x%X",LineScanData[i]);
+            if(i==MAX_LINE_SCAN)  // when last data reached put in line return
+                TERMINAL_PRINTF("\r\n",LineScanData[i]);
+            else
+                TERMINAL_PRINTF(",",LineScanData[i]);
+        }
+    }
+}
+
+/****** SteeringControl() ******************************************************
+Purpose: This function decides how much to turn the front wheels to keep the car
+    on course.
+    The CurrentSteerSetting variable is set here.
+    Its results are printed out to the terminal if terminal output has been
+    enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void SteeringControl()
+{
+    float ReadPot1 = 1;
+    
+    // target to achieve for line position
+    float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5);
+    
+    float KP;                         // proportional control factor
+    float KI;                         // integral control factor
+    float KD;                         // derivative control factor
+    
+    // PID terms
+    float Pout;
+    float Iout;
+    float Dout;
+    
+    // Calculate error
+    // make error to the right positive
+    // i.e. if LINE to the right-- then CurrentLinePosError > 0
+    //      if LINE to the left -- then CurrentLinePosError < 0
+    CurrentLinePosError = CurrentLinePosition - targetPosition;
+    
+    // Get absolute error
+    if (CurrentLinePosError >= 0) 
+        AbsError = CurrentLinePosError;
+    else
+        AbsError = -1 * CurrentLinePosError;
+    
+    // CHOOSE SET OF PID CONTROL PARAMETERS
+    switch (CONTROL_METHOD)
+    {
+        // Pure proportional control based on 
+        // range of steering values vs. range of error values
+        case 0:
+            KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT )
+                / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
+            KD = 0;
+            KI = 0;
+            break;
+        // Proportional control but 50% more aggressive around the bends
+        case 1:
+            ReadPot1 = TFC_ReadPot(1)+1; // pot range 0-2
+            KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT )
+                / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
+            KP = KP * ReadPot1;
+            KD = 0;
+            KI = 0;
+            break;
+        // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters)
+        case 2:
+            KP = TUNE_KP;
+            KI = TUNE_KI;
+            KD = TUNE_KD;
+        case 3:
+            if (AbsError < ABS_ERROR_THRESH)
+            {
+                KP = 0.003;  // when relatively straight, keep KP gain low
+            }
+            else
+            {
+                KP = 0.010;  // when curve begins or off track, increase KP gain
+            }
+            KI = 0;
+            KD = 0;
+        default:
+            break;
+    }
+
+    /* Pseudocode
+     previous_error = 0
+     integral = 0 
+     start:
+       error = setpoint - measured_value
+       integral = integral + error*dt
+       derivative = (error - previous_error)/dt
+       output = Kp*error + Ki*integral + Kd*derivative
+       previous_error = error
+       wait(dt)
+       goto start 
+    */
+
+    if (terminalOutput)
+    {
+        TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
+        TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition);
+    }
+
+    // Update integral of error
+    // i.e. if LINE stays to the right, then SumLinePosError increases
+    // i.e. if LINE stays to the left, then SumLinePosError decreases
+    SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT );
+    
+    DerivError = (CurrentLinePosError - LastLinePosError) / DT;
+    
+    if (terminalOutput) {
+        TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n",
+            CurrentLinePosition);
+        TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n",
+            CurrentLinePosError);
+    }
+    
+    // SECOND- calculate new servo position
+    
+    // proportional control term
+    Pout = KP * CurrentLinePosError;
+    
+    // integral control term
+    Iout = KI * SumLinePosError;
+    
+    // Derivative control term
+    Dout = KD * DerivError;
+    
+    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);
+        TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout);
+        TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout);
+        TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout);
+    }
+    
+    // add offset to steering to account for non-centered servo mounting
+    // CurrentSteerSetting = Pout + Iout + Dout
+    //     + ((float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2);
+    CurrentSteerSetting = Pout
+        + ((float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
+    
+    // store for next cycle deriv calculation
+    LastLinePosError = CurrentLinePosError;
+    
+    // for tuning control algo only
+    if (1 == 0)
+    {
+        TERMINAL_PRINTF("*** ******************************** \r\n");
+        TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n",
+            CurrentLinePosition);
+        TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError);
+        TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n",
+            SumLinePosError);
+        TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError);
+        TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n",
+            CurrentSteerSetting);
+        TERMINAL_PRINTF("*** PI STEER SETTING  %9.3f *** \r\n",
+            (CurrentSteerSetting + Iout));
+        TERMINAL_PRINTF("*** ******************************** \r\n");
+        wait_ms(1000);
+    }
+}
+
+/****** Steer() ****************************************************************
+Purpose: Sets the steering servo output to the value necessary for the current
+    steering needs. It also checks the value needed to make sure the board isn't
+    trying to steer harder than the wheels can turn.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void Steer()
+{
+    // make sure doesn't go beyond steering limits
+    if (CurrentSteerSetting > MAX_STEER_RIGHT)
+    {
+        CurrentSteerSetting = MAX_STEER_RIGHT;
+    }
+    else if(CurrentSteerSetting < MAX_STEER_LEFT)
+    {
+        CurrentSteerSetting = MAX_STEER_LEFT;
+    }
+    if(terminalOutput)
+    {
+        TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n",
+            CurrentSteerSetting);
+    }
+    TFC_SetServo(0,CurrentSteerSetting);  
+}
+
+/****** printDerivLineScanData() ***********************************************
+Purpose: This function prints out the line scan derivative data to the terminal
+    for use in testing and debugging.
+Parameters: Takes the array of line scan data derivatives.
+Returns: None
+*******************************************************************************/
+void printDerivLineScanData(float* derivLineScanData)
+{
+    uint32_t i;
+    uint32_t minCnt = 0;
+    uint32_t maxCnt = 0;
+
+    minCnt = FILTER_ENDS;
+    maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
+
+    TERMINAL_PRINTF("DERIVATIVE DATA:,");
+
+    // print one line worth of data (128) from Camera 0
+    for(i=minCnt;i<maxCnt;i++)
+    {
+        TERMINAL_PRINTF("%9.3f",derivLineScanData[i]);
+        if(i==maxCnt-1)          // when last data reached put in line return
+            TERMINAL_PRINTF("\r\n",derivLineScanData[i]);
+        else
+            TERMINAL_PRINTF(", ",derivLineScanData[i]);
+    }
+}
+
+/****** printEdgesFound() ******************************************************
+Purpose: This function prints the line edge data to the terminal for line use in
+    debugging.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void printEdgesFound()
+{
+    int i;
+
+    // Check that neg edges captured ok
+    TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,");
+    for(i=0;i<=numNegEdges-1;i++)
+    {
+        TERMINAL_PRINTF("%9.3f",NegEdges[i]);
+        // when last data reached put in line return
+        if(i==numNegEdges-1)
+            TERMINAL_PRINTF("\r\n");
+        else
+            TERMINAL_PRINTF(", ");
+    }
+
+    // Check that pos edges captured ok
+    TERMINAL_PRINTF("POSITIVE EDGES FOUND:,");
+    for(i=0;i<=numPosEdges-1;i++)
+    {
+        TERMINAL_PRINTF("%9.3f",PosEdges[i]);
+        // when last data reached put in line return
+        if(i==numPosEdges-1)
+            TERMINAL_PRINTF("\r\n");
+        else
+            TERMINAL_PRINTF(", ");
+    }
+}
+
+/****** PrintOutSomeData() *****************************************************
+Purpose: Prints out a selection of Track mode data after image processing.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void PrintOutSomeData()
+{
+    printLineScanData(&GrabLineScanImage0[0]);
+    printDerivLineScanData(&DerivLineScanImage0[0]);
+    printAdjustLightsData();
+    printEdgesFound();
+}
+
+/****** ActOnTrackStatus() *****************************************************
+Purpose: This function decides what to do next based on the current track
+    status. Its results are printed out to the terminal if terminal output has
+    been enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ActOnTrackStatus()
+{
+    // Decide what to do next based on current track status
+
+    // LINE FOUND!
+    if (CurrentTrackStatus == LineFound)
+    {
+        if (terminalOutput) {
+            TERMINAL_PRINTF("***************************************** \r\n");
+            TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n",
+                CurrentLinePosition);
+            TERMINAL_PRINTF("***************************************** \r\n");
+        }
+
+        // Update steering position 
+        SteeringControl();
+
+        // Apply to servo    
+        Steer();
+    } 
+
+    // STARTING GATE FOUND
+    else if (CurrentTrackStatus == StartGateFound)
+    {
+        if (terminalOutput)
+        {
+            TERMINAL_PRINTF("***************************************** \r\n");
+            TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n");
+            TERMINAL_PRINTF("**********     count = %d      ********** \r\n",
+                StartGateFoundCount);
+            TERMINAL_PRINTF("***************************************** \r\n");
+        }
+
+        // END RACE!
+        if (startGateStop)
+        {
+            if (StartGateFoundCount > STARTGATEFOUNDMAX)
+            {
+               go = false;   // STOP!!
+            } 
+        }
+    }
+}
+
+/****** feedbackLights() *******************************************************
+Purpose: Turns on board LED's to indicate specific status for debugging
+    purposes.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void feedbackLights()
+{
+    switch (CurrentTrackStatus)
+    {
+        case LineFound:
+            TFC_BAT_LED0_OFF;
+            TFC_BAT_LED1_ON;
+            TFC_BAT_LED2_ON;
+            TFC_BAT_LED3_OFF;
+            break;
+        case StartGateFound:
+            TFC_BAT_LED0_ON;
+            TFC_BAT_LED1_OFF;
+            TFC_BAT_LED2_OFF;
+            TFC_BAT_LED3_ON;
+            break;
+        default:
+            TFC_BAT_LED0_OFF;
+            TFC_BAT_LED1_OFF;
+            TFC_BAT_LED2_OFF;
+            TFC_BAT_LED3_OFF;
+    }
+}
+
+/****** SpeedControl() *********************************************************
+Purpose: This function determines the speed settings in the code and set on the
+    board itself. It then offers multiple ways to apply those settings to the
+    actual speed output to the wheels.
+    Its results are printed out to the terminal if terminal output has been
+    enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void SpeedControl()
+{
+    // Get max speed setting from reading pot1 and then adjust
+    float ErrLimit;
+    float LeftDriveRatio;
+    float RightDriveRatio;
+
+    // set maximum speed
+    if (doRisky)
+        MaxSpeed = LUDICROUS_SPEED * ((TFC_ReadPot(0)+1)/2.0); // faster
+    else
+        MaxSpeed = LIGHT_SPEED * ((TFC_ReadPot(0)+1)/2.0);     // slower
+
+    switch (SPEED_ADJUST)
+    {
+        // SPEED ADJUST METHOD 0
+        // no speed adjust
+        case 0:
+            LeftDriveRatio = MAX_POWER;
+            RightDriveRatio = LeftDriveRatio;
+
+        // SPEED ADJUST METHOD 1
+        // High speed when error is low, low speed when error is high
+        // lower speed when more than third outside of center
+        case 1:
+            ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33;
+            if (AbsError > ErrLimit) {
+              LeftDriveRatio = MIN_POWER;
+            } else {
+              LeftDriveRatio = MAX_POWER;
+            }
+            RightDriveRatio = LeftDriveRatio;
+            break;
+
+        // SPEED ADJUST METHOD 2
+        // max/min speed adjusts proportional to absolute value of line error
+        case 2:
+            ErrLimit = ((float) RANGE )  * 0.5 * ERR_RATIO; 
+            LeftDriveRatio =
+                MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit));
+            RightDriveRatio = LeftDriveRatio;
+            break;
+
+        // SPEED ADJUST METHOD 3
+        // wheel relative speed proportional to absolute value of line error
+        case 3:
+            // heading right, slow right wheel down
+            if (CurrentLinePosError > 0)
+            {
+                LeftDriveRatio = MAX_POWER;
+                RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER)
+                    * (CurrentLinePosError * 2 / ( (float) RANGE ) );
+            }
+            // heading left, slow left wheel down
+            else if (CurrentLinePosError < 0)
+            {
+                // note sign change due to error being negative
+                LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER)
+                    * (CurrentLinePosError * 2 / ( (float) RANGE ) );
+                RightDriveRatio = MAX_POWER;
+            }
+            else
+            {
+                LeftDriveRatio = MAX_POWER;
+                RightDriveRatio = MAX_POWER;
+            }
+            break;
+        case 4:
+            // SPEED ADJUST METHOD 4
+            // 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;
+
+            // right turn-- slow right wheel down a bit
+            if (CurrentLinePosError > ErrLimit)
+            {
+              LeftDriveRatio = MAX_POWER;
+              RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER)
+                * (CurrentLinePosError * 2 / ( (float) RANGE ) );
+            }
+
+            // left turn-- slow left wheel down a bit
+            else if(CurrentLinePosError < (-1 * ErrLimit))
+            {
+                // note sign change due to error being negative
+                LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER)
+                    * (CurrentLinePosError * 2 / ( (float) RANGE ) );
+                RightDriveRatio = MAX_POWER;
+            }
+
+            // when in center drive full speed
+            else
+            {
+              LeftDriveRatio = MAX_POWER;
+              RightDriveRatio = MAX_POWER;
+            }
+            break;
+
+        // SPEED ADJUST METHOD 5
+        // High speed when error is low, low speed when error is high
+        // lower speed when more than third outside of center
+        case 5:
+            ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2;
+            if (AbsError > ErrLimit)
+            {
+                LeftDriveRatio = MIN_POWER;
+            }
+            
+            else
+            {
+                LeftDriveRatio = MAX_POWER;
+            }
+            
+            RightDriveRatio = LeftDriveRatio;
+            break;   
+        
+        // SPEED ADJUST METHOD 6
+        // High speed when error is low, low speed when error is high
+        // lower speed when more than third outside of center
+        case 6:
+            if (AbsError > ABS_ERROR_THRESH)
+            {
+                LeftDriveRatio = MIN_POWER;
+            }
+            
+            else
+            {
+              LeftDriveRatio = MAX_POWER;
+            }
+            
+            RightDriveRatio = LeftDriveRatio;
+            break;                 
+        
+        default:
+            break;
+    }
+    // TBD-- add speed adjust based on Xaccel sensor!
+
+    // currently no control mechanism as don't have speed sensor  
+    CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed;
+    CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed;
+
+    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\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);
+    }
+}
+
+/****** Drive() ****************************************************************
+Purpose: This is the function that enables the motors to run.  It monitors the
+    pressing of buttons A and B to start and stop the car. It also monitors for
+    emergency stop conditions and will turn off the motors to stop the car if
+    necessary.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void Drive()
+{
+    // START!
+    // if not going, go when button A is pressed
+    if (!go)
+    {
+        if(TFC_PUSH_BUTTON_0_PRESSED)
+        {
+            go = true;
+            UnknownCount = 0;
+            StartGateFoundCount = 0;
+            startRaceTicker = TFC_Ticker[0];  // keep track of start of race
+            logDataIndex = 0;                 // reset log data index
+        }
+    }
+
+    // STOP!
+    // if going, stop when button B is pressed
+    if (go) {              
+     if(TFC_PUSH_BUTTON_1_PRESSED) {
+        go = false;
+        StartGateFoundCount = 0;
+      }
+    }
+
+    // EMERGENCY STOP!
+    // 'kill switch' to prevent crashes off-track
+    if (killSwitch)
+    {
+        // if track not found after certain time
+        if (UnknownCount > UNKNOWN_COUNT_MAX)
+        {
+            // kill engine
+            go = false;
+            StartGateFoundCount = 0;
+        }
+    }
+
+    // stop!
+    if (!go)
+    {
+        // make sure motors are off 
+        TFC_SetMotorPWM(0,0);
+        TFC_HBRIDGE_DISABLE;
+    }
+
+    // go!
+    if (go)
+    {
+        TFC_HBRIDGE_ENABLE;
+        // motor A = right, motor B = left based on way it is mounted
+        TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting);
+    }
+}
+
+/****** GoSlow() ***************************************************************
+Purpose: Sets the doRisky variable to false
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GoSlow()
+{
+    doRisky = false;
+}
+
+/****** GoFast() ***************************************************************
+Purpose: Sets the doRisky variable to true
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GoFast()
+{
+    doRisky = true;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RaceCar/TheDetails.h	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,256 @@
+#include "mbed.h"
+#include "TFC.h"
+
+/****** MENTORING MATTERS LAYER DEFINITIONS ******/
+
+#define DIP_SWITCH_SETTING          (TFC_GetDIP_Switch()&0x07)
+#define CHECK_POT_1_VALUE           TFC_ReadPot(0);
+#define CHECK_POT_2_VALUE           TFC_ReadPot(1);
+
+#define DISABLE_THE_MOTORS          TFC_HBRIDGE_DISABLE
+#define ENABLE_THE_MOTORS           TFC_HBRIDGE_ENABLE
+
+#define TURN_ON_LED_0               TFC_BAT_LED0_ON
+#define TURN_ON_LED_1               TFC_BAT_LED1_ON
+#define TURN_ON_LED_2               TFC_BAT_LED2_ON
+#define TURN_ON_LED_3               TFC_BAT_LED3_ON
+
+#define TURN_OFF_LED_0              TFC_BAT_LED0_OFF
+#define TURN_OFF_LED_1              TFC_BAT_LED1_OFF
+#define TURN_OFF_LED_2              TFC_BAT_LED2_OFF
+#define TURN_OFF_LED_3              TFC_BAT_LED3_OFF
+
+#define BUTTON_A_IS_PRESSED         TFC_PUSH_BUTTON_0_PRESSED
+#define BUTTON_B_IS_PRESSED         TFC_PUSH_BUTTON_1_PRESSED
+
+#define CLOCK_NUMBER_1              TFC_Ticker[0]
+
+#define SERVO_NUMBER_1              0
+
+#define CAMERA_READY                TFC_LineScanImageReady
+#define LINE_IMAGE                  terminalMode()
+#define DEPENDS_ON_DIP_SWITCH_4     terminalMode()
+#define THE_PIECE_OF_IMAGE          TFC_LineScanImage0
+#define LOOKS_IMPORTANT             (4096 / 20)
+#define USE_FAKE_IMAGES             debugFakeMode
+
+/*************************************************/
+
+extern bool debugFakeMode;
+
+/****** terminalMode() *********************************************************
+Purpose: Checks the current setting of dip switch 4.
+Parameters: None
+Returns: True if dip switch 4 is set.
+*******************************************************************************/
+bool terminalMode();
+
+/****** GiveDriveSettingsToMotors() ********************************************
+Purpose: Simply passes parameters to TFC_SetMotorPWM().
+Parameters: The desired settings for each of the two motors
+Returns: None
+*******************************************************************************/
+void GiveDriveSettingsToMotors(float right_drive_setting,
+                               float left_drive_setting);
+
+/****** GiveSteeringSettingTo() ************************************************
+Purpose: Simply passes parameters to TFC_SetServo().
+Parameters: The servo number and the steering setting.
+Returns: None
+*******************************************************************************/
+void GiveSteeringSettingTo(uint8_t ServoNumber, float Position);
+
+/****** ResetCamera() **********************************************************
+Purpose: This function resets the camera after an image has been used.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ResetCamera();
+
+/****** ResetMaximumLightReading() *********************************************
+Purpose: Resets the maximum light intensity variable
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ResetMaximumLightReading();
+
+/****** ResetMinimumLightReading() *********************************************
+Purpose: Resets the minimum light intensity variable
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ResetMinimumLightReading();
+
+/****** grabCameraFrame() ******************************************************
+Purpose: This function gets a scan from the camera or fakes a scan from the
+    camera. There are different types of fake scans that can be used to test
+    other parts of the car to see how the car reacts to certain types of line
+    conditions.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void grabCameraFrame();
+
+/****** GetTheImageToUse() *****************************************************
+Purpose: Retrieves whichever image we are supposed to use.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GetTheImageToUse();
+
+/****** GetCameraData() *****************************************************
+Purpose: Processes an image to get the camera's raw data.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GetCameraData();
+
+/****** derivativeLineScan() ***************************************************
+Purpose: Calculates derivative of line scan data.
+Parameters: Pointer to current image scan data, and a pointer to where you want
+    the derivative to be output.
+Returns: None
+*******************************************************************************/
+void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut);
+
+/****** printAdjustLightsData() ************************************************
+Purpose: Prints out light intensity data to the terminal.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void printAdjustLightsData();
+
+/****** adjustLights() *********************************************************
+Purpose: Adjusts the line found threshold for different lighting conditions.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void adjustLights();
+
+/****** FindTheLine() *********************************************************
+Purpose: Simply a link to whichever version of the edge-finding functions you
+    choose.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void FindTheLine();
+
+/****** findEdges_v2() *********************************************************
+Purpose: This function is used to determine where the edges of the line are
+    found in the camera image. The location of the line edges is printed out to
+    the terminal if terminal output has been enabled.
+Parameters: Pointer to the array of line scan data derivatives
+Returns: None
+*******************************************************************************/
+void findEdges_v2(float* derivLineScanData);
+
+/****** reviewEdges() **********************************************************
+Purpose: This function check which edges were found and decides whether or not
+    the line has been found. Its results are printed out to the terminal if
+    terminal output has been enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void reviewEdges();
+
+/****** PrintOutSomeData() *****************************************************
+Purpose: Prints out a selection of Track mode data after image processing.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void PrintOutSomeData();
+
+/****** ActOnTrackStatus() *****************************************************
+Purpose: This function decides what to do next based on the current track
+    status. Its results are printed out to the terminal if terminal output has
+    been enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void ActOnTrackStatus();
+
+/****** feedbackLights() *******************************************************
+Purpose: Turns on board LED's to indicate specific status for debugging
+    purposes.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void feedbackLights();
+
+/****** SpeedControl() *********************************************************
+Purpose: This function determines the speed settings in the code and set on the
+    board itself. It then offers multiple ways to apply those settings to the
+    actual speed output to the wheels.
+    Its results are printed out to the terminal if terminal output has been
+    enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void SpeedControl();
+
+/****** Drive() ****************************************************************
+Purpose: This is the function that enables the motors to run.  It monitors the
+    pressing of buttons A and B to start and stop the car. It also monitors for
+    emergency stop conditions and will turn off the motors to stop the car if
+    necessary.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void Drive();
+/****** printLineScanData() ****************************************************
+Purpose: This function prints out the camera data to the terminal for use in
+    testing and debugging.
+Parameters: Takes the most recent scan returned by the camera.
+Returns: None
+*******************************************************************************/
+void printLineScanData(uint16_t* LineScanData);
+
+/****** printDerivLineScanData() ***********************************************
+Purpose: This function prints out the line scan derivative data to the terminal
+    for use in testing and debugging.
+Parameters: Takes the array of line scan data derivatives.
+Returns: None
+*******************************************************************************/
+void printDerivLineScanData(float* derivLineScanData);
+
+/****** printEdgesFound() ******************************************************
+Purpose: This function prints the line edge data to the terminal for line use in
+    debugging.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void printEdgesFound();
+
+/****** SteeringControl() ******************************************************
+Purpose: This function decides how much to turn the front wheels to keep the car
+    on course.
+    The CurrentSteerSetting variable is set here.
+    Its results are printed out to the terminal if terminal output has been
+    enabled.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void SteeringControl();
+
+/****** Steer() ****************************************************************
+Purpose: Sets the steering servo output to the value necessary for the current
+    steering needs. It also checks the value needed to make sure the board isn't
+    trying to steer harder than the wheels can turn.
+Parameters: None
+Returns: None
+*******************************************************************************/
+void Steer();
+
+/****** GoSlow() ***************************************************************
+Purpose: Sets the DoRisky variable to false
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GoSlow();
+
+/****** GoFast() ***************************************************************
+Purpose: Sets the doRisky variable to true
+Parameters: None
+Returns: None
+*******************************************************************************/
+void GoFast();
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common.cpp	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,9 @@
+#include "mbed.h"
+
+#include "common.h"
+
+Serial PC(USBTX,USBRX); 
+
+Ticker TFC_TickerObj;
+
+volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,22 @@
+#include "mbed.h"
+
+#ifndef _COMMON_H
+#define _COMMON_H
+
+//This macro is to maintain compatibility with Codewarrior version of the sample.   This version uses the MBED libraries for serial port access
+extern Serial PC;
+
+#define TERMINAL_PRINTF     PC.printf
+
+ 
+ //This ticker code is used to maintain compability with the Codewarrior version of the sample.   This code uses an MBED Ticker for background timing.
+ 
+#define NUM_TFC_TICKERS 4
+
+extern Ticker TFC_TickerObj;
+ 
+
+extern volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
+
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 16 15:36:33 2014 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"           // stuff for the program we used to write the code
+#include "TFC.h"            // stuff for the board we're using on the car
+#include "Inits.h"          // where to find the initialization directions
+#include "TheDetails.h"     // this layer translates the lower level code to the
+                            // Freescale Mentoring Matters level of abstraction
+#include "Modes.h"          // where to find our instructions for each mode
+
+/****** main() *****************************************************************
+Purpose: This is the software engine that runs the race car. It is a set of
+    commands that run from the beginning to the end and then start over each
+    time for as long as the car is turned on. This is called an 'infinite loop'.
+Parameters: None
+Returns: The main function always returns an 'int'.
+*******************************************************************************/
+int main()
+{
+    // 'mode' will always equal a number to indicate the mode the code is using
+    uint16_t mode;
+
+    // First, we have to turn on the car and all of its motors and parts.
+    WhatToTurnOn();
+
+    // This is the infinite loop for driving the race car starts here and runs
+    // forever.
+    for(;;)
+    {
+        // The loop determines what mode the car is in by reading the dip
+        // switch number every time the loop starts over
+        mode = DIP_SWITCH_SETTING;
+
+        // Next, the loop runs the car in whichever mode the dip switches are
+        // set for by 'switching' to that mode below. It only picks one mode
+        // each time, runs that mode only once, and then starts the loop over
+        // again by checking to see if the dip switches have been changed.
+
+        // This code uses the files at the top of this page to find the
+        // instructions for whichever mode it wants to go and run.
+        switch(mode)
+        {
+            // Mode 0: Diagnostic that mode tests that the board is powered on.
+            case 0:
+                RunMode0();
+                break;
+
+            // Mode 1: Garage mode that tests motor speed and direction.
+            case 1:
+                RunMode1();
+                break;
+
+            // Mode 2: Garage mode that tests steering servo.
+            case 2:
+                RunMode2();
+                break;
+
+            // Mode 3: Garage mode that tests the camera settings and input.
+            case 3:
+                RunMode3();
+                break;
+
+            // Mode 4: Track mode with low speed settings for safer (slower)
+                    // initial track testing.
+            case 4:
+                RunMode4();
+                break;
+
+            // Mode 5: Race mode with high speed settings used for faster actual
+                    // racing speeds.
+            case 5:
+                RunMode5();
+                break;
+
+            // default mode activates diagnostic mode 0 to prevent unwanted
+                    // damage or corruption/loss of calibrated data.
+            default:
+                RunMode0();
+                break;
+        } // end case
+    }
+}
\ No newline at end of file