Car 2: Electric Boogaloo
Fork of Car2 by
Code for an NXP Cup car using a linescan Camera
main.cpp@6:971236e48adc, 2017-03-13 (annotated)
- Committer:
- zamatthews
- Date:
- Mon Mar 13 09:40:12 2017 +0000
- Revision:
- 6:971236e48adc
- Parent:
- 5:137dfb3e692f
- Child:
- 9:644102f863a5
Added camera class to liobrary
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:dadfc15fc2d1 | 22 | |
zamatthews | 3:dadfc15fc2d1 | 23 | //int start = 0; |
zamatthews | 3:dadfc15fc2d1 | 24 | //int end = 0; |
zamatthews | 3:dadfc15fc2d1 | 25 | //int rotationTime = 0; |
zamatthews | 3:dadfc15fc2d1 | 26 | //float speed = 0; |
zamatthews | 3:dadfc15fc2d1 | 27 | //int startFinish = 0; |
zamatthews | 3:dadfc15fc2d1 | 28 | //int frame[RESWIDTH][RESHEIGHT]; |
zamatthews | 0:b761ef827157 | 29 | |
zamatthews | 3:dadfc15fc2d1 | 30 | /* |
zamatthews | 3:dadfc15fc2d1 | 31 | int frame(){ //triggered by interrupt from camera or request frame from camera? |
zamatthews | 3:dadfc15fc2d1 | 32 | //get all the pixels |
zamatthews | 3:dadfc15fc2d1 | 33 | int i = 0; |
zamatthews | 3:dadfc15fc2d1 | 34 | int j = 0; |
zamatthews | 3:dadfc15fc2d1 | 35 | for(i; i<RESWIDTH; i++){ |
zamatthews | 3:dadfc15fc2d1 | 36 | for(j; j<RESHEIGHT; j++){ |
zamatthews | 3:dadfc15fc2d1 | 37 | frame[i][j] = ???; |
zamatthews | 3:dadfc15fc2d1 | 38 | }//j for |
zamatthews | 3:dadfc15fc2d1 | 39 | }//i for |
zamatthews | 3:dadfc15fc2d1 | 40 | return frame; |
zamatthews | 3:dadfc15fc2d1 | 41 | } |
lmstthomas | 4:f4852befd69c | 42 | */ |
zamatthews | 3:dadfc15fc2d1 | 43 | |
lmstthomas | 4:f4852befd69c | 44 | void setAccel(float turnAngle){//, float speed){ |
zamatthews | 3:dadfc15fc2d1 | 45 | //ififififififif |
lmstthomas | 4:f4852befd69c | 46 | turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035 |
lmstthomas | 4:f4852befd69c | 47 | turnAngle = abs(turnAngle); |
lmstthomas | 4:f4852befd69c | 48 | float turnRatio = turnAngle/0.00035f; |
zamatthews | 5:137dfb3e692f | 49 | float newSpeed = (0.7*(1-turnRatio)/4)+0.3; |
zamatthews | 5:137dfb3e692f | 50 | motor_left.write(newSpeed); |
zamatthews | 5:137dfb3e692f | 51 | motor_right.write(newSpeed); |
zamatthews | 3:dadfc15fc2d1 | 52 | } |
zamatthews | 3:dadfc15fc2d1 | 53 | |
lmstthomas | 4:f4852befd69c | 54 | void turnWheels(){//int[][] frame, float speed){ |
zamatthews | 3:dadfc15fc2d1 | 55 | //ififififififif |
lmstthomas | 4:f4852befd69c | 56 | for(float p=FULLLEFT; p<=FULLRIGHT; p += 0.00005f) { |
lmstthomas | 4:f4852befd69c | 57 | servo.pulsewidth(p); |
lmstthomas | 4:f4852befd69c | 58 | setAccel(p); |
lmstthomas | 4:f4852befd69c | 59 | wait(.1); |
lmstthomas | 4:f4852befd69c | 60 | }//for p increase |
lmstthomas | 4:f4852befd69c | 61 | for(float p=FULLRIGHT; p>=FULLLEFT; p -= 0.00005f) { |
lmstthomas | 4:f4852befd69c | 62 | servo.pulsewidth(p); |
lmstthomas | 4:f4852befd69c | 63 | setAccel(p); |
lmstthomas | 4:f4852befd69c | 64 | wait(.1); |
lmstthomas | 4:f4852befd69c | 65 | }//for p decrease |
zamatthews | 3:dadfc15fc2d1 | 66 | } |
zamatthews | 3:dadfc15fc2d1 | 67 | |
lmstthomas | 4:f4852befd69c | 68 | |
lmstthomas | 4:f4852befd69c | 69 | /* |
zamatthews | 3:dadfc15fc2d1 | 70 | int startFinish(int[][] frame){ |
zamatthews | 3:dadfc15fc2d1 | 71 | //if start |
zamatthews | 3:dadfc15fc2d1 | 72 | return 1; |
zamatthews | 3:dadfc15fc2d1 | 73 | //if notFinished |
zamatthews | 3:dadfc15fc2d1 | 74 | return 1; |
zamatthews | 3:dadfc15fc2d1 | 75 | //if finished |
zamatthews | 3:dadfc15fc2d1 | 76 | return 0; |
zamatthews | 3:dadfc15fc2d1 | 77 | } |
zamatthews | 3:dadfc15fc2d1 | 78 | */ |
zamatthews | 2:0db7cc5ad6db | 79 | |
zamatthews | 2:0db7cc5ad6db | 80 | int main() { |
zamatthews | 3:dadfc15fc2d1 | 81 | //timer.start(); |
zamatthews | 5:137dfb3e692f | 82 | motor_left.period_us(50); |
zamatthews | 5:137dfb3e692f | 83 | motor_right.period_us(50); |
lmstthomas | 4:f4852befd69c | 84 | //motor.write(0.25); |
zamatthews | 5:137dfb3e692f | 85 | DIR_R = 1; |
zamatthews | 2:0db7cc5ad6db | 86 | DIR_L = 0; |
zamatthews | 3:dadfc15fc2d1 | 87 | servo.period(0.020f); |
lmstthomas | 4:f4852befd69c | 88 | //servo.pulsewidth(STRAIGHT); |
zamatthews | 2:0db7cc5ad6db | 89 | while(1){ |
zamatthews | 3:dadfc15fc2d1 | 90 | /* |
zamatthews | 3:dadfc15fc2d1 | 91 | if(rotation){ |
zamatthews | 3:dadfc15fc2d1 | 92 | end = time.read_ms(); |
zamatthews | 3:dadfc15fc2d1 | 93 | rotationTime = end-start; |
zamatthews | 3:dadfc15fc2d1 | 94 | speed = CIRCUFERENCE / rotationTime; //inches/ms |
zamatthews | 3:dadfc15fc2d1 | 95 | speed *= 56.8182 //convert to miles/hour |
zamatthews | 3:dadfc15fc2d1 | 96 | start = timer.read_ms(); |
zamatthews | 3:dadfc15fc2d1 | 97 | } |
zamatthews | 3:dadfc15fc2d1 | 98 | //camera.rise(&frame); //idea is that camera tells MCU that it's sending a frame |
zamatthews | 3:dadfc15fc2d1 | 99 | //frame = frame(); |
zamatthews | 3:dadfc15fc2d1 | 100 | if(startFinish(frame)){; //frame returns the result |
zamatthews | 3:dadfc15fc2d1 | 101 | */ |
lmstthomas | 4:f4852befd69c | 102 | turnWheels();//frame, speed); //includes setAccel(); |
lmstthomas | 4:f4852befd69c | 103 | //setAccel(frame, speed); |
zamatthews | 3:dadfc15fc2d1 | 104 | //} //end if(startFinish(frame)) |
zamatthews | 3:dadfc15fc2d1 | 105 | }//while 1 |
zamatthews | 3:dadfc15fc2d1 | 106 | }//main |