Car 2: Electric Boogaloo
Fork of Car2 by
Code for an NXP Cup car using a linescan Camera
main.cpp@20:ebdfeb37309c, 2017-04-11 (annotated)
- Committer:
- zamatthews
- Date:
- Tue Apr 11 20:25:51 2017 +0000
- Revision:
- 20:ebdfeb37309c
- Parent:
- 19:25f22034a3e2
fuck zach
Who changed what in which revision?
User | Revision | Line number | New 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; |
zamatthews | 19:25f22034a3e2 | 112 | int lightGap = 0; |
zamatthews | 19:25f22034a3e2 | 113 | for(int i = 0; i < 128; i++){ |
zamatthews | 19:25f22034a3e2 | 114 | if(lookForDark){ |
zamatthews | 20:ebdfeb37309c | 115 | |
zamatthews | 20:ebdfeb37309c | 116 | if(true){ |
zamatthews | 20:ebdfeb37309c | 117 | if(frame[i] < threshold){ |
zamatthews | 20:ebdfeb37309c | 118 | darkBlockSize++; |
zamatthews | 20:ebdfeb37309c | 119 | if(darkBlockSize > 4){ //minimum size for a dark block |
zamatthews | 20:ebdfeb37309c | 120 | darkBlocks++; |
zamatthews | 20:ebdfeb37309c | 121 | lookForDark = false; |
zamatthews | 20:ebdfeb37309c | 122 | darkBlockSize = 0; |
zamatthews | 20:ebdfeb37309c | 123 | } |
zamatthews | 20:ebdfeb37309c | 124 | } |
zamatthews | 20:ebdfeb37309c | 125 | else if(frame[i] > threshold){ |
zamatthews | 19:25f22034a3e2 | 126 | darkBlockSize = 0; |
zamatthews | 19:25f22034a3e2 | 127 | } |
zamatthews | 19:25f22034a3e2 | 128 | } |
zamatthews | 20:ebdfeb37309c | 129 | if(!lookForDark){ |
zamatthews | 20:ebdfeb37309c | 130 | if(frame[i] > threshold){ |
zamatthews | 20:ebdfeb37309c | 131 | lightGap++; |
zamatthews | 20:ebdfeb37309c | 132 | } |
zamatthews | 20:ebdfeb37309c | 133 | else{ |
zamatthews | 20:ebdfeb37309c | 134 | if(lightGap > 20 && lightGap < 55){ |
zamatthews | 20:ebdfeb37309c | 135 | lookForDark = true; |
zamatthews | 20:ebdfeb37309c | 136 | lightGap = 0; |
zamatthews | 20:ebdfeb37309c | 137 | } |
zamatthews | 20:ebdfeb37309c | 138 | else{ |
zamatthews | 20:ebdfeb37309c | 139 | lightGap = 0; |
zamatthews | 20:ebdfeb37309c | 140 | } |
zamatthews | 19:25f22034a3e2 | 141 | } |
zamatthews | 19:25f22034a3e2 | 142 | } |
zamatthews | 19:25f22034a3e2 | 143 | } |
zamatthews | 19:25f22034a3e2 | 144 | } |
zamatthews | 19:25f22034a3e2 | 145 | if(darkBlocks > 1){ |
zamatthews | 19:25f22034a3e2 | 146 | idle = !idle; //toggle idle |
zamatthews | 19:25f22034a3e2 | 147 | startFinishCounter = 1; |
zamatthews | 20:ebdfeb37309c | 148 | if(!idle){ |
zamatthews | 20:ebdfeb37309c | 149 | led = 0.0; |
zamatthews | 20:ebdfeb37309c | 150 | servo.pulsewidth(STRAIGHT); |
zamatthews | 20:ebdfeb37309c | 151 | wait(5); |
zamatthews | 20:ebdfeb37309c | 152 | } |
zamatthews | 20:ebdfeb37309c | 153 | else led = 1.0; |
zamatthews | 19:25f22034a3e2 | 154 | } |
zamatthews | 19:25f22034a3e2 | 155 | } |
zamatthews | 19:25f22034a3e2 | 156 | |
zamatthews | 12:4ccf304800fe | 157 | void display(int frame[]){ |
zamatthews | 12:4ccf304800fe | 158 | char draw = 'x'; |
zamatthews | 12:4ccf304800fe | 159 | for(int i = 0; i< 128; i++){ |
zamatthews | 17:846417c48571 | 160 | |
zamatthews | 17:846417c48571 | 161 | if (frame[i] < threshold) draw = '|'; |
zamatthews | 12:4ccf304800fe | 162 | else draw = '-'; |
zamatthews | 12:4ccf304800fe | 163 | pc.printf("%c", draw); |
zamatthews | 12:4ccf304800fe | 164 | draw = 'x'; |
zamatthews | 12:4ccf304800fe | 165 | } |
zamatthews | 20:ebdfeb37309c | 166 | pc.printf("\r"); |
zamatthews | 3:dadfc15fc2d1 | 167 | } |
zamatthews | 16:60e70bef7828 | 168 | |
zamatthews | 16:60e70bef7828 | 169 | void setThreshold(){ |
zamatthews | 16:60e70bef7828 | 170 | cam.capture(); |
zamatthews | 16:60e70bef7828 | 171 | int low = 99; |
zamatthews | 16:60e70bef7828 | 172 | int high = 0; |
zamatthews | 16:60e70bef7828 | 173 | for(int i = 0; i < 128; i++){ |
zamatthews | 16:60e70bef7828 | 174 | if(cam.imageData[i] > high) high = cam.imageData[i]; |
zamatthews | 16:60e70bef7828 | 175 | if(cam.imageData[i] < low) low = cam.imageData[i]; |
zamatthews | 16:60e70bef7828 | 176 | } |
zamatthews | 20:ebdfeb37309c | 177 | threshold = low + (high - low) * 0.35; //(high + 2 * low) / 3; |
zamatthews | 16:60e70bef7828 | 178 | } |
zamatthews | 16:60e70bef7828 | 179 | |
zamatthews | 2:0db7cc5ad6db | 180 | int main() { |
zamatthews | 16:60e70bef7828 | 181 | setThreshold(); |
zamatthews | 5:137dfb3e692f | 182 | motor_left.period_us(50); |
zamatthews | 5:137dfb3e692f | 183 | motor_right.period_us(50); |
zamatthews | 5:137dfb3e692f | 184 | DIR_R = 1; |
zamatthews | 2:0db7cc5ad6db | 185 | DIR_L = 0; |
zamatthews | 3:dadfc15fc2d1 | 186 | servo.period(0.020f); |
zamatthews | 20:ebdfeb37309c | 187 | led = 1.0; |
zamatthews | 2:0db7cc5ad6db | 188 | while(1){ |
zamatthews | 18:8dbd05e65211 | 189 | wait_ms(5); |
zamatthews | 9:644102f863a5 | 190 | cam.capture(); |
zamatthews | 12:4ccf304800fe | 191 | //display(cam.imageData); |
zamatthews | 19:25f22034a3e2 | 192 | turnWheels(cam.imageData); |
zamatthews | 19:25f22034a3e2 | 193 | setAccel(wheelPos); |
zamatthews | 20:ebdfeb37309c | 194 | if(numDarks > 15) detectStartFinish(cam.imageData); |
zamatthews | 12:4ccf304800fe | 195 | } |
zamatthews | 12:4ccf304800fe | 196 | } |