File content as of revision 5:30cf394eca48:

#include "mbed.h"
#include <iostream>
#include <math.h> 
//General
#define TI  .001f           //1kHz sample time
#define LEDOFF 0.0f         //LED low value
#define LEDON 1.0f          //LED high value
#define BATTRATIO 3.9625f   //Battery Input ratio
#define BATTCUTOFF 1.0f     //Battery Cutoff threshold
#define LOOPTIME 0.75f      //Time(s) between loops
#define ADCRATIO 0.30303f   //Ratio from port input to ADC value
#define ZERO 0.0f           //Zero
#ifndef M_PI
#define M_PI 3.14f          //Defines Pi
#endif

//Driving
#define dKP  5.1491f        //Proportional Gain
#define dKI  119.3089f      //Integral Gain
#define MOTORPWM 0.00004f   //PWM Frequency
#define MOTORMAX 0.9f       //Max Value of Buck Converter
#define MOTORMIN 0.0f       //Minimum Value of Buck Converter

//Steering
#define SERVOPWM 0.02f      //Servo Frequency
#define sKP 3.5f            //Proportional Gain
#define sKD 0.000f          //Derivative Gain was .01
#define DCINTERCEPT 0.061f  //Duty cycle intercept was 0.061
#define DCSLOPE 0.05f       //Duty cycle slope
#define TOL 0.005f          //Tolerance
#define FULLLEFT 0.1f       //PWM value of servo turning left
#define NLANDMARKS 10       //Number of Landmarks
#define OFFTRACKRIGHT 0.401f//Right Off Track Threshold was .40152
#define OFFTRACKLEFT 0.409f //Left Off Track Threshold was .4091
#define STEERINGREF -0.1f   //Centerpoint reference for steering was -0.1



//Pins
    //_p - pin
    //AI - Analog in
    //PWM - PWM out
    //DO - Digital out
    //IN - Interupt in
//General Pins
PwmOut      led_red(LED1);
PwmOut      led_blue(LED3);
PwmOut      led_green(LED2);

//Motor Pins
AnalogIn    AI_pSpeedReference(A0);
AnalogIn    AI_pTacho(A1);
PwmOut      PWM_pMotorControl(PTD0);
DigitalOut  DO_pBreakSignal(PTC9);
AnalogIn    AI_pBattMon(A4);

//Steering Pins
AnalogIn    AI_pLeftSensors(A2);
AnalogIn    AI_pRightSensors(A3);
PwmOut      PWM_pServo(PTE20);

//Binary LED
DigitalOut  DO_pFirstBinaryLED(A5);
DigitalOut  DO_pSecondBinaryLED(PTE21);
DigitalOut  DO_pThirdBinaryLED(PTD2);
DigitalOut  DO_pFourthBinaryLED(PTE31);

//Hardware Interupts
Ticker Update;
DigitalIn IN_pRunButton(PTA5);
DigitalIn IN_pStopButton(PTA4);
InterruptIn IN_pBumperStop(PTD3);

InterruptIn IN_pArrowheadLeft(PTD5);
InterruptIn IN_pArrowheadRight(PTA13);


//Data Value
    //_f - Float
    //_n - Integer
    //_b - Boolean
float   _fErrorArea;
float   _fError;
float   _fControllerOutput;
float   _fMotorReference;
float   _fFeedbackPrev;
int     _nCounter;
bool    _bArrowHitRight;
bool    _bArrowHitLeft;


//States
    //_o - object
enum States {RUN, WAIT, STOP, EMPTY};
States  _oNextState;
States  _oCurrentState;

//General Methods
void Init();
void Loop();
void UpdateMethod();
void UpdateLowBatteryStop();
void OnboardLEDColor(float, float, float);
void RunButtonMethod();
void StopButtonMethod();
void BumperStopMethod();

//Motor Methods
void UpdateDriveControl();
void UpdateSteeringControl();
void SetMotor(float);
void BreaksOn();
void BreaksOff();

//Steering Methods
void UpdateLeft();
void UpdateRight();
void UpdateBinaryCounter();


//State Methods
void TriggerSwitch();
void StopMethod();
void WaitMethod();
void RunMethod();

