Daniel Hadad / TFC-RACING-DEMO

This is an example program that actually allows the car to race using the FRDM-TFC library!

Dependencies:   FRDM-TFC

Overview

This program is an example program that uses the FRDM-TFC library to actually permit a car using the FDRM-TFC shield + FRDM-KL25Z to race around a center line track.

Car Hookup

Motors

  • Code written assuming left motor hooked to B1(red)/B2(black) and right motor hooked to A1(red)/A2(black).

Battery

  • Be sure to hook positive (red) to 'Vbat' and negative (black) to 'Gnd'

Steering Servo

  • Servo must be hooked up with black wire innermost (away from LEDs).
  • Also be sure to mount servo on chassis with wire coming out the right side of the car.

Camera

  • Camera must be hooked up with black wire towards LEDs on the shield board.
  • Be sure to mount camera on tower with connector down towards the bottom.

Potentiometers (Pots)

  • Pots by default should have arrows pointing toward battery/motor hookup (for demo mods default).

Car Hardware Controls

DIP SWITCH CONTROLS:

  • Switch 1: Controls whether running main racing program (ON) or Demo program (OFF). Function of other 3 switches depend on this first switch.

RACING MODE (Switch 1 = ON)

  • Switch 2: Logging frame camera data to RAM while racing. Enable (ON) or Disable (OFF).
  • Switch 3: Risky / faster racing (ON); Conservative / slower race option (OFF). NOTE: Faster mode may cause car to fall off track. Needs tweaking...
  • Switch 4: Enable Start Gate Stop (ON) or Disable (OFF). NOTE: May not actually stop at start gate. Needs tweaking...
  • POTs 1,2 - controls nothing at the moment
  • PUSHBUTTON A - START car race!
  • PUSHBUTTON B - END CAR RACE / (while holding down when 'log frame data' active will also output terminal data gathered during race)
  • LEDs 1,2 light when Track is found
  • LEDs 0,3 light when Start Gate is Found

DEMO MODE 0 (Switch 1 = OFF; Switch 2 = OFF, Switch 3 = OFF)

  • PUSHBUTTON A - Light LED 0
  • PUSHBUTTON B - Light LED 3

DEMO MODE 1 (Switch 1 = OFF; Switch 2 = ON, Switch 3 = OFF)

  • POT 1 - Controls Servo: CCW = left; CW = right

DEMO MODE 2 (Switch 1 = OFF; Switch 2 = OFF, Switch 3 = ON)

  • POT 1 - Controls speed of right wheel: CCW = reverse; CW = forward
  • POT 2 - Controls speed of left wheel: CCW = reverse; CW = forward
  • NOTE In this mode the high pitched whine heard is of the H-bridge being active. To disable whine, briefly put into demo mode 1 above.

DEMO MODE 3 (Switch 1 = OFF; Switch 2 = ON, Switch 3 = ON)

  • Outputs camera data captured to terminal in CSV format, once every second.

Car Calibration

