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@15:50d5cfa98425, 2017-03-27 (annotated)

Committer:
zamatthews
Date:
Mon Mar 27 00:25:38 2017 +0000
Revision:
15:50d5cfa98425
Parent:
14:c6f0a3c4e222
Child:
16:60e70bef7828
adjusted timers

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