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@18:8dbd05e65211, 2017-04-02 (annotated)

Committer:
zamatthews
Date:
Sun Apr 02 23:04:56 2017 +0000
Revision:
18:8dbd05e65211
Parent:
17:846417c48571
Child:
19:25f22034a3e2
final revision for now

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