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@21:18f2dc256df2, 2017-04-11 (annotated)

Committer:
lmstthomas
Date:
Tue Apr 11 20:57:17 2017 +0000
Revision:
21:18f2dc256df2
Parent:
20:ebdfeb37309c
Child:
22:9ef4a01e5038
mostly functional detectStartFinish?????

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"
zamatthews 17:846417c48571 3 #define STRAIGHT 0.00092f
lmstthomas 4:f4852befd69c 4 #define FULLRIGHT 0.0013f
zamatthews 14:c6f0a3c4e222 5 #define FULLLEFT 0.0005
zamatthews 14:c6f0a3c4e222 6 #define MIN_TURN_RATIO 0
zamatthews 14:c6f0a3c4e222 7 #define MAX_TURN_RATIO 1
zamatthews 20:ebdfeb37309c 8 #define MIN_SPEED 0.17 //.15 seems to be optimal
zamatthews 20:ebdfeb37309c 9 #define MAX_SPEED 0.45 //.5
zamatthews 17:846417c48571 10 #define TURN_TIME 0
zamatthews 20:ebdfeb37309c 11 #define STRAIGHT_TIME 5
zamatthews 19:25f22034a3e2 12 #define START_FINISH_TIME 30
zamatthews 16:60e70bef7828 13 #define DEFAULT_THRESHOLD 65
zamatthews 17:846417c48571 14 #define BLIND_LENGTH 30
zamatthews 17:846417c48571 15 #define DIFF_RATIO 0.5
zamatthews 3:dadfc15fc2d1 16
zamatthews 3:dadfc15fc2d1 17 PwmOut servo(PTE20);
zamatthews 5:137dfb3e692f 18 PwmOut motor_left(PTA5);
zamatthews 5:137dfb3e692f 19 PwmOut motor_right(PTC8);
zamatthews 3:dadfc15fc2d1 20 DigitalOut DIR_L(PTD4);
zamatthews 3:dadfc15fc2d1 21 DigitalOut DIR_R(PTA4);
zamatthews 9:644102f863a5 22 Serial pc(USBTX, USBRX);
zamatthews 9:644102f863a5 23 Camera cam(PTE23, PTE21, PTB3);
zamatthews 11:45f345aad8ba 24 int turnCounter = 0;
zamatthews 19:25f22034a3e2 25 int startFinishCounter = 0;
zamatthews 16:60e70bef7828 26 int threshold = DEFAULT_THRESHOLD;
zamatthews 11:45f345aad8ba 27 float wheelPos = STRAIGHT;
zamatthews 19:25f22034a3e2 28 bool idle = true;
zamatthews 16:60e70bef7828 29 int leftBlind = 0;
zamatthews 16:60e70bef7828 30 int rightBlind = 0;
zamatthews 20:ebdfeb37309c 31 int numDarks = 0;
zamatthews 20:ebdfeb37309c 32 PwmOut led(LED_GREEN);
zamatthews 0:b761ef827157 33
zamatthews 12:4ccf304800fe 34 /*
zamatthews 12:4ccf304800fe 35 Function: setAccel
zamatthews 12:4ccf304800fe 36 Description: Sets the speed for the right and left motors individually based
zamatthews 12:4ccf304800fe 37 on the turning angle.
zamatthews 12:4ccf304800fe 38 */
zamatthews 19:25f22034a3e2 39 void setAccel(float turnAngle){
zamatthews 20:ebdfeb37309c 40 //idle = false;
zamatthews 19:25f22034a3e2 41 if(!idle){
zamatthews 19:25f22034a3e2 42 turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
zamatthews 19:25f22034a3e2 43 float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
zamatthews 19:25f22034a3e2 44 float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
zamatthews 19:25f22034a3e2 45 motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
zamatthews 19:25f22034a3e2 46 motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
zamatthews 19:25f22034a3e2 47 }
zamatthews 19:25f22034a3e2 48 else{
zamatthews 19:25f22034a3e2 49 motor_left.write(0);
zamatthews 19:25f22034a3e2 50 motor_right.write(0);
zamatthews 19:25f22034a3e2 51 }
zamatthews 12:4ccf304800fe 52 }//end setAccel
zamatthews 3:dadfc15fc2d1 53
zamatthews 12:4ccf304800fe 54 /*
zamatthews 12:4ccf304800fe 55 Function: turnWheels
zamatthews 12:4ccf304800fe 56 Description: Turns the wheels in order to stay between two black lines seen
zamatthews 12:4ccf304800fe 57 by the camera
zamatthews 12:4ccf304800fe 58 */
zamatthews 14:c6f0a3c4e222 59 void turnWheels(int frame[]){
zamatthews 10:246782426144 60 int positionSum = 0;
zamatthews 20:ebdfeb37309c 61 numDarks = 0;
zamatthews 10:246782426144 62 for(int i = 0; i < 128; i++){
zamatthews 16:60e70bef7828 63 if(frame[i] < threshold){
zamatthews 10:246782426144 64 positionSum += i;
zamatthews 10:246782426144 65 numDarks++;
zamatthews 10:246782426144 66 }
zamatthews 10:246782426144 67 }
zamatthews 10:246782426144 68 float averagePos = 0;
zamatthews 12:4ccf304800fe 69
zamatthews 14:c6f0a3c4e222 70 if (numDarks == 0) {
zamatthews 15:50d5cfa98425 71 if(turnCounter >= (STRAIGHT_TIME)){
zamatthews 15:50d5cfa98425 72 wheelPos = STRAIGHT;
zamatthews 15:50d5cfa98425 73 turnCounter = TURN_TIME;
zamatthews 16:60e70bef7828 74 leftBlind = 0;
zamatthews 16:60e70bef7828 75 rightBlind = 0;
zamatthews 15:50d5cfa98425 76 }
zamatthews 11:45f345aad8ba 77 }
zamatthews 12:4ccf304800fe 78
zamatthews 12:4ccf304800fe 79 else {
zamatthews 12:4ccf304800fe 80 averagePos = positionSum / numDarks;
zamatthews 16:60e70bef7828 81
zamatthews 16:60e70bef7828 82 if(((averagePos <= 64 - leftBlind)) && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
zamatthews 16:60e70bef7828 83 float powerRatio = (averagePos / (64 - leftBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
zamatthews 15:50d5cfa98425 84 powerRatio = sqrt(powerRatio);
zamatthews 12:4ccf304800fe 85 wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
zamatthews 12:4ccf304800fe 86 turnCounter = 0;
zamatthews 16:60e70bef7828 87 leftBlind = 0;
zamatthews 16:60e70bef7828 88 rightBlind = BLIND_LENGTH;
zamatthews 11:45f345aad8ba 89 }
zamatthews 12:4ccf304800fe 90
zamatthews 16:60e70bef7828 91 else if((averagePos >= 64 + rightBlind) && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
zamatthews 16:60e70bef7828 92 float powerRatio = (1 - (averagePos - 64 - rightBlind) / (64 - rightBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
zamatthews 15:50d5cfa98425 93 powerRatio = sqrt(powerRatio);
zamatthews 12:4ccf304800fe 94 wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
zamatthews 12:4ccf304800fe 95 turnCounter = 0;
zamatthews 16:60e70bef7828 96 leftBlind = BLIND_LENGTH;
zamatthews 16:60e70bef7828 97 rightBlind = 0;
zamatthews 11:45f345aad8ba 98 }
zamatthews 14:c6f0a3c4e222 99 }
zamatthews 12:4ccf304800fe 100 turnCounter++;
zamatthews 10:246782426144 101 servo.pulsewidth(wheelPos);
zamatthews 3:dadfc15fc2d1 102 }
zamatthews 3:dadfc15fc2d1 103
zamatthews 19:25f22034a3e2 104 /*
zamatthews 19:25f22034a3e2 105 Function: detectStartFinish
zamatthews 19:25f22034a3e2 106 Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
zamatthews 19:25f22034a3e2 107 */
zamatthews 19:25f22034a3e2 108 void detectStartFinish(int frame[]){
zamatthews 20:ebdfeb37309c 109 bool lookForDark = false;
zamatthews 19:25f22034a3e2 110 int darkBlockSize = 0;
zamatthews 19:25f22034a3e2 111 int darkBlocks = 0;
lmstthomas 21:18f2dc256df2 112 int lightGapSize = 0;
lmstthomas 21:18f2dc256df2 113 int minLightGap = 1;
lmstthomas 21:18f2dc256df2 114 int maxLightGap = 55;
lmstthomas 21:18f2dc256df2 115 int minDarkBlock = 4;
lmstthomas 21:18f2dc256df2 116 int maxDarkBlock = 40;
zamatthews 19:25f22034a3e2 117 for(int i = 0; i < 128; i++){
zamatthews 19:25f22034a3e2 118 if(lookForDark){
lmstthomas 21:18f2dc256df2 119 if(frame[i] < threshold){
lmstthomas 21:18f2dc256df2 120 darkBlockSize++;
lmstthomas 21:18f2dc256df2 121 }
lmstthomas 21:18f2dc256df2 122 else{
lmstthomas 21:18f2dc256df2 123 if(darkBlockSize > minDarkBlock && darkBlockSize < maxDarkBlock){
lmstthomas 21:18f2dc256df2 124 darkBlocks++;
lmstthomas 21:18f2dc256df2 125 lookForDark = false;
lmstthomas 21:18f2dc256df2 126 darkBlockSize = 0;
lmstthomas 21:18f2dc256df2 127 lightGapSize = 1;
zamatthews 20:ebdfeb37309c 128 }
lmstthomas 21:18f2dc256df2 129 else{
zamatthews 19:25f22034a3e2 130 darkBlockSize = 0;
zamatthews 19:25f22034a3e2 131 }
zamatthews 19:25f22034a3e2 132 }
lmstthomas 21:18f2dc256df2 133 }
lmstthomas 21:18f2dc256df2 134 if(!lookForDark){
lmstthomas 21:18f2dc256df2 135 if(frame[i] > threshold){
lmstthomas 21:18f2dc256df2 136 lightGapSize++;
lmstthomas 21:18f2dc256df2 137 }
lmstthomas 21:18f2dc256df2 138 else{
lmstthomas 21:18f2dc256df2 139 if(lightGapSize > minLightGap && lightGapSize < maxLightGap){
lmstthomas 21:18f2dc256df2 140 lookForDark = true;
lmstthomas 21:18f2dc256df2 141 minLightGap = 20;
lmstthomas 21:18f2dc256df2 142 lightGapSize = 0;
lmstthomas 21:18f2dc256df2 143 darkBlockSize = 1;
zamatthews 20:ebdfeb37309c 144 }
zamatthews 20:ebdfeb37309c 145 else{
lmstthomas 21:18f2dc256df2 146 lightGapSize = 0;
zamatthews 19:25f22034a3e2 147 }
zamatthews 19:25f22034a3e2 148 }
zamatthews 19:25f22034a3e2 149 }
zamatthews 19:25f22034a3e2 150 }
zamatthews 19:25f22034a3e2 151 if(darkBlocks > 1){
zamatthews 19:25f22034a3e2 152 idle = !idle; //toggle idle
zamatthews 19:25f22034a3e2 153 startFinishCounter = 1;
zamatthews 20:ebdfeb37309c 154 if(!idle){
zamatthews 20:ebdfeb37309c 155 led = 0.0;
zamatthews 20:ebdfeb37309c 156 servo.pulsewidth(STRAIGHT);
zamatthews 20:ebdfeb37309c 157 wait(5);
zamatthews 20:ebdfeb37309c 158 }
zamatthews 20:ebdfeb37309c 159 else led = 1.0;
zamatthews 19:25f22034a3e2 160 }
zamatthews 19:25f22034a3e2 161 }
zamatthews 19:25f22034a3e2 162
zamatthews 12:4ccf304800fe 163 void display(int frame[]){
zamatthews 12:4ccf304800fe 164 char draw = 'x';
zamatthews 12:4ccf304800fe 165 for(int i = 0; i< 128; i++){
zamatthews 17:846417c48571 166
zamatthews 17:846417c48571 167 if (frame[i] < threshold) draw = '|';
zamatthews 12:4ccf304800fe 168 else draw = '-';
zamatthews 12:4ccf304800fe 169 pc.printf("%c", draw);
zamatthews 12:4ccf304800fe 170 draw = 'x';
zamatthews 12:4ccf304800fe 171 }
zamatthews 20:ebdfeb37309c 172 pc.printf("\r");
zamatthews 3:dadfc15fc2d1 173 }
zamatthews 16:60e70bef7828 174
zamatthews 16:60e70bef7828 175 void setThreshold(){
zamatthews 16:60e70bef7828 176 cam.capture();
zamatthews 16:60e70bef7828 177 int low = 99;
zamatthews 16:60e70bef7828 178 int high = 0;
zamatthews 16:60e70bef7828 179 for(int i = 0; i < 128; i++){
zamatthews 16:60e70bef7828 180 if(cam.imageData[i] > high) high = cam.imageData[i];
zamatthews 16:60e70bef7828 181 if(cam.imageData[i] < low) low = cam.imageData[i];
zamatthews 16:60e70bef7828 182 }
zamatthews 20:ebdfeb37309c 183 threshold = low + (high - low) * 0.35; //(high + 2 * low) / 3;
zamatthews 16:60e70bef7828 184 }
zamatthews 16:60e70bef7828 185
zamatthews 2:0db7cc5ad6db 186 int main() {
zamatthews 16:60e70bef7828 187 setThreshold();
zamatthews 5:137dfb3e692f 188 motor_left.period_us(50);
zamatthews 5:137dfb3e692f 189 motor_right.period_us(50);
zamatthews 5:137dfb3e692f 190 DIR_R = 1;
zamatthews 2:0db7cc5ad6db 191 DIR_L = 0;
zamatthews 3:dadfc15fc2d1 192 servo.period(0.020f);
zamatthews 20:ebdfeb37309c 193 led = 1.0;
zamatthews 2:0db7cc5ad6db 194 while(1){
zamatthews 18:8dbd05e65211 195 wait_ms(5);
zamatthews 9:644102f863a5 196 cam.capture();
zamatthews 12:4ccf304800fe 197 //display(cam.imageData);
zamatthews 19:25f22034a3e2 198 turnWheels(cam.imageData);
zamatthews 19:25f22034a3e2 199 setAccel(wheelPos);
zamatthews 20:ebdfeb37309c 200 if(numDarks > 15) detectStartFinish(cam.imageData);
zamatthews 12:4ccf304800fe 201 }
zamatthews 12:4ccf304800fe 202 }