int main() {
    cout << "\rCarStart" << endl;
    Init();
    while(1) {wait(LOOPTIME); Loop();
    if(IN_pStopButton.read()>=.9)
        StopMethod();
    if(IN_pRunButton.read()>=.9)
        TriggerSwitch();
                 
    }
    //int r = counter; for(int b = 3; b <= 0; b--){ if(r > pow(2,b+1)){ LED[b] = LEDON; r = r - pow(2,b+1);} 
                 
}

void Init() //Initializer
{
    //Variable Init
    OnboardLEDColor(1.0f,1.0f,1.0f);
    _fError = ZERO;
    _fErrorArea = ZERO;
    _fMotorReference = ZERO;
    _fFeedbackPrev = ZERO;
    _nCounter = ZERO;
    _oNextState = EMPTY;
    _oCurrentState = EMPTY;
    _bArrowHitRight = false;
    _bArrowHitLeft = false;
    
    //Motor Init
    PWM_pMotorControl.period(MOTORPWM);
    SetMotor(MOTORMIN); //Turns off the motor
    StopMethod();
    TriggerSwitch();
    BreaksOn();
    
    //Steering Init
    PWM_pServo.period(SERVOPWM);
    
    //Interupt Init
    //IN_pRunButton.rise(&TriggerSwitch);
    //IN_pStopButton.rise(&StopMethod);
    IN_pBumperStop.rise(&BumperStopMethod);
    IN_pArrowheadLeft.rise(&UpdateLeft);
    IN_pArrowheadRight.rise(&UpdateRight);
    Update.attach(&UpdateMethod, TI);
}

void Loop() //Repeated Loop
{
    _bArrowHitRight = false;
    _bArrowHitLeft = false;
    UpdateLowBatteryStop();
}

void UpdateMethod() //
{
    UpdateDriveControl();
    UpdateSteeringControl();
} 
    
void UpdateDriveControl()
{
    if(_oCurrentState == RUN)
    {
    _fMotorReference = 0.85;//AI_pSpeedReference.read();
    _fError = _fMotorReference - AI_pTacho.read();
    _fErrorArea += TI*_fError;
    _fControllerOutput = dKP*_fError+ dKI*_fErrorArea;
    }
    SetMotor(_fControllerOutput);
}

void UpdateSteeringControl(void)
{
    if(_oCurrentState != STOP){
    //float feedbackPrev = 0;
    float feedback = AI_pLeftSensors.read() - AI_pRightSensors.read();
    float reference = STEERINGREF;
    float err = reference - feedback;
    float feedbackChange = (/*0.0f*/feedback - _fFeedbackPrev)/TI;
    float controllerOut = sKP*err + sKD*feedbackChange;
    float controllerOutNorm = (controllerOut + M_PI/2.0f)/M_PI;
    float dutyCycle = DCSLOPE*controllerOutNorm + DCINTERCEPT;
    if (abs(dutyCycle-PWM_pServo.read()) > TOL)
        PWM_pServo.write(dutyCycle);
    _fFeedbackPrev = feedback;
    if (AI_pLeftSensors.read() < OFFTRACKLEFT && AI_pRightSensors.read() < OFFTRACKRIGHT){
        StopMethod();
    }
    }
    
}

void UpdateLowBatteryStop()
{
    float fVal = ZERO;       // variable for the A/D value
    float fPinVoltage = ZERO; // variable to hold the calculated voltage
    float fBatteryVoltage = ZERO; //variable for battery voltage
    fVal = AI_pBattMon.read();    // read the voltage on the divider  
  
    fPinVoltage = fVal * ADCRATIO;       //  Calculate the voltage on the A/D pin                          
                                    
    fBatteryVoltage = fPinVoltage * BATTRATIO;    //  Use the ratio calculated for the voltage divider
                                          //  to calculate the battery voltage
    if(fBatteryVoltage < BATTCUTOFF){
      cout << "\n\rBattery Voltage is too low. Stop Method " << endl;
      StopMethod();
    } 
    else{
        cout << "\n\rBattery Voltage is: " << fBatteryVoltage << endl;
    }
    
}

void SetMotor(float speed)
{
    float invD = ZERO;
    
    if(speed < MOTORMIN) //Speed is above floor
    invD = MOTORMIN;
    else if(speed> MOTORMAX) //Caps the speed at a max value
    invD = MOTORMAX;
    else
    invD = speed;   
 
    PWM_pMotorControl.write(invD);
}