Serial Terminal

  • Download your favorite Terminal application (I use TeraTerm. Set it for 115200 baud, 8bit, none, 1bit.
  • But first you have to be sure that Windows mbed serial driver has been installed: Windows serial config.

Camera

  • If you want to hook up a scope, here are the pins of the camera interface from left to right: ground, 3.3V, SI, CLK, Analog_Out.
  • You want to hook up channel 1 between Analog_Out and Ground, channel 2 between SI and ground. SI can serve as your trigger.
  • Alternatively if you only want to use one channel, you can only hook up to Analog_Out and use triggering on the same channel.
  • Be sure to focus the camera properly. See demo video.
  • Be sure to prevent light noise on camera: Avoid Light Noise
  • More camera info

Servo/Steering

  • You will need to put the car into demo mode 1 and connect up a terminal to the serial port in order to get feedback on the values for your particular servo as hooked up. Then change MAX_STEER_LEFT and MAX_STEER_RIGHT in Spices.cpp with these values to tell the program how your servo is hooked up.
  • Here's a video where values are output to LCD to make it easier: http://youtu.be/fp5gyfEMf50

Speed Control

  • This program does not have proper speed control but does modify the speed slightly based on recent error results from the camera. It also modifies the differential speed to each wheel to have better control around curves.
  • While debugging your car you may want to lower the speed. See the function SpeedControl in Spices.cpp. There you can change which speed method you'd like to use. Currently is using value of 1, which gets maximum speed from TUNE_SPEED constant. Reduce to 0.4 or 0.5 when debugging car around the car (mainly to minimize crash forces!).

Strange Gotchas

Glitchy Motors

  • Apparently there is contention between TPM0_CH0 and OpenSDA micro that causes strange issues with Motors (PWM interference). This will cause glitches in motor activty when hooked up to USB only: Found contention

More Info

Spices/Spices.h@0:98e98e01a6ce, 2014-04-20 (annotated)

Committer:
redxeth
Date:
Sun Apr 20 03:33:03 2014 +0000
Revision:
0:98e98e01a6ce
Initial version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
redxeth 0:98e98e01a6ce 1 #include "mbed.h"
redxeth 0:98e98e01a6ce 2
redxeth 0:98e98e01a6ce 3 #ifndef _SPICES_H
redxeth 0:98e98e01a6ce 4 #define _SPICES_H
redxeth 0:98e98e01a6ce 5
redxeth 0:98e98e01a6ce 6 // *******************************************
redxeth 0:98e98e01a6ce 7 // MASTER CONTROL PROGRAM
redxeth 0:98e98e01a6ce 8
redxeth 0:98e98e01a6ce 9 // Loop at Servo cycle (currently 50Hz / 20mS)
redxeth 0:98e98e01a6ce 10 // Camera gets updated at its own period
redxeth 0:98e98e01a6ce 11 // (currently 50Hz / 20mS)
redxeth 0:98e98e01a6ce 12 //
redxeth 0:98e98e01a6ce 13 // REQUIREMENTS:
redxeth 0:98e98e01a6ce 14 // - Control servos in response to track line position
redxeth 0:98e98e01a6ce 15 // - Gather as much camera data as possible between
redxeth 0:98e98e01a6ce 16 // servo update cycles to get best read on the line
redxeth 0:98e98e01a6ce 17 // (currently 1:1 though @ 50Hz)
redxeth 0:98e98e01a6ce 18 // - Ignore erronous signals:
redxeth 0:98e98e01a6ce 19 // - "mud" on track
redxeth 0:98e98e01a6ce 20 // - low contrast light situations
redxeth 0:98e98e01a6ce 21 // - track crossing itself (all black situation)
redxeth 0:98e98e01a6ce 22 // - Detects 'starting line' -- ignores first detect (START)
redxeth 0:98e98e01a6ce 23 // and stops after second detect (FINISH)
redxeth 0:98e98e01a6ce 24 //
redxeth 0:98e98e01a6ce 25 // INPUTS:
redxeth 0:98e98e01a6ce 26 // - linescancamera data (0-127)
redxeth 0:98e98e01a6ce 27 // - maximum speed
redxeth 0:98e98e01a6ce 28 // - filter settings:
redxeth 0:98e98e01a6ce 29 // A = 15 (number of pixels at each end to ignore)
redxeth 0:98e98e01a6ce 30 // B = 20 (estimated width of line)(possible issue if line far away)
redxeth 0:98e98e01a6ce 31 // C = 2^12 (max value based on ADC sample rate of 12 bits)
redxeth 0:98e98e01a6ce 32 // D = Edge threshold of Derivative (max / 4 = 2^10)
redxeth 0:98e98e01a6ce 33 // - control parameters, KP and KD
redxeth 0:98e98e01a6ce 34 //
redxeth 0:98e98e01a6ce 35 // CONTROL ALGORITHM:
redxeth 0:98e98e01a6ce 36 // 1 - Get Line Position
redxeth 0:98e98e01a6ce 37 // 2 - Compute Error from Line Position
redxeth 0:98e98e01a6ce 38 // 3 - Set Servo position based on Error and Derivative of Error
redxeth 0:98e98e01a6ce 39 // 4 - Store error for next cycle to perform derivative
redxeth 0:98e98e01a6ce 40 //
redxeth 0:98e98e01a6ce 41 //
redxeth 0:98e98e01a6ce 42 // GET LINE POSITION
redxeth 0:98e98e01a6ce 43 // INPUTS: linescan data
redxeth 0:98e98e01a6ce 44 // RETURNS: either position or starting gate flag
redxeth 0:98e98e01a6ce 45 // ALGO:
redxeth 0:98e98e01a6ce 46 // - Find line edges:
redxeth 0:98e98e01a6ce 47 // - filter out first and last A number of pixels from left and right
redxeth 0:98e98e01a6ce 48 // - calculate derivative of data
redxeth 0:98e98e01a6ce 49 // - search for line edges in derivative data (search for either track line or starting gate)
redxeth 0:98e98e01a6ce 50 // - Search for all pixels with Value < -D, indicating negative edge (large negative means going from white to black)
redxeth 0:98e98e01a6ce 51 // - Search for all pixels with Value > D, indicating positive edge (large positive means going from black to white)
redxeth 0:98e98e01a6ce 52 // - Clean up adjacent line edges-- edges 1 pixel away from each other combined to be considered a single edge, average pixel value
redxeth 0:98e98e01a6ce 53 // - if starting gate then send up flag
redxeth 0:98e98e01a6ce 54 // - if track, calculate position
redxeth 0:98e98e01a6ce 55 //
redxeth 0:98e98e01a6ce 56 //
redxeth 0:98e98e01a6ce 57 // COMPUTE LINE ERROR
redxeth 0:98e98e01a6ce 58 // - take line position and target-- calculate error (negative or positive)
redxeth 0:98e98e01a6ce 59 //
redxeth 0:98e98e01a6ce 60 // SET SERVO POSITION
redxeth 0:98e98e01a6ce 61 // - int servoPosition = KP * Error + KD * (Error - lastError);
redxeth 0:98e98e01a6ce 62 // - lastError = Error;
redxeth 0:98e98e01a6ce 63 //
redxeth 0:98e98e01a6ce 64 //
redxeth 0:98e98e01a6ce 65 // TBD:
redxeth 0:98e98e01a6ce 66 // - store multiple edge positions in order to average prior to next servo update
redxeth 0:98e98e01a6ce 67 //
redxeth 0:98e98e01a6ce 68 // *******************************************
redxeth 0:98e98e01a6ce 69 //
redxeth 0:98e98e01a6ce 70 void MasterControlProgram();
redxeth 0:98e98e01a6ce 71
redxeth 0:98e98e01a6ce 72
redxeth 0:98e98e01a6ce 73 // prints out line scan data for you
redxeth 0:98e98e01a6ce 74 void printLineScanData(uint16_t *LineScanData);
redxeth 0:98e98e01a6ce 75
redxeth 0:98e98e01a6ce 76 // calculates derivative of line scan data
redxeth 0:98e98e01a6ce 77 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut);
redxeth 0:98e98e01a6ce 78
redxeth 0:98e98e01a6ce 79 // prints out derivative of line scan data -- assumes deriv already calculated
redxeth 0:98e98e01a6ce 80 void printDerivLineScanData(float* derivLineScanData);
redxeth 0:98e98e01a6ce 81
redxeth 0:98e98e01a6ce 82 // serves to grab a frame of camera data
redxeth 0:98e98e01a6ce 83 void grabCameraFrame();
redxeth 0:98e98e01a6ce 84
redxeth 0:98e98e01a6ce 85 // find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
redxeth 0:98e98e01a6ce 86 void findEdges(float* derivLineScanData);
redxeth 0:98e98e01a6ce 87
redxeth 0:98e98e01a6ce 88 // improved algo to find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
redxeth 0:98e98e01a6ce 89 void findEdges_v2(float* derivLineScanData);
redxeth 0:98e98e01a6ce 90
redxeth 0:98e98e01a6ce 91 // prints out edge data found
redxeth 0:98e98e01a6ce 92 void printEdgesFound();
redxeth 0:98e98e01a6ce 93
redxeth 0:98e98e01a6ce 94 // review edge data
redxeth 0:98e98e01a6ce 95 // - report line position if there
redxeth 0:98e98e01a6ce 96 // - set starting gate flag if there
redxeth 0:98e98e01a6ce 97 // - do nothing with position value otherwise
redxeth 0:98e98e01a6ce 98 void reviewEdges();
redxeth 0:98e98e01a6ce 99
redxeth 0:98e98e01a6ce 100 // Decide new actions based on track status
redxeth 0:98e98e01a6ce 101 void ActOnTrackStatus();
redxeth 0:98e98e01a6ce 102
redxeth 0:98e98e01a6ce 103 // Update Steering settings
redxeth 0:98e98e01a6ce 104 void SteeringControl();
redxeth 0:98e98e01a6ce 105
redxeth 0:98e98e01a6ce 106 // Apply steering setting to servo!
redxeth 0:98e98e01a6ce 107 void Steer();
redxeth 0:98e98e01a6ce 108
redxeth 0:98e98e01a6ce 109 // Update speed settings
redxeth 0:98e98e01a6ce 110 void SpeedControl();
redxeth 0:98e98e01a6ce 111
redxeth 0:98e98e01a6ce 112 // Apply speed settings to motors!
redxeth 0:98e98e01a6ce 113 void Drive();
redxeth 0:98e98e01a6ce 114
redxeth 0:98e98e01a6ce 115 // Adjust parameters based on max light measured value
redxeth 0:98e98e01a6ce 116 void adjustLights();
redxeth 0:98e98e01a6ce 117
redxeth 0:98e98e01a6ce 118 // print out light adjust data
redxeth 0:98e98e01a6ce 119 void printAdjustLightsData();
redxeth 0:98e98e01a6ce 120
redxeth 0:98e98e01a6ce 121 // Give user feedback as to detection state via LEDs
redxeth 0:98e98e01a6ce 122 void feedbackLights();
redxeth 0:98e98e01a6ce 123
redxeth 0:98e98e01a6ce 124 // read DIP switches and potentiometers for setting changes
redxeth 0:98e98e01a6ce 125 void readSwitches();
redxeth 0:98e98e01a6ce 126
redxeth 0:98e98e01a6ce 127 // capture log data
redxeth 0:98e98e01a6ce 128 void captureData();
redxeth 0:98e98e01a6ce 129
redxeth 0:98e98e01a6ce 130 // dump log data to terminal
redxeth 0:98e98e01a6ce 131 void dumpData();
redxeth 0:98e98e01a6ce 132
redxeth 0:98e98e01a6ce 133 #endif