File content as of revision 23:6e1e142b7baf:

#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.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

#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


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

/*
    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 lightGapSize = 0;
    int minLightGap = 1;
    int maxLightGap = 55;
    int minDarkBlock = 4;
    int maxDarkBlock = 40;
    if(tsi.readPercentage() != lastSlide){
        idle = !idle;
        led = 0.0;
        wait(0.5);
    }
    if(numDarks <= 15) return;
    for(int i = 0; i < 128; i++){
        if(lookForDark){
            if(frame[i] < threshold){
                darkBlockSize++;
            }
            else{
                if(darkBlockSize > minDarkBlock && darkBlockSize < maxDarkBlock){
                    darkBlocks++;
                    lookForDark = false;
                    darkBlockSize = 0;
                    lightGapSize = 1;
                }
                else{
                    darkBlockSize = 0;
                }
            }
        }
        if(!lookForDark){
            if(frame[i] > threshold){
                lightGapSize++;
            }
            else{
                if(lightGapSize > minLightGap && lightGapSize < maxLightGap){
                    lookForDark = true;
                    minLightGap = 20;
                    lightGapSize = 0;
                    darkBlockSize = 1;
                }
                else{
                    lightGapSize = 0;
                }
            }
        }
    }
        if(darkBlocks > 1){
            idle = !idle; //toggle idle
            startFinishCounter = 1;
            if(!idle){ 
                led = 1.0 - led;
                servo.pulsewidth(STRAIGHT);
                wait(5);
            }
            else led = 1.0;
        }
}

/*
    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("\r");
}

/*
    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 = low + (high - low) * 0.35;        //(high + 2 * low) / 3;
    threshold = (low + high) / 2;
}

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(5);
        cam.capture();
        //display(cam.imageData);
        turnWheels(cam.imageData);
        setAccel(wheelPos);
        detectStartFinish(cam.imageData);
    }
}