Keith Cyr
TFC-Mentoring-Matters-Abstraction
Revision 0:0699eb71778e, committed 2014-07-16
- 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
diff -r 000000000000 -r 0699eb71778e FRDM-TFC.lib --- /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
diff -r 000000000000 -r 0699eb71778e RaceCar/Inits.cpp
--- /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
diff -r 000000000000 -r 0699eb71778e RaceCar/Inits.h --- /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();
diff -r 000000000000 -r 0699eb71778e RaceCar/Modes.cpp
--- /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");
+ }
+ }
+}
diff -r 000000000000 -r 0699eb71778e RaceCar/Modes.h --- /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();
diff -r 000000000000 -r 0699eb71778e RaceCar/TheDetails.cpp
--- /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;
+}
diff -r 000000000000 -r 0699eb71778e RaceCar/TheDetails.h --- /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();
diff -r 000000000000 -r 0699eb71778e common.cpp --- /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
diff -r 000000000000 -r 0699eb71778e common.h --- /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
diff -r 000000000000 -r 0699eb71778e main.cpp
--- /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