void BreaksOn()
{
    SetMotor(MOTORMIN); //Sets speed to Zero
    _fErrorArea = ZERO; //Resets Error area to zero
    DO_pBreakSignal = 1; //Turns on breaks
    PWM_pServo= FULLLEFT; //Turns servo to left
}

void BreaksOff()
{
    SetMotor(MOTORMIN); //Sets speed to Zero
    _fErrorArea = ZERO; //Resets Error area to zero
    DO_pBreakSignal = ZERO; //Turns off brakes
}

void WaitMethod()
{
    _oCurrentState = WAIT;
    BreaksOn();
    OnboardLEDColor(.125f, 1.0f, 0.125f); //Sets to Yellow
    _oNextState = RUN;
}

void RunMethod()
{
    _oCurrentState = RUN;
    BreaksOff();
    OnboardLEDColor(1.0f, 1.0f, 0.25f); //Sets to Green
    _oNextState = WAIT;
}

void StopMethod()
{   
    _oCurrentState = STOP;
    BreaksOn();
    OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
    _oNextState = WAIT;
}

void BumperStopMethod()
{   
    _oCurrentState = STOP;
    BreaksOn();
    OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
    _oNextState = WAIT;
}

void LowBatteryStopMethod()
{
    _oCurrentState = STOP;
    BreaksOn();
    OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
    _oNextState = WAIT;
}

void TriggerSwitch()
{
    switch(_oNextState){
    case STOP:
    StopMethod();
    break;
    
    case  WAIT:
    WaitMethod();
    break;
    
    case RUN:
    RunMethod();
    break;
    
    case EMPTY:
    OnboardLEDColor(0.25f, 0.25f, 0.25f); //Sets to White
    _oNextState = WAIT;
    break;
    }
}
     
void UpdateLeft(void)
{
 
    if(_nCounter >= NLANDMARKS) {
        _nCounter = 0; //Resets Counter when it reaches the max
    } else {
        _nCounter += 1;
    }
    if(_bArrowHitRight)
    {_nCounter = 0;_bArrowHitRight=!_bArrowHitRight;} //Resets Counter on double hit
    
    cout << "\n\rcounter =  " << _nCounter << endl;
    cout << "\n\rLEFT" << endl;
    
    UpdateBinaryCounter();
    _bArrowHitLeft = true;
}
    
void UpdateRight(void)
{
 
    if(_nCounter >= NLANDMARKS) {
        _nCounter = 0; //Resets Counter when it reaches the max
    } else {
        _nCounter += 1;
    }
    if(_bArrowHitLeft)
    {_nCounter = 0;_bArrowHitLeft=!_bArrowHitLeft;} //Resets Counter on double hit
    
    cout << "\n\rcounter =  " << _nCounter << endl;
    cout << "\n\rRIGHT" << endl;
    
    UpdateBinaryCounter();
    _bArrowHitRight = true;
}

void UpdateBinaryCounter()
{
    switch(_nCounter){
        case 0:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDOFF;
        break;
        case 1:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDOFF;
        break; 
        case 2:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDOFF;
        break;
        case 3:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDOFF;
        break;    
        case 4:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDOFF;
        break;
        case 5:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDOFF;
        break;    
        case 6:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDOFF;
        break;
        case 7:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDOFF;
        break;
        case 8:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 9:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 10:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 11:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 12:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 13:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 14:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDON;
        break;
        case 15:
            DO_pFirstBinaryLED = LEDON;
            DO_pSecondBinaryLED = LEDON;
            DO_pThirdBinaryLED = LEDON;
            DO_pFourthBinaryLED = LEDON;
        break;
        default:
            DO_pFirstBinaryLED = LEDOFF;
            DO_pSecondBinaryLED = LEDOFF;
            DO_pThirdBinaryLED = LEDOFF;
            DO_pFourthBinaryLED = LEDOFF;
        break;               
    } 
}

void OnboardLEDColor(float fRed= 1.0f, float fGreen = 1.0f, float fBlue = 1.0f)
{
    led_red = fRed;
    led_blue = fGreen;
    led_green = fBlue;
}