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@9:644102f863a5, 2017-03-13 (annotated)

Committer:
zamatthews
Date:
Mon Mar 13 22:51:41 2017 +0000
Revision:
9:644102f863a5
Parent:
6:971236e48adc
Child:
10:246782426144
rough start on code;

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 3:dadfc15fc2d1 11
zamatthews 3:dadfc15fc2d1 12 //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 13 //Timer timer;
zamatthews 3:dadfc15fc2d1 14 //DigitalIn rotation(PXXX);
zamatthews 3:dadfc15fc2d1 15 //camera data pins used are (D0-D7): PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11
zamatthews 3:dadfc15fc2d1 16 //other camera pins: SCL->PTE5, SDA->PTE4, PCLK->PTE21, VSYNC->PTE30, H->PTE29
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 3:dadfc15fc2d1 24
zamatthews 3:dadfc15fc2d1 25 //int start = 0;
zamatthews 3:dadfc15fc2d1 26 //int end = 0;
zamatthews 3:dadfc15fc2d1 27 //int rotationTime = 0;
zamatthews 3:dadfc15fc2d1 28 //float speed = 0;
zamatthews 3:dadfc15fc2d1 29 //int startFinish = 0;
zamatthews 0:b761ef827157 30
zamatthews 3:dadfc15fc2d1 31
lmstthomas 4:f4852befd69c 32 void setAccel(float turnAngle){//, float speed){
zamatthews 3:dadfc15fc2d1 33 //ififififififif
lmstthomas 4:f4852befd69c 34 turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
zamatthews 9:644102f863a5 35 float turnRatio = abs(turnAngle)/0.00035f;
zamatthews 9:644102f863a5 36 float newSpeed = (0.2*(1-turnRatio)/4)+0.15; //range of 0.15 - 0.35
zamatthews 9:644102f863a5 37 motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
zamatthews 9:644102f863a5 38 motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
zamatthews 3:dadfc15fc2d1 39 }
zamatthews 3:dadfc15fc2d1 40
lmstthomas 4:f4852befd69c 41 void turnWheels(){//int[][] frame, float speed){
zamatthews 3:dadfc15fc2d1 42 //ififififififif
lmstthomas 4:f4852befd69c 43 for(float p=FULLLEFT; p<=FULLRIGHT; p += 0.00005f) {
lmstthomas 4:f4852befd69c 44 servo.pulsewidth(p);
lmstthomas 4:f4852befd69c 45 setAccel(p);
lmstthomas 4:f4852befd69c 46 wait(.1);
lmstthomas 4:f4852befd69c 47 }//for p increase
lmstthomas 4:f4852befd69c 48 for(float p=FULLRIGHT; p>=FULLLEFT; p -= 0.00005f) {
lmstthomas 4:f4852befd69c 49 servo.pulsewidth(p);
lmstthomas 4:f4852befd69c 50 setAccel(p);
lmstthomas 4:f4852befd69c 51 wait(.1);
lmstthomas 4:f4852befd69c 52 }//for p decrease
zamatthews 3:dadfc15fc2d1 53 }
zamatthews 3:dadfc15fc2d1 54
lmstthomas 4:f4852befd69c 55
lmstthomas 4:f4852befd69c 56 /*
zamatthews 3:dadfc15fc2d1 57 int startFinish(int[][] frame){
zamatthews 3:dadfc15fc2d1 58 //if start
zamatthews 3:dadfc15fc2d1 59 return 1;
zamatthews 3:dadfc15fc2d1 60 //if notFinished
zamatthews 3:dadfc15fc2d1 61 return 1;
zamatthews 3:dadfc15fc2d1 62 //if finished
zamatthews 3:dadfc15fc2d1 63 return 0;
zamatthews 3:dadfc15fc2d1 64 }
zamatthews 3:dadfc15fc2d1 65 */
zamatthews 2:0db7cc5ad6db 66
zamatthews 2:0db7cc5ad6db 67 int main() {
zamatthews 3:dadfc15fc2d1 68 //timer.start();
zamatthews 5:137dfb3e692f 69 motor_left.period_us(50);
zamatthews 5:137dfb3e692f 70 motor_right.period_us(50);
lmstthomas 4:f4852befd69c 71 //motor.write(0.25);
zamatthews 5:137dfb3e692f 72 DIR_R = 1;
zamatthews 2:0db7cc5ad6db 73 DIR_L = 0;
zamatthews 3:dadfc15fc2d1 74 servo.period(0.020f);
lmstthomas 4:f4852befd69c 75 //servo.pulsewidth(STRAIGHT);
zamatthews 2:0db7cc5ad6db 76 while(1){
zamatthews 9:644102f863a5 77 wait_ms(10);
zamatthews 9:644102f863a5 78 cam.capture();
zamatthews 9:644102f863a5 79 int positionSum = 0;
zamatthews 9:644102f863a5 80 int numDarks = 0;
zamatthews 9:644102f863a5 81 for(int i = 0; i < 128; i++){
zamatthews 9:644102f863a5 82 //pc.printf("%d ", cam.imageData[i]);
zamatthews 9:644102f863a5 83 if(cam.imageData[i] < 60){
zamatthews 9:644102f863a5 84 positionSum += i;
zamatthews 9:644102f863a5 85 numDarks++;
zamatthews 9:644102f863a5 86 }
zamatthews 9:644102f863a5 87 }
zamatthews 9:644102f863a5 88 //pc.printf("\n\n\r\r");
zamatthews 9:644102f863a5 89 float averagePos;
zamatthews 9:644102f863a5 90 if(numDarks > 0) averagePos = positionSum / numDarks;
zamatthews 9:644102f863a5 91 else averagePos = 0;
zamatthews 9:644102f863a5 92 float wheelPos = (FULLRIGHT - STRAIGHT) * ((averagePos - 64.0) / 64.0) + STRAIGHT;
zamatthews 9:644102f863a5 93 servo.pulsewidth(wheelPos);
zamatthews 9:644102f863a5 94 setAccel(wheelPos);
zamatthews 9:644102f863a5 95
zamatthews 9:644102f863a5 96 //pc.printf("%f\n\n\r\r", wheelPos);
zamatthews 9:644102f863a5 97
zamatthews 9:644102f863a5 98
zamatthews 3:dadfc15fc2d1 99 /*
zamatthews 3:dadfc15fc2d1 100 if(rotation){
zamatthews 3:dadfc15fc2d1 101 end = time.read_ms();
zamatthews 3:dadfc15fc2d1 102 rotationTime = end-start;
zamatthews 3:dadfc15fc2d1 103 speed = CIRCUFERENCE / rotationTime; //inches/ms
zamatthews 3:dadfc15fc2d1 104 speed *= 56.8182 //convert to miles/hour
zamatthews 3:dadfc15fc2d1 105 start = timer.read_ms();
zamatthews 3:dadfc15fc2d1 106 }
zamatthews 3:dadfc15fc2d1 107 //camera.rise(&frame); //idea is that camera tells MCU that it's sending a frame
zamatthews 3:dadfc15fc2d1 108 //frame = frame();
zamatthews 3:dadfc15fc2d1 109 if(startFinish(frame)){; //frame returns the result
zamatthews 3:dadfc15fc2d1 110 */
zamatthews 9:644102f863a5 111 // turnWheels();//frame, speed); //includes setAccel();
lmstthomas 4:f4852befd69c 112 //setAccel(frame, speed);
zamatthews 3:dadfc15fc2d1 113 //} //end if(startFinish(frame))
zamatthews 3:dadfc15fc2d1 114 }//while 1
zamatthews 3:dadfc15fc2d1 115 }//main