File content as of revision 13:9175be9c2b9e:

#include "mbed.h"
#include "Camera.h"
#define STRAIGHT 0.00095f
#define FULLRIGHT 0.0013f
#define FULLLEFT 0.0006
#define MIN_TURN_RATIO 0.3
#define MAX_TURN_RATIO 0.7
#define MIN_SPEED 0.1
#define MAX_SPEED 0.6
#define TURN_TIME 50

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;

/*
    Function: setAccel
    Description: Sets the speed for the right and left motors individually based
                 on the turning angle. 
*/
void setAccel(float turnAngle){//, float speed){
    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;
    motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
    motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
}//end setAccel

/*
    Function: turnWheels
    Description: Turns the wheels in order to stay between two black lines seen
                 by the camera
*/
void turnWheels(int frame[]){  //int[][] frame, float speed){
    int positionSum = 0;
    int numDarks = 0;
    for(int i = 0; i < 128; i++){
        if(frame[i] < 65){
            positionSum += i;
            numDarks++;
        }
    }
    float averagePos = 0;

        if (numDarks == 0 && turnCounter >= (TURN_TIME / 2)) {
             wheelPos = STRAIGHT;
        }
        
        else {
            averagePos = positionSum / numDarks;
            if(averagePos <= 64 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
                float powerRatio = (averagePos / 64) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                if(averagePos >= 45) powerRatio = 1;
                wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
                turnCounter = 0;
            }
            
            else if(averagePos >= 66 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
                float powerRatio = (1 - (averagePos - 64) / 64) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                if(averagePos <= 85) powerRatio = 1;
                wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
                turnCounter = 0;
            }
        turnCounter++;
        }
        
    servo.pulsewidth(wheelPos);
    setAccel(wheelPos);
}

void display(int frame[]){
    char draw = 'x';
    for(int i = 0; i< 128; i++){
        if (frame[i] <65) draw = '|';
        else draw = '-';
        pc.printf("%c", draw);
        draw = 'x';
    }
    pc.printf("\r");
}
 
int main() {    
    motor_left.period_us(50);
    motor_right.period_us(50);
    DIR_R = 1;
    DIR_L = 0;
    servo.period(0.020f);
    
    while(1){
        wait_ms(10);
        cam.capture();
        //display(cam.imageData);
        turnWheels(cam.imageData);      
        setAccel(wheelPos);
    }
}