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@26:4afa8c5c5156, 2017-04-20 (annotated)

Committer:
lmstthomas
Date:
Thu Apr 20 19:32:16 2017 +0000
Revision:
26:4afa8c5c5156
Parent:
25:74c12b0acf0c 
final commit????;

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