Zach Matthews / Mbed 2 deprecated Car2

Car 2: Electric Boogaloo

Dependencies:   camera mbed

Fork of Car2 by NXP Group 13

Code for an NXP Cup car using a linescan Camera

main.cpp@19:25f22034a3e2, 2017-04-04 (annotated)

Committer:
zamatthews
Date:
Tue Apr 04 22:50:00 2017 +0000
Revision:
19:25f22034a3e2
Parent:
18:8dbd05e65211
Child:
20:ebdfeb37309c
added mostly functional detectStartFinish; -Levi

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