File content as of revision 11:45f345aad8ba:

#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
#define MIN_SPEED 0.2
#define MAX_SPEED 0.4

//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 turnCounter = 0;
float wheelPos = STRAIGHT;
//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 = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/4)+MIN_SPEED;  //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[]){  //int[][] frame, float speed){
    int positionSum = 0;
    int numDarks = 0;
    for(int i = 0; i < 128; i++){
        //pc.printf("%d ", cam.imageData[i]);
        if(frame[i] < 65){
            positionSum += i;
            numDarks++;
        }
    }
    //pc.printf("\n\n\r\r");
    float averagePos = 0;
    if(numDarks > 0) averagePos = positionSum / numDarks;
    float turnPercent = ((averagePos - 64.0)/64.0);
    if (turnPercent > 0) turnPercent = (1-turnPercent) * 0.4 + 0.4;
    else turnPercent = (1 + turnPercent) * -0.5 - 0.4;
    //follow right line
    //(amount from full right to straight) * (% distance black line away from middle of camera's view) + STRAIGHT
    //basically figures out how much to turn away from straight to follow line
//    float wheelPos = (FULLRIGHT - STRAIGHT) * ((averagePos - 64.0) / 64.0) + STRAIGHT;
    //stay between lines
    turnCounter++;
        if (numDarks == 0) { //no darks found, go straight
            if(turnCounter >= 10){
             wheelPos = STRAIGHT;
            } 
        }
        else { //otherwise darks found, turn away
            float newWheelPos = STRAIGHT - (FULLRIGHT - STRAIGHT) * turnPercent;
            if(((wheelPos > STRAIGHT) && (newWheelPos <= STRAIGHT)) || ((wheelPos < STRAIGHT) && (newWheelPos >= STRAIGHT))){
                if(turnCounter >= 50){
                    wheelPos = newWheelPos;
                }
            }
            else{
                wheelPos = newWheelPos;
            }
        if(wheelPos != STRAIGHT) turnCounter = 0;
        }
    turnCounter++;
    //pc.printf("averagePos: %f \n\n\r\r wheelPos: %f \n\n\r\r", averagePos, wheelPos);
    //turn/change speed
    servo.pulsewidth(wheelPos);
    setAccel(wheelPos);
    //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();
        turnWheels(cam.imageData);
        
        
        //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