NXP Group 13 / Mbed 2 deprecated Car3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   camera mbed tsi_sensor

Fork of Car2 by Zach Matthews

main.cpp@23:6e1e142b7baf, 2017-04-17 (annotated)

Committer:
lmstthomas
Date:
Mon Apr 17 01:08:11 2017 +0000
Revision:
23:6e1e142b7baf
Parent:
22:9ef4a01e5038
Child:
24:6219b8ce421f
function documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zamatthews 0:b761ef827157 1 #include "mbed.h"
zamatthews 6:971236e48adc 2 #include "Camera.h"
lmstthomas 22:9ef4a01e5038 3 #include "tsi_sensor.h"
zamatthews 17:846417c48571 4 #define STRAIGHT 0.00092f
lmstthomas 4:f4852befd69c 5 #define FULLRIGHT 0.0013f
zamatthews 14:c6f0a3c4e222 6 #define FULLLEFT 0.0005
zamatthews 14:c6f0a3c4e222 7 #define MIN_TURN_RATIO 0
zamatthews 14:c6f0a3c4e222 8 #define MAX_TURN_RATIO 1
zamatthews 20:ebdfeb37309c 9 #define MIN_SPEED 0.17 //.15 seems to be optimal
zamatthews 20:ebdfeb37309c 10 #define MAX_SPEED 0.45 //.5
zamatthews 17:846417c48571 11 #define TURN_TIME 0
zamatthews 20:ebdfeb37309c 12 #define STRAIGHT_TIME 5
zamatthews 19:25f22034a3e2 13 #define START_FINISH_TIME 30
zamatthews 16:60e70bef7828 14 #define DEFAULT_THRESHOLD 65
zamatthews 17:846417c48571 15 #define BLIND_LENGTH 30
zamatthews 17:846417c48571 16 #define DIFF_RATIO 0.5
zamatthews 3:dadfc15fc2d1 17
lmstthomas 22:9ef4a01e5038 18 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
lmstthomas 22:9ef4a01e5038 19 #define ELEC0 9
lmstthomas 22:9ef4a01e5038 20 #define ELEC1 10
lmstthomas 22:9ef4a01e5038 21 #elif defined (TARGET_KL05Z)
lmstthomas 22:9ef4a01e5038 22 #define ELEC0 9
lmstthomas 22:9ef4a01e5038 23 #define ELEC1 8
lmstthomas 22:9ef4a01e5038 24 #else
lmstthomas 22:9ef4a01e5038 25 #error TARGET NOT DEFINED
lmstthomas 22:9ef4a01e5038 26 #endif
lmstthomas 22:9ef4a01e5038 27
lmstthomas 22:9ef4a01e5038 28
zamatthews 3:dadfc15fc2d1 29 PwmOut servo(PTE20);
zamatthews 5:137dfb3e692f 30 PwmOut motor_left(PTA5);
zamatthews 5:137dfb3e692f 31 PwmOut motor_right(PTC8);
zamatthews 3:dadfc15fc2d1 32 DigitalOut DIR_L(PTD4);
zamatthews 3:dadfc15fc2d1 33 DigitalOut DIR_R(PTA4);
zamatthews 9:644102f863a5 34 Serial pc(USBTX, USBRX);
zamatthews 9:644102f863a5 35 Camera cam(PTE23, PTE21, PTB3);
lmstthomas 22:9ef4a01e5038 36 TSIAnalogSlider tsi(ELEC0, ELEC1, 40);
zamatthews 11:45f345aad8ba 37 int turnCounter = 0;
zamatthews 19:25f22034a3e2 38 int startFinishCounter = 0;
zamatthews 16:60e70bef7828 39 int threshold = DEFAULT_THRESHOLD;
zamatthews 11:45f345aad8ba 40 float wheelPos = STRAIGHT;
zamatthews 19:25f22034a3e2 41 bool idle = true;
zamatthews 16:60e70bef7828 42 int leftBlind = 0;
zamatthews 16:60e70bef7828 43 int rightBlind = 0;
lmstthomas 22:9ef4a01e5038 44 float lastSlide;
zamatthews 20:ebdfeb37309c 45 int numDarks = 0;
zamatthews 20:ebdfeb37309c 46 PwmOut led(LED_GREEN);
lmstthomas 22:9ef4a01e5038 47 PwmOut redLed(LED_RED);
zamatthews 0:b761ef827157 48
zamatthews 12:4ccf304800fe 49 /*
zamatthews 12:4ccf304800fe 50 Function: setAccel
zamatthews 12:4ccf304800fe 51 Description: Sets the speed for the right and left motors individually based
zamatthews 12:4ccf304800fe 52 on the turning angle.
zamatthews 12:4ccf304800fe 53 */
zamatthews 19:25f22034a3e2 54 void setAccel(float turnAngle){
zamatthews 20:ebdfeb37309c 55 //idle = false;
zamatthews 19:25f22034a3e2 56 if(!idle){
zamatthews 19:25f22034a3e2 57 turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
zamatthews 19:25f22034a3e2 58 float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
zamatthews 19:25f22034a3e2 59 float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
zamatthews 19:25f22034a3e2 60 motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
zamatthews 19:25f22034a3e2 61 motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
zamatthews 19:25f22034a3e2 62 }
zamatthews 19:25f22034a3e2 63 else{
zamatthews 19:25f22034a3e2 64 motor_left.write(0);
zamatthews 19:25f22034a3e2 65 motor_right.write(0);
zamatthews 19:25f22034a3e2 66 }
zamatthews 12:4ccf304800fe 67 }//end setAccel
zamatthews 3:dadfc15fc2d1 68
zamatthews 12:4ccf304800fe 69 /*
zamatthews 12:4ccf304800fe 70 Function: turnWheels
zamatthews 12:4ccf304800fe 71 Description: Turns the wheels in order to stay between two black lines seen
zamatthews 12:4ccf304800fe 72 by the camera
zamatthews 12:4ccf304800fe 73 */
zamatthews 14:c6f0a3c4e222 74 void turnWheels(int frame[]){
zamatthews 10:246782426144 75 int positionSum = 0;
zamatthews 20:ebdfeb37309c 76 numDarks = 0;
zamatthews 10:246782426144 77 for(int i = 0; i < 128; i++){
zamatthews 16:60e70bef7828 78 if(frame[i] < threshold){
zamatthews 10:246782426144 79 positionSum += i;
zamatthews 10:246782426144 80 numDarks++;
zamatthews 10:246782426144 81 }
zamatthews 10:246782426144 82 }
zamatthews 10:246782426144 83 float averagePos = 0;
zamatthews 12:4ccf304800fe 84
zamatthews 14:c6f0a3c4e222 85 if (numDarks == 0) {
zamatthews 15:50d5cfa98425 86 if(turnCounter >= (STRAIGHT_TIME)){
zamatthews 15:50d5cfa98425 87 wheelPos = STRAIGHT;
zamatthews 15:50d5cfa98425 88 turnCounter = TURN_TIME;
zamatthews 16:60e70bef7828 89 leftBlind = 0;
zamatthews 16:60e70bef7828 90 rightBlind = 0;
zamatthews 15:50d5cfa98425 91 }
zamatthews 11:45f345aad8ba 92 }
zamatthews 12:4ccf304800fe 93
zamatthews 12:4ccf304800fe 94 else {
zamatthews 12:4ccf304800fe 95 averagePos = positionSum / numDarks;
zamatthews 16:60e70bef7828 96
zamatthews 16:60e70bef7828 97 if(((averagePos <= 64 - leftBlind)) && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
zamatthews 16:60e70bef7828 98 float powerRatio = (averagePos / (64 - leftBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
zamatthews 15:50d5cfa98425 99 powerRatio = sqrt(powerRatio);
zamatthews 12:4ccf304800fe 100 wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
zamatthews 12:4ccf304800fe 101 turnCounter = 0;
zamatthews 16:60e70bef7828 102 leftBlind = 0;
zamatthews 16:60e70bef7828 103 rightBlind = BLIND_LENGTH;
zamatthews 11:45f345aad8ba 104 }
zamatthews 12:4ccf304800fe 105
zamatthews 16:60e70bef7828 106 else if((averagePos >= 64 + rightBlind) && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
zamatthews 16:60e70bef7828 107 float powerRatio = (1 - (averagePos - 64 - rightBlind) / (64 - rightBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
zamatthews 15:50d5cfa98425 108 powerRatio = sqrt(powerRatio);
zamatthews 12:4ccf304800fe 109 wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
zamatthews 12:4ccf304800fe 110 turnCounter = 0;
zamatthews 16:60e70bef7828 111 leftBlind = BLIND_LENGTH;
zamatthews 16:60e70bef7828 112 rightBlind = 0;
zamatthews 11:45f345aad8ba 113 }
zamatthews 14:c6f0a3c4e222 114 }
zamatthews 12:4ccf304800fe 115 turnCounter++;
zamatthews 10:246782426144 116 servo.pulsewidth(wheelPos);
zamatthews 3:dadfc15fc2d1 117 }
zamatthews 3:dadfc15fc2d1 118
zamatthews 19:25f22034a3e2 119 /*
zamatthews 19:25f22034a3e2 120 Function: detectStartFinish
zamatthews 19:25f22034a3e2 121 Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
zamatthews 19:25f22034a3e2 122 */
zamatthews 19:25f22034a3e2 123 void detectStartFinish(int frame[]){
zamatthews 20:ebdfeb37309c 124 bool lookForDark = false;
zamatthews 19:25f22034a3e2 125 int darkBlockSize = 0;
zamatthews 19:25f22034a3e2 126 int darkBlocks = 0;
lmstthomas 21:18f2dc256df2 127 int lightGapSize = 0;
lmstthomas 21:18f2dc256df2 128 int minLightGap = 1;
lmstthomas 21:18f2dc256df2 129 int maxLightGap = 55;
lmstthomas 21:18f2dc256df2 130 int minDarkBlock = 4;
lmstthomas 21:18f2dc256df2 131 int maxDarkBlock = 40;
lmstthomas 22:9ef4a01e5038 132 if(tsi.readPercentage() != lastSlide){
lmstthomas 22:9ef4a01e5038 133 idle = !idle;
lmstthomas 22:9ef4a01e5038 134 led = 0.0;
lmstthomas 22:9ef4a01e5038 135 wait(0.5);
lmstthomas 22:9ef4a01e5038 136 }
lmstthomas 22:9ef4a01e5038 137 if(numDarks <= 15) return;
zamatthews 19:25f22034a3e2 138 for(int i = 0; i < 128; i++){
zamatthews 19:25f22034a3e2 139 if(lookForDark){
lmstthomas 21:18f2dc256df2 140 if(frame[i] < threshold){
lmstthomas 21:18f2dc256df2 141 darkBlockSize++;
lmstthomas 21:18f2dc256df2 142 }
lmstthomas 21:18f2dc256df2 143 else{
lmstthomas 21:18f2dc256df2 144 if(darkBlockSize > minDarkBlock && darkBlockSize < maxDarkBlock){
lmstthomas 21:18f2dc256df2 145 darkBlocks++;
lmstthomas 21:18f2dc256df2 146 lookForDark = false;
lmstthomas 21:18f2dc256df2 147 darkBlockSize = 0;
lmstthomas 21:18f2dc256df2 148 lightGapSize = 1;
zamatthews 20:ebdfeb37309c 149 }
lmstthomas 21:18f2dc256df2 150 else{
zamatthews 19:25f22034a3e2 151 darkBlockSize = 0;
zamatthews 19:25f22034a3e2 152 }
zamatthews 19:25f22034a3e2 153 }
lmstthomas 21:18f2dc256df2 154 }
lmstthomas 21:18f2dc256df2 155 if(!lookForDark){
lmstthomas 21:18f2dc256df2 156 if(frame[i] > threshold){
lmstthomas 21:18f2dc256df2 157 lightGapSize++;
lmstthomas 21:18f2dc256df2 158 }
lmstthomas 21:18f2dc256df2 159 else{
lmstthomas 21:18f2dc256df2 160 if(lightGapSize > minLightGap && lightGapSize < maxLightGap){
lmstthomas 21:18f2dc256df2 161 lookForDark = true;
lmstthomas 21:18f2dc256df2 162 minLightGap = 20;
lmstthomas 21:18f2dc256df2 163 lightGapSize = 0;
lmstthomas 21:18f2dc256df2 164 darkBlockSize = 1;
zamatthews 20:ebdfeb37309c 165 }
zamatthews 20:ebdfeb37309c 166 else{
lmstthomas 21:18f2dc256df2 167 lightGapSize = 0;
zamatthews 19:25f22034a3e2 168 }
zamatthews 19:25f22034a3e2 169 }
zamatthews 19:25f22034a3e2 170 }
zamatthews 19:25f22034a3e2 171 }
zamatthews 19:25f22034a3e2 172 if(darkBlocks > 1){
zamatthews 19:25f22034a3e2 173 idle = !idle; //toggle idle
zamatthews 19:25f22034a3e2 174 startFinishCounter = 1;
zamatthews 20:ebdfeb37309c 175 if(!idle){
lmstthomas 22:9ef4a01e5038 176 led = 1.0 - led;
zamatthews 20:ebdfeb37309c 177 servo.pulsewidth(STRAIGHT);
zamatthews 20:ebdfeb37309c 178 wait(5);
zamatthews 20:ebdfeb37309c 179 }
zamatthews 20:ebdfeb37309c 180 else led = 1.0;
zamatthews 19:25f22034a3e2 181 }
zamatthews 19:25f22034a3e2 182 }
zamatthews 19:25f22034a3e2 183
lmstthomas 23:6e1e142b7baf 184 /*
lmstthomas 23:6e1e142b7baf 185 Function: display
lmstthomas 23:6e1e142b7baf 186 Description: This function is used to display what the camera sees to a
lmstthomas 23:6e1e142b7baf 187 computer.
lmstthomas 23:6e1e142b7baf 188 *Before using this function, the car should be connected to the
lmstthomas 23:6e1e142b7baf 189 computer. The car should also not be running. This is necessary because
lmstthomas 23:6e1e142b7baf 190 printing while the car is running will slow it way down and potentially
lmstthomas 23:6e1e142b7baf 191 cause it to crash.
lmstthomas 23:6e1e142b7baf 192
lmstthomas 23:6e1e142b7baf 193 */
zamatthews 12:4ccf304800fe 194 void display(int frame[]){
zamatthews 12:4ccf304800fe 195 char draw = 'x';
zamatthews 12:4ccf304800fe 196 for(int i = 0; i< 128; i++){
zamatthews 17:846417c48571 197
zamatthews 17:846417c48571 198 if (frame[i] < threshold) draw = '|';
zamatthews 12:4ccf304800fe 199 else draw = '-';
zamatthews 12:4ccf304800fe 200 pc.printf("%c", draw);
zamatthews 12:4ccf304800fe 201 draw = 'x';
zamatthews 12:4ccf304800fe 202 }
zamatthews 20:ebdfeb37309c 203 pc.printf("\r");
zamatthews 3:dadfc15fc2d1 204 }
zamatthews 16:60e70bef7828 205
lmstthomas 23:6e1e142b7baf 206 /*
lmstthomas 23:6e1e142b7baf 207 Function: setThreshold
lmstthomas 23:6e1e142b7baf 208 Description: This function is used when the car first starts. It checks the
lmstthomas 23:6e1e142b7baf 209 lightest value and the darkest value it can find, averages them and uses
lmstthomas 23:6e1e142b7baf 210 that as the light/dark threshold.
lmstthomas 23:6e1e142b7baf 211 */
zamatthews 16:60e70bef7828 212 void setThreshold(){
zamatthews 16:60e70bef7828 213 cam.capture();
zamatthews 16:60e70bef7828 214 int low = 99;
zamatthews 16:60e70bef7828 215 int high = 0;
zamatthews 16:60e70bef7828 216 for(int i = 0; i < 128; i++){
zamatthews 16:60e70bef7828 217 if(cam.imageData[i] > high) high = cam.imageData[i];
zamatthews 16:60e70bef7828 218 if(cam.imageData[i] < low) low = cam.imageData[i];
zamatthews 16:60e70bef7828 219 }
lmstthomas 22:9ef4a01e5038 220 // threshold = low + (high - low) * 0.35; //(high + 2 * low) / 3;
lmstthomas 22:9ef4a01e5038 221 threshold = (low + high) / 2;
zamatthews 16:60e70bef7828 222 }
zamatthews 16:60e70bef7828 223
zamatthews 2:0db7cc5ad6db 224 int main() {
zamatthews 16:60e70bef7828 225 setThreshold();
zamatthews 5:137dfb3e692f 226 motor_left.period_us(50);
zamatthews 5:137dfb3e692f 227 motor_right.period_us(50);
zamatthews 5:137dfb3e692f 228 DIR_R = 1;
zamatthews 2:0db7cc5ad6db 229 DIR_L = 0;
zamatthews 3:dadfc15fc2d1 230 servo.period(0.020f);
zamatthews 20:ebdfeb37309c 231 led = 1.0;
lmstthomas 22:9ef4a01e5038 232 redLed = 1.0;
lmstthomas 22:9ef4a01e5038 233 idle = true;
lmstthomas 22:9ef4a01e5038 234 lastSlide = tsi.readPercentage();
zamatthews 2:0db7cc5ad6db 235 while(1){
zamatthews 18:8dbd05e65211 236 wait_ms(5);
zamatthews 9:644102f863a5 237 cam.capture();
zamatthews 12:4ccf304800fe 238 //display(cam.imageData);
zamatthews 19:25f22034a3e2 239 turnWheels(cam.imageData);
zamatthews 19:25f22034a3e2 240 setAccel(wheelPos);
lmstthomas 22:9ef4a01e5038 241 detectStartFinish(cam.imageData);
zamatthews 12:4ccf304800fe 242 }
zamatthews 12:4ccf304800fe 243 }