Car 2: Electric Boogaloo
Fork of Car2 by
Code for an NXP Cup car using a linescan Camera
main.cpp
- Committer:
- zamatthews
- Date:
- 2017-03-13
- Revision:
- 9:644102f863a5
- Parent:
- 6:971236e48adc
- Child:
- 10:246782426144
File content as of revision 9:644102f863a5:
#include "mbed.h" #include "Servo.h" #include "Camera.h" //#define CIRCUMFERENCE SOMEINT //circumference in some unit of measurement (inches perhaps?) //#define RESWIDTH 80 //resolution width/height. change these based on actual resolution //#define RESHEIGHT 60 #define STRAIGHT 0.00095f #define FULLRIGHT 0.0013f #define FULLLEFT 0.0006 //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? //Timer timer; //DigitalIn rotation(PXXX); //camera data pins used are (D0-D7): PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11 //other camera pins: SCL->PTE5, SDA->PTE4, PCLK->PTE21, VSYNC->PTE30, H->PTE29 PwmOut servo(PTE20); PwmOut motor_left(PTA5); PwmOut motor_right(PTC8); DigitalOut DIR_L(PTD4); DigitalOut DIR_R(PTA4); Serial pc(USBTX, USBRX); Camera cam(PTE23, PTE21, PTB3); //int start = 0; //int end = 0; //int rotationTime = 0; //float speed = 0; //int startFinish = 0; void setAccel(float turnAngle){//, float speed){ //ififififififif turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035 float turnRatio = abs(turnAngle)/0.00035f; float newSpeed = (0.2*(1-turnRatio)/4)+0.15; //range of 0.15 - 0.35 motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f)); motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f)); } void turnWheels(){//int[][] frame, float speed){ //ififififififif for(float p=FULLLEFT; p<=FULLRIGHT; p += 0.00005f) { servo.pulsewidth(p); setAccel(p); wait(.1); }//for p increase for(float p=FULLRIGHT; p>=FULLLEFT; p -= 0.00005f) { servo.pulsewidth(p); setAccel(p); wait(.1); }//for p decrease } /* int startFinish(int[][] frame){ //if start return 1; //if notFinished return 1; //if finished return 0; } */ int main() { //timer.start(); motor_left.period_us(50); motor_right.period_us(50); //motor.write(0.25); DIR_R = 1; DIR_L = 0; servo.period(0.020f); //servo.pulsewidth(STRAIGHT); while(1){ wait_ms(10); cam.capture(); int positionSum = 0; int numDarks = 0; for(int i = 0; i < 128; i++){ //pc.printf("%d ", cam.imageData[i]); if(cam.imageData[i] < 60){ positionSum += i; numDarks++; } } //pc.printf("\n\n\r\r"); float averagePos; if(numDarks > 0) averagePos = positionSum / numDarks; else averagePos = 0; float wheelPos = (FULLRIGHT - STRAIGHT) * ((averagePos - 64.0) / 64.0) + STRAIGHT; servo.pulsewidth(wheelPos); setAccel(wheelPos); //pc.printf("%f\n\n\r\r", wheelPos); /* if(rotation){ end = time.read_ms(); rotationTime = end-start; speed = CIRCUFERENCE / rotationTime; //inches/ms speed *= 56.8182 //convert to miles/hour start = timer.read_ms(); } //camera.rise(&frame); //idea is that camera tells MCU that it's sending a frame //frame = frame(); if(startFinish(frame)){; //frame returns the result */ // turnWheels();//frame, speed); //includes setAccel(); //setAccel(frame, speed); //} //end if(startFinish(frame)) }//while 1 }//main