File content as of revision 26:4afa8c5c5156:

#include "mbed.h"
#include "Camera.h"
#include "tsi_sensor.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.13
#define MAX_SPEED 0.4 
#define SPEED_TRIAL_MIN 0.25
#define SPEED_TRIAL_MAX 0.55
#define TURN_TIME 0
#define STRAIGHT_TIME 20
#define START_FINISH_TIME 60
#define DEFAULT_THRESHOLD 65
#define BLIND_LENGTH 30
#define DIFF_RATIO 0.5
#define FINISH_RANGE 30

#if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
  #define ELEC0 9
  #define ELEC1 10
#elif defined (TARGET_KL05Z)
  #define ELEC0 9
  #define ELEC1 8
#else
  #error TARGET NOT DEFINED
#endif

float turn_speed = MIN_SPEED;
float straight_speed = MAX_SPEED;
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);
TSIAnalogSlider tsi(ELEC0, ELEC1, 40);
int turnCounter = 0;
int threshold = DEFAULT_THRESHOLD;
float wheelPos = STRAIGHT;
bool idle = true;
int leftBlind = 0;
int rightBlind = 0;
float lastSlide;
int positionSum = 0;
int numDarks = 0;
int minLightGap = 1;
int maxLightGap = 55;
int minDarkBlock = 4;
int maxDarkBlock = 40;
float averagePos = 0;
int positionOffThreshold = 3;
struct darkBlock *darkBlockHead;
int noBlocks;
PwmOut led(LED_GREEN);
PwmOut redLed(LED_RED);
bool finished = false;

/*
    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 = ((straight_speed - turn_speed)*(1-turnRatio)/3)+turn_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[]){
    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);
}

/*
    deprecated
*/
struct darkBlock{
    int position;
    int length;
    int TTL; //time to live
    struct darkBlock *next;
    struct darkBlock *prev;
};

/*
    Function: detectStartFinish
    Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
*/
void detectStartFinish(){

    //idle override by touching the slider
    if(tsi.readPercentage() != lastSlide){
        idle = !idle;
        led = 1.0 - led;
        turn_speed = MIN_SPEED + (SPEED_TRIAL_MIN - MIN_SPEED) *  tsi.readPercentage();
        straight_speed = MAX_SPEED + (SPEED_TRIAL_MAX - MAX_SPEED) *  tsi.readPercentage();
        wait(0.5);
    }
    
    if(numDarks > 18 && averagePos > 63 - FINISH_RANGE && averagePos < 63 + FINISH_RANGE && noBlocks <= 2 && (!finished || !idle)){
        led = 1.0 - led;
        idle = !idle; //toggle idle
            if(!idle){ 
                servo.pulsewidth(STRAIGHT);
                wait(2);
            }
            else finished = true;
    }
}
/*
    Function: processInput
    Description: This function processes the input gathered from the camera, and calculates variables used by other parts of the car
    
*/
void processInput(int frame[]){
        positionSum = 0;
    numDarks = 0;
    bool flag = true;
    noBlocks = 0;
    for(int i = 0; i < 128; i++){
        if(frame[i] < threshold){
            positionSum += i;
            numDarks++;
            if(flag) {
                flag = false;
                noBlocks++;
            }
        }
        else flag = true;
    }
    }

/*
    Function: display
    Description: This function is used to display what the camera sees to a 
        computer.
                *Before using this function, the car should be connected to the
        computer. The car should also not be running. This is necessary because
        printing while the car is running will slow it way down and potentially
        cause it to crash.
    
*/
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(" darks %d av %d blocks %d\r", numDarks, averagePos, noBlocks);
}

/*
    Function: setThreshold
    Description: This function is used when the car first starts. It checks the 
        lightest value and the darkest value it can find, averages them and uses
        that as the light/dark threshold.
*/
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 = ((2 * low) + high) / 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;
    redLed = 1.0;
    idle = true;
    lastSlide = tsi.readPercentage();
    while(1){
        wait_ms(1);
        cam.capture();
        processInput(cam.imageData);
        detectStartFinish();
        turnWheels(cam.imageData);
        //display(cam.imageData);
        setAccel(wheelPos);
    }
}