File content as of revision 20:ebdfeb37309c:

#include "mbed.h"
#include "Camera.h"
#define STRAIGHT 0.00092f
#define FULLRIGHT 0.0013f
#define FULLLEFT 0.0005
#define MIN_TURN_RATIO 0
#define MAX_TURN_RATIO 1
#define MIN_SPEED 0.17  //.15 seems to be optimal
#define MAX_SPEED 0.45  //.5
#define TURN_TIME 0
#define STRAIGHT_TIME 5
#define START_FINISH_TIME 30
#define DEFAULT_THRESHOLD 65
#define BLIND_LENGTH 30
#define DIFF_RATIO 0.5

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;
int startFinishCounter = 0;
int threshold = DEFAULT_THRESHOLD;
float wheelPos = STRAIGHT;
bool idle = true;
int leftBlind = 0;
int rightBlind = 0;
int numDarks = 0;
PwmOut led(LED_GREEN);

/*
    Function: setAccel
    Description: Sets the speed for the right and left motors individually based
                 on the turning angle. 
*/
void setAccel(float turnAngle){
    //idle = false;
    if(!idle){
        turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
        float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
        float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
        motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
        motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
    }
    else{
        motor_left.write(0);
        motor_right.write(0);
    }
}//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 positionSum = 0;
    numDarks = 0;
    for(int i = 0; i < 128; i++){
        if(frame[i] < threshold){
            positionSum += i;
            numDarks++;
        }
    }
    float averagePos = 0;

        if (numDarks == 0) {
             if(turnCounter >= (STRAIGHT_TIME)){
                 wheelPos = STRAIGHT;
                 turnCounter = TURN_TIME;
                 leftBlind = 0;
                 rightBlind = 0;
            }
        }
        
        else {
            averagePos = positionSum / numDarks;
            
            if(((averagePos <= 64 - leftBlind)) && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
                float powerRatio = (averagePos / (64 - leftBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                powerRatio = sqrt(powerRatio);
                wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
                turnCounter = 0;
                leftBlind = 0;
                rightBlind = BLIND_LENGTH;
            }
            
            else if((averagePos >= 64 + rightBlind) && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
                float powerRatio = (1 - (averagePos - 64 - rightBlind) / (64 - rightBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                powerRatio = sqrt(powerRatio);
                wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
                turnCounter = 0;
                leftBlind = BLIND_LENGTH;
                rightBlind = 0;
            }
        }
        turnCounter++;
    servo.pulsewidth(wheelPos);
}

/*
    Function: detectStartFinish
    Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
*/
void detectStartFinish(int frame[]){
    bool lookForDark = false;
    int darkBlockSize = 0;
    int darkBlocks = 0;
    int lightGap = 0;
    for(int i = 0; i < 128; i++){
        if(lookForDark){
            
            if(true){
                if(frame[i] < threshold){
                    darkBlockSize++;
                    if(darkBlockSize > 4){ //minimum size for a dark block
                        darkBlocks++;
                        lookForDark = false;
                        darkBlockSize = 0;
                    }
                }
                else if(frame[i] > threshold){
                    darkBlockSize = 0;
                }
            }
            if(!lookForDark){
                if(frame[i] > threshold){
                    lightGap++;
                }
                else{
                    if(lightGap > 20 && lightGap < 55){
                        lookForDark = true;
                        lightGap = 0;
                    }
                    else{
                        lightGap = 0;
                    }
                }
            }
        }
    }
        if(darkBlocks > 1){
            idle = !idle; //toggle idle
            startFinishCounter = 1;
            if(!idle){ 
                led = 0.0;
                servo.pulsewidth(STRAIGHT);
                wait(5);
            }
            else led = 1.0;
        }
}

void display(int frame[]){
    char draw = 'x';
    for(int i = 0; i< 128; i++){
        
        if (frame[i] < threshold) draw = '|';
        else draw = '-';
        pc.printf("%c", draw);
        draw = 'x';
    }
    pc.printf("\r");
}

void setThreshold(){
    cam.capture();
    int low = 99;
    int high = 0;
    for(int i = 0; i < 128; i++){
        if(cam.imageData[i] > high) high = cam.imageData[i];
        if(cam.imageData[i] < low) low = cam.imageData[i];
    }
    threshold = low + (high - low) * 0.35;        //(high + 2 * low) / 3;
}

int main() {    
    setThreshold();
    motor_left.period_us(50);
    motor_right.period_us(50);
    DIR_R = 1;
    DIR_L = 0;
    servo.period(0.020f);
    led = 1.0;
    while(1){
        wait_ms(5);
        cam.capture();
        //display(cam.imageData);
        turnWheels(cam.imageData);
        setAccel(wheelPos);
        if(numDarks > 15) detectStartFinish(cam.imageData);
    }
}