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@11:45f345aad8ba, 2017-03-15 (annotated)

Committer:
zamatthews
Date:
Wed Mar 15 23:33:30 2017 +0000
Revision:
11:45f345aad8ba
Parent:
10:246782426144
Child:
12:4ccf304800fe
howdy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zamatthews 0:b761ef827157 1 #include "mbed.h"
zamatthews 2:0db7cc5ad6db 2 #include "Servo.h"
zamatthews 6:971236e48adc 3 #include "Camera.h"
zamatthews 2:0db7cc5ad6db 4
zamatthews 3:dadfc15fc2d1 5 //#define CIRCUMFERENCE SOMEINT //circumference in some unit of measurement (inches perhaps?)
zamatthews 3:dadfc15fc2d1 6 //#define RESWIDTH 80 //resolution width/height. change these based on actual resolution
zamatthews 3:dadfc15fc2d1 7 //#define RESHEIGHT 60
lmstthomas 4:f4852befd69c 8 #define STRAIGHT 0.00095f
lmstthomas 4:f4852befd69c 9 #define FULLRIGHT 0.0013f
lmstthomas 4:f4852befd69c 10 #define FULLLEFT 0.0006
zamatthews 11:45f345aad8ba 11 #define MIN_SPEED 0.2
zamatthews 11:45f345aad8ba 12 #define MAX_SPEED 0.4
zamatthews 3:dadfc15fc2d1 13
zamatthews 3:dadfc15fc2d1 14 //InterruptIn camera(PXXX); //can set up to get a frame from the camera whenever is sends one? does the camera even have a pin for this?
zamatthews 3:dadfc15fc2d1 15 //Timer timer;
zamatthews 3:dadfc15fc2d1 16 //DigitalIn rotation(PXXX);
zamatthews 3:dadfc15fc2d1 17 //camera data pins used are (D0-D7): PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11
zamatthews 3:dadfc15fc2d1 18 //other camera pins: SCL->PTE5, SDA->PTE4, PCLK->PTE21, VSYNC->PTE30, H->PTE29
zamatthews 3:dadfc15fc2d1 19 PwmOut servo(PTE20);
zamatthews 5:137dfb3e692f 20 PwmOut motor_left(PTA5);
zamatthews 5:137dfb3e692f 21 PwmOut motor_right(PTC8);
zamatthews 3:dadfc15fc2d1 22 DigitalOut DIR_L(PTD4);
zamatthews 3:dadfc15fc2d1 23 DigitalOut DIR_R(PTA4);
zamatthews 9:644102f863a5 24 Serial pc(USBTX, USBRX);
zamatthews 9:644102f863a5 25 Camera cam(PTE23, PTE21, PTB3);
zamatthews 3:dadfc15fc2d1 26
zamatthews 11:45f345aad8ba 27 int turnCounter = 0;
zamatthews 11:45f345aad8ba 28 float wheelPos = STRAIGHT;
zamatthews 3:dadfc15fc2d1 29 //int start = 0;
zamatthews 3:dadfc15fc2d1 30 //int end = 0;
zamatthews 3:dadfc15fc2d1 31 //int rotationTime = 0;
zamatthews 3:dadfc15fc2d1 32 //float speed = 0;
zamatthews 3:dadfc15fc2d1 33 //int startFinish = 0;
zamatthews 0:b761ef827157 34
zamatthews 3:dadfc15fc2d1 35
lmstthomas 4:f4852befd69c 36 void setAccel(float turnAngle){//, float speed){
zamatthews 3:dadfc15fc2d1 37 //ififififififif
lmstthomas 4:f4852befd69c 38 turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
zamatthews 9:644102f863a5 39 float turnRatio = abs(turnAngle)/0.00035f;
zamatthews 11:45f345aad8ba 40 float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/4)+MIN_SPEED; //range of 0.15 - 0.35
zamatthews 9:644102f863a5 41 motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
zamatthews 9:644102f863a5 42 motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
zamatthews 3:dadfc15fc2d1 43 }
zamatthews 3:dadfc15fc2d1 44
zamatthews 10:246782426144 45 void turnWheels(int frame[]){ //int[][] frame, float speed){
zamatthews 10:246782426144 46 int positionSum = 0;
zamatthews 10:246782426144 47 int numDarks = 0;
zamatthews 10:246782426144 48 for(int i = 0; i < 128; i++){
zamatthews 10:246782426144 49 //pc.printf("%d ", cam.imageData[i]);
zamatthews 11:45f345aad8ba 50 if(frame[i] < 65){
zamatthews 10:246782426144 51 positionSum += i;
zamatthews 10:246782426144 52 numDarks++;
zamatthews 10:246782426144 53 }
zamatthews 10:246782426144 54 }
zamatthews 10:246782426144 55 //pc.printf("\n\n\r\r");
zamatthews 10:246782426144 56 float averagePos = 0;
zamatthews 10:246782426144 57 if(numDarks > 0) averagePos = positionSum / numDarks;
zamatthews 10:246782426144 58 float turnPercent = ((averagePos - 64.0)/64.0);
zamatthews 11:45f345aad8ba 59 if (turnPercent > 0) turnPercent = (1-turnPercent) * 0.4 + 0.4;
zamatthews 11:45f345aad8ba 60 else turnPercent = (1 + turnPercent) * -0.5 - 0.4;
zamatthews 10:246782426144 61 //follow right line
zamatthews 10:246782426144 62 //(amount from full right to straight) * (% distance black line away from middle of camera's view) + STRAIGHT
zamatthews 10:246782426144 63 //basically figures out how much to turn away from straight to follow line
zamatthews 10:246782426144 64 // float wheelPos = (FULLRIGHT - STRAIGHT) * ((averagePos - 64.0) / 64.0) + STRAIGHT;
zamatthews 10:246782426144 65 //stay between lines
zamatthews 11:45f345aad8ba 66 turnCounter++;
zamatthews 11:45f345aad8ba 67 if (numDarks == 0) { //no darks found, go straight
zamatthews 11:45f345aad8ba 68 if(turnCounter >= 10){
zamatthews 11:45f345aad8ba 69 wheelPos = STRAIGHT;
zamatthews 11:45f345aad8ba 70 }
zamatthews 11:45f345aad8ba 71 }
zamatthews 11:45f345aad8ba 72 else { //otherwise darks found, turn away
zamatthews 11:45f345aad8ba 73 float newWheelPos = STRAIGHT - (FULLRIGHT - STRAIGHT) * turnPercent;
zamatthews 11:45f345aad8ba 74 if(((wheelPos > STRAIGHT) && (newWheelPos <= STRAIGHT)) || ((wheelPos < STRAIGHT) && (newWheelPos >= STRAIGHT))){
zamatthews 11:45f345aad8ba 75 if(turnCounter >= 50){
zamatthews 11:45f345aad8ba 76 wheelPos = newWheelPos;
zamatthews 11:45f345aad8ba 77 }
zamatthews 11:45f345aad8ba 78 }
zamatthews 11:45f345aad8ba 79 else{
zamatthews 11:45f345aad8ba 80 wheelPos = newWheelPos;
zamatthews 11:45f345aad8ba 81 }
zamatthews 11:45f345aad8ba 82 if(wheelPos != STRAIGHT) turnCounter = 0;
zamatthews 11:45f345aad8ba 83 }
zamatthews 11:45f345aad8ba 84 turnCounter++;
zamatthews 10:246782426144 85 //pc.printf("averagePos: %f \n\n\r\r wheelPos: %f \n\n\r\r", averagePos, wheelPos);
zamatthews 10:246782426144 86 //turn/change speed
zamatthews 10:246782426144 87 servo.pulsewidth(wheelPos);
zamatthews 10:246782426144 88 setAccel(wheelPos);
zamatthews 3:dadfc15fc2d1 89 //ififififififif
zamatthews 10:246782426144 90 // for(float p=FULLLEFT; p<=FULLRIGHT; p += 0.00005f) {
zamatthews 10:246782426144 91 // servo.pulsewidth(p);
zamatthews 10:246782426144 92 // setAccel(p);
zamatthews 10:246782426144 93 // wait(.1);
zamatthews 10:246782426144 94 // }//for p increase
zamatthews 10:246782426144 95 // for(float p=FULLRIGHT; p>=FULLLEFT; p -= 0.00005f) {
zamatthews 10:246782426144 96 // servo.pulsewidth(p);
zamatthews 10:246782426144 97 // setAccel(p);
zamatthews 10:246782426144 98 // wait(.1);
zamatthews 10:246782426144 99 // }//for p decrease
zamatthews 3:dadfc15fc2d1 100 }
zamatthews 3:dadfc15fc2d1 101
lmstthomas 4:f4852befd69c 102
lmstthomas 4:f4852befd69c 103 /*
zamatthews 3:dadfc15fc2d1 104 int startFinish(int[][] frame){
zamatthews 3:dadfc15fc2d1 105 //if start
zamatthews 3:dadfc15fc2d1 106 return 1;
zamatthews 3:dadfc15fc2d1 107 //if notFinished
zamatthews 3:dadfc15fc2d1 108 return 1;
zamatthews 3:dadfc15fc2d1 109 //if finished
zamatthews 3:dadfc15fc2d1 110 return 0;
zamatthews 3:dadfc15fc2d1 111 }
zamatthews 3:dadfc15fc2d1 112 */
zamatthews 2:0db7cc5ad6db 113
zamatthews 2:0db7cc5ad6db 114 int main() {
zamatthews 3:dadfc15fc2d1 115 //timer.start();
zamatthews 5:137dfb3e692f 116 motor_left.period_us(50);
zamatthews 5:137dfb3e692f 117 motor_right.period_us(50);
lmstthomas 4:f4852befd69c 118 //motor.write(0.25);
zamatthews 5:137dfb3e692f 119 DIR_R = 1;
zamatthews 2:0db7cc5ad6db 120 DIR_L = 0;
zamatthews 3:dadfc15fc2d1 121 servo.period(0.020f);
lmstthomas 4:f4852befd69c 122 //servo.pulsewidth(STRAIGHT);
zamatthews 2:0db7cc5ad6db 123 while(1){
zamatthews 9:644102f863a5 124 wait_ms(10);
zamatthews 9:644102f863a5 125 cam.capture();
zamatthews 10:246782426144 126 turnWheels(cam.imageData);
zamatthews 10:246782426144 127
zamatthews 9:644102f863a5 128
zamatthews 9:644102f863a5 129 //pc.printf("%f\n\n\r\r", wheelPos);
zamatthews 9:644102f863a5 130
zamatthews 9:644102f863a5 131
zamatthews 3:dadfc15fc2d1 132 /*
zamatthews 3:dadfc15fc2d1 133 if(rotation){
zamatthews 3:dadfc15fc2d1 134 end = time.read_ms();
zamatthews 3:dadfc15fc2d1 135 rotationTime = end-start;
zamatthews 3:dadfc15fc2d1 136 speed = CIRCUFERENCE / rotationTime; //inches/ms
zamatthews 3:dadfc15fc2d1 137 speed *= 56.8182 //convert to miles/hour
zamatthews 3:dadfc15fc2d1 138 start = timer.read_ms();
zamatthews 3:dadfc15fc2d1 139 }
zamatthews 3:dadfc15fc2d1 140 //camera.rise(&frame); //idea is that camera tells MCU that it's sending a frame
zamatthews 3:dadfc15fc2d1 141 //frame = frame();
zamatthews 3:dadfc15fc2d1 142 if(startFinish(frame)){; //frame returns the result
zamatthews 3:dadfc15fc2d1 143 */
zamatthews 9:644102f863a5 144 // turnWheels();//frame, speed); //includes setAccel();
lmstthomas 4:f4852befd69c 145 //setAccel(frame, speed);
zamatthews 3:dadfc15fc2d1 146 //} //end if(startFinish(frame))
zamatthews 3:dadfc15fc2d1 147 }//while 1
zamatthews 3:dadfc15fc2d1 148 }//main