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@16:60e70bef7828, 2017-03-27 (annotated)

Committer:
zamatthews
Date:
Mon Mar 27 23:36:07 2017 +0000
Revision:
16:60e70bef7828
Parent:
15:50d5cfa98425
Child:
17:846417c48571
made a blind window to give the car some room to cross the line

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 16:60e70bef7828 3 #define STRAIGHT 0.00094f
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 14:c6f0a3c4e222 9 #define MAX_SPEED 0.4
zamatthews 16:60e70bef7828 10 #define TURN_TIME 40
zamatthews 15:50d5cfa98425 11 #define STRAIGHT_TIME 15
zamatthews 16:60e70bef7828 12 #define DEFAULT_THRESHOLD 65
zamatthews 16:60e70bef7828 13 #define BLIND_LENGTH 20
zamatthews 3:dadfc15fc2d1 14
zamatthews 3:dadfc15fc2d1 15 PwmOut servo(PTE20);
zamatthews 5:137dfb3e692f 16 PwmOut motor_left(PTA5);
zamatthews 5:137dfb3e692f 17 PwmOut motor_right(PTC8);
zamatthews 3:dadfc15fc2d1 18 DigitalOut DIR_L(PTD4);
zamatthews 3:dadfc15fc2d1 19 DigitalOut DIR_R(PTA4);
zamatthews 9:644102f863a5 20 Serial pc(USBTX, USBRX);
zamatthews 9:644102f863a5 21 Camera cam(PTE23, PTE21, PTB3);
zamatthews 11:45f345aad8ba 22 int turnCounter = 0;
zamatthews 16:60e70bef7828 23 int threshold = DEFAULT_THRESHOLD;
zamatthews 11:45f345aad8ba 24 float wheelPos = STRAIGHT;
zamatthews 16:60e70bef7828 25 bool idle = false;
zamatthews 16:60e70bef7828 26 int leftBlind = 0;
zamatthews 16:60e70bef7828 27 int rightBlind = 0;
zamatthews 0:b761ef827157 28
zamatthews 12:4ccf304800fe 29 /*
zamatthews 12:4ccf304800fe 30 Function: setAccel
zamatthews 12:4ccf304800fe 31 Description: Sets the speed for the right and left motors individually based
zamatthews 12:4ccf304800fe 32 on the turning angle.
zamatthews 12:4ccf304800fe 33 */
lmstthomas 4:f4852befd69c 34 void setAccel(float turnAngle){//, float speed){
lmstthomas 4:f4852befd69c 35 turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
zamatthews 9:644102f863a5 36 float turnRatio = abs(turnAngle)/0.00035f;
zamatthews 14:c6f0a3c4e222 37 float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
zamatthews 15:50d5cfa98425 38 motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
zamatthews 15:50d5cfa98425 39 motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
zamatthews 12:4ccf304800fe 40 }//end setAccel
zamatthews 3:dadfc15fc2d1 41
zamatthews 12:4ccf304800fe 42 /*
zamatthews 12:4ccf304800fe 43 Function: turnWheels
zamatthews 12:4ccf304800fe 44 Description: Turns the wheels in order to stay between two black lines seen
zamatthews 12:4ccf304800fe 45 by the camera
zamatthews 12:4ccf304800fe 46 */
zamatthews 14:c6f0a3c4e222 47 void turnWheels(int frame[]){
zamatthews 10:246782426144 48 int positionSum = 0;
zamatthews 10:246782426144 49 int numDarks = 0;
zamatthews 10:246782426144 50 for(int i = 0; i < 128; i++){
zamatthews 16:60e70bef7828 51 if(frame[i] < threshold){
zamatthews 10:246782426144 52 positionSum += i;
zamatthews 10:246782426144 53 numDarks++;
zamatthews 10:246782426144 54 }
zamatthews 10:246782426144 55 }
zamatthews 10:246782426144 56 float averagePos = 0;
zamatthews 12:4ccf304800fe 57
zamatthews 14:c6f0a3c4e222 58 if (numDarks == 0) {
zamatthews 15:50d5cfa98425 59 if(turnCounter >= (STRAIGHT_TIME)){
zamatthews 15:50d5cfa98425 60 wheelPos = STRAIGHT;
zamatthews 15:50d5cfa98425 61 turnCounter = TURN_TIME;
zamatthews 16:60e70bef7828 62 leftBlind = 0;
zamatthews 16:60e70bef7828 63 rightBlind = 0;
zamatthews 15:50d5cfa98425 64 }
zamatthews 11:45f345aad8ba 65 }
zamatthews 12:4ccf304800fe 66
zamatthews 12:4ccf304800fe 67 else {
zamatthews 12:4ccf304800fe 68 averagePos = positionSum / numDarks;
zamatthews 16:60e70bef7828 69
zamatthews 16:60e70bef7828 70 if(((averagePos <= 64 - leftBlind)) && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
zamatthews 16:60e70bef7828 71 float powerRatio = (averagePos / (64 - leftBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
zamatthews 15:50d5cfa98425 72 powerRatio = sqrt(powerRatio);
zamatthews 12:4ccf304800fe 73 wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
zamatthews 12:4ccf304800fe 74 turnCounter = 0;
zamatthews 16:60e70bef7828 75 leftBlind = 0;
zamatthews 16:60e70bef7828 76 rightBlind = BLIND_LENGTH;
zamatthews 11:45f345aad8ba 77 }
zamatthews 12:4ccf304800fe 78
zamatthews 16:60e70bef7828 79 else if((averagePos >= 64 + rightBlind) && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
zamatthews 16:60e70bef7828 80 float powerRatio = (1 - (averagePos - 64 - rightBlind) / (64 - rightBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
zamatthews 15:50d5cfa98425 81 powerRatio = sqrt(powerRatio);
zamatthews 12:4ccf304800fe 82 wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
zamatthews 12:4ccf304800fe 83 turnCounter = 0;
zamatthews 16:60e70bef7828 84 leftBlind = BLIND_LENGTH;
zamatthews 16:60e70bef7828 85 rightBlind = 0;
zamatthews 11:45f345aad8ba 86 }
zamatthews 14:c6f0a3c4e222 87 }
zamatthews 12:4ccf304800fe 88 turnCounter++;
zamatthews 10:246782426144 89 servo.pulsewidth(wheelPos);
zamatthews 3:dadfc15fc2d1 90 }
zamatthews 3:dadfc15fc2d1 91
zamatthews 12:4ccf304800fe 92 void display(int frame[]){
zamatthews 12:4ccf304800fe 93 char draw = 'x';
zamatthews 12:4ccf304800fe 94 for(int i = 0; i< 128; i++){
zamatthews 12:4ccf304800fe 95 if (frame[i] <65) draw = '|';
zamatthews 12:4ccf304800fe 96 else draw = '-';
zamatthews 12:4ccf304800fe 97 pc.printf("%c", draw);
zamatthews 12:4ccf304800fe 98 draw = 'x';
zamatthews 12:4ccf304800fe 99 }
zamatthews 12:4ccf304800fe 100 pc.printf("\r");
zamatthews 3:dadfc15fc2d1 101 }
zamatthews 16:60e70bef7828 102
zamatthews 16:60e70bef7828 103 void setThreshold(){
zamatthews 16:60e70bef7828 104 cam.capture();
zamatthews 16:60e70bef7828 105 int low = 99;
zamatthews 16:60e70bef7828 106 int high = 0;
zamatthews 16:60e70bef7828 107 for(int i = 0; i < 128; i++){
zamatthews 16:60e70bef7828 108 if(cam.imageData[i] > high) high = cam.imageData[i];
zamatthews 16:60e70bef7828 109 if(cam.imageData[i] < low) low = cam.imageData[i];
zamatthews 16:60e70bef7828 110 }
zamatthews 16:60e70bef7828 111 threshold = (high + 2 * low) / 3;
zamatthews 16:60e70bef7828 112 }
zamatthews 16:60e70bef7828 113
zamatthews 2:0db7cc5ad6db 114 int main() {
zamatthews 16:60e70bef7828 115 setThreshold();
zamatthews 5:137dfb3e692f 116 motor_left.period_us(50);
zamatthews 5:137dfb3e692f 117 motor_right.period_us(50);
zamatthews 5:137dfb3e692f 118 DIR_R = 1;
zamatthews 2:0db7cc5ad6db 119 DIR_L = 0;
zamatthews 3:dadfc15fc2d1 120 servo.period(0.020f);
zamatthews 2:0db7cc5ad6db 121 while(1){
zamatthews 9:644102f863a5 122 wait_ms(10);
zamatthews 9:644102f863a5 123 cam.capture();
zamatthews 12:4ccf304800fe 124 //display(cam.imageData);
zamatthews 12:4ccf304800fe 125 turnWheels(cam.imageData);
zamatthews 16:60e70bef7828 126 if(!idle) setAccel(wheelPos);
zamatthews 12:4ccf304800fe 127 }
zamatthews 12:4ccf304800fe 128 }