Daniel Hadad / TFC-RACING-FSLMENTORMATTERS

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

Dependencies:   FRDM-TFC

Fork of TFC-RACING-DEMO by Daniel Hadad

Overview

This 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.

Exercises designed for Mentoring Matters Car Summer Camp for Summer 2014 at Freescale, Inc. in Austin, Texas

Car MODES

5 MODES OR EXERCISES THAT WILL ALIGN WITH DIP SWITCH SETTINGS IN BINARY FASHION e.g. Mode 1 = 001 = switch 1 is on, switch 2 is off, switch 3 is off

0 = 000 = Garage Mode, button light test to see if car alive!!

  • PUSHBUTTON A - Light LEDs 0 and 1
  • PUSHBUTTON B - Light LEDs 2 and 3
  • (switch 4 does nothing)

1 = 001 = Garage Mode, forward/reverse adjust, no auto steer, terminal output

  • 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.
  • (switch 4 does nothing)

2 = 010 = Garage Mode, steering adjust, no auto steer, terminal output

  • POT 1 - Controls Servo: CCW = left; CW = right
  • (switch 4 does nothing)

3 = 011 = Garage Mode, Camera test, some auto steer, terminal output

  • switch 4 : OFF = normal dec data, ON = o-scope mode

4 = 100 = Track Mode, Auto Steer, safe settings

  • LIGHT SPEED = 0.4 default
  • switch 4 = terminal output on/off
  • Pot0 = controls motor speed
  • Pot1 = controls value to multiply with Kp for proportional control

5 = 101 = Track Mode, Auto Steer, fast settings

  • LUDICROUS SPEED = 0.6 default
  • switch 4 = terminal output on/off
  • Pot0 = controls motor speed
  • Pot1 = controls value to multiply with Kp for proportional control

6 = 110 = future upgrades

7 = 111 = future upgrades

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 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

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.

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 constants LIGHT_SPEED and LUDICROUS_SPEED in Spices.cpp. There you can change the speeds used for Modes 4 and 5 above. Range of valid values are 0.4-0.7. Lower is better when debugging the car around the track (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@2:999ca57934bf, 2014-06-25 (annotated)

Committer:
simonchow
Date:
Wed Jun 25 18:25:18 2014 +0000
Revision:
2:999ca57934bf
Parent:
1:87b28e8b9941 
Track mode: Speed controlled with pot0; Kp controlled with pot1;

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 1:87b28e8b9941 70 void TrackMode();
redxeth 0:98e98e01a6ce 71
redxeth 1:87b28e8b9941 72 uint16_t getMode();
redxeth 1:87b28e8b9941 73 bool terminalMode();
redxeth 0:98e98e01a6ce 74
redxeth 0:98e98e01a6ce 75 // prints out line scan data for you
redxeth 0:98e98e01a6ce 76 void printLineScanData(uint16_t *LineScanData);
redxeth 0:98e98e01a6ce 77
redxeth 0:98e98e01a6ce 78 // calculates derivative of line scan data
redxeth 0:98e98e01a6ce 79 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut);
redxeth 0:98e98e01a6ce 80
redxeth 0:98e98e01a6ce 81 // prints out derivative of line scan data -- assumes deriv already calculated
redxeth 0:98e98e01a6ce 82 void printDerivLineScanData(float* derivLineScanData);
redxeth 0:98e98e01a6ce 83
redxeth 0:98e98e01a6ce 84 // serves to grab a frame of camera data
redxeth 0:98e98e01a6ce 85 void grabCameraFrame();
redxeth 0:98e98e01a6ce 86
redxeth 0:98e98e01a6ce 87 // find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
redxeth 0:98e98e01a6ce 88 void findEdges(float* derivLineScanData);
redxeth 0:98e98e01a6ce 89
redxeth 0:98e98e01a6ce 90 // improved algo to find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
redxeth 0:98e98e01a6ce 91 void findEdges_v2(float* derivLineScanData);
redxeth 0:98e98e01a6ce 92
redxeth 0:98e98e01a6ce 93 // prints out edge data found
redxeth 0:98e98e01a6ce 94 void printEdgesFound();
redxeth 0:98e98e01a6ce 95
redxeth 0:98e98e01a6ce 96 // review edge data
redxeth 0:98e98e01a6ce 97 // - report line position if there
redxeth 0:98e98e01a6ce 98 // - set starting gate flag if there
redxeth 0:98e98e01a6ce 99 // - do nothing with position value otherwise
redxeth 0:98e98e01a6ce 100 void reviewEdges();
redxeth 0:98e98e01a6ce 101
redxeth 0:98e98e01a6ce 102 // Decide new actions based on track status
redxeth 0:98e98e01a6ce 103 void ActOnTrackStatus();
redxeth 0:98e98e01a6ce 104
redxeth 0:98e98e01a6ce 105 // Update Steering settings
redxeth 0:98e98e01a6ce 106 void SteeringControl();
redxeth 0:98e98e01a6ce 107
redxeth 0:98e98e01a6ce 108 // Apply steering setting to servo!
redxeth 0:98e98e01a6ce 109 void Steer();
redxeth 0:98e98e01a6ce 110
redxeth 0:98e98e01a6ce 111 // Update speed settings
redxeth 0:98e98e01a6ce 112 void SpeedControl();
redxeth 0:98e98e01a6ce 113
redxeth 0:98e98e01a6ce 114 // Apply speed settings to motors!
redxeth 0:98e98e01a6ce 115 void Drive();
redxeth 0:98e98e01a6ce 116
redxeth 0:98e98e01a6ce 117 // Adjust parameters based on max light measured value
redxeth 0:98e98e01a6ce 118 void adjustLights();
redxeth 0:98e98e01a6ce 119
redxeth 0:98e98e01a6ce 120 // print out light adjust data
redxeth 0:98e98e01a6ce 121 void printAdjustLightsData();
redxeth 0:98e98e01a6ce 122
redxeth 0:98e98e01a6ce 123 // Give user feedback as to detection state via LEDs
redxeth 0:98e98e01a6ce 124 void feedbackLights();
redxeth 0:98e98e01a6ce 125
redxeth 1:87b28e8b9941 126 void useMode();
redxeth 0:98e98e01a6ce 127
redxeth 0:98e98e01a6ce 128 // capture log data
redxeth 0:98e98e01a6ce 129 void captureData();
redxeth 0:98e98e01a6ce 130
redxeth 0:98e98e01a6ce 131 // dump log data to terminal
redxeth 0:98e98e01a6ce 132 void dumpData();
redxeth 0:98e98e01a6ce 133
redxeth 0:98e98e01a6ce 134 #endif