File content as of revision 1:01aa18e7c4e5:

#include "mbed.h"
#include <string>
#include <vector>
#include <iostream>

using namespace std;

Serial pc(USBTX,USBRX);
AnalogIn wheelAngleControl(PTB0);
AnalogIn leftWheelDSense(PTB1);
AnalogIn rightWheelDSense(PTB2);
PwmOut pwmWheelAngle(PTE20);
PwmOut BlueLED(LED3);

//Control System Variables
#define TI 0.02 //50Hz Sample Time
#define KP 2.6f //Proportional Gain    0.2
#define KD 0.135f //Derivative Gain    //1.6
# define M_PI           3.14159265358979323846

//All inital values and variables declared
float max=0.1f;
float min=0.05f;
float mid;
float range;
float currVal=0.0f;
float leftDist=0.5f;
float rightDist=0.5f;
float leftDat=0.0f;
float rightDat=0.0f;
float average;

std::vector<float> V;

float errorSignal;
float errorLast;
        
int state=0;  // 0=off    1=on    2=wait;

Ticker sampler;

void sample(void)
{
    leftDist=leftWheelDSense.read();
    rightDist=rightWheelDSense.read();
    average=(leftDist-rightDist);
    errorLast=errorSignal;
    errorSignal=(13.421*average+3.1051)*(1.0f/100.0f);
    V[1]=V[0];  //Store the last value
    V[0]=errorSignal;
    
          //  pc.printf("Made it erhe %f\r\n\r\n",leftDist);
}

int main() {
        
        //Control System Variables
        
        
        //All inital values and variables declared
        float max=0.1f;
        float min=0.05f;
        float mid=(max+min)/2;
        float range=max-min;
        pwmWheelAngle.period(0.02f);
        float currVal=0.0f;
        float duty=mid;
        float reference=0;
        
        V = std::vector<float>(2,0);
        
        float errorSignal = 0.0f;
        float errorCurr = 0.0f;
        float errorLast = 0.0f;
        float errorChange = 0.0f;
        float controllerOutput=M_PI/2;
        
        int state=0;  // 0=off    1=on    2=wait;
        
        //LED cycles on and off to show proper response
        BlueLED=0.9f;
        wait(0.9);
        BlueLED=1.0f;
        wait(0.9);
        BlueLED=0.9f;
        wait(0.9);
        BlueLED=1.0f;
        
        
        state=1;
        
        sampler.attach(sample,TI);
    while(1) {   //main loop that cycles forever
    
    //Import Values
    errorSignal=V[0];
    errorLast=V[1];
        
    //State check
        if(state==0){
            return 0;
        }
        else if(state==2){
            BlueLED=0.80f;
        } else {
            BlueLED=1.0f;
        }
        
    //PD Tranfer Function
        errorChange=(errorSignal-errorLast)/TI; // NO REFERNCE DERIVATIVE, NOT SURE WHAT TO PUT HERE-0.036495
        errorCurr=errorSignal-reference;
        controllerOutput=KP*errorCurr+KD*errorChange;
        
    //Output Value Derivation
        currVal=(controllerOutput*-1)+(M_PI/2);
        duty=min+((currVal/M_PI)*range);
        if(duty<min){
            duty=min;
        }
        if(duty>max){
            duty=max;
        }
        
        
    //Changes PWM signal to appropriate value (steering control)
        if(state==1){
            pwmWheelAngle.write(duty); //uses potentiometer value
            pc.printf("Duty at %f\r\n",duty);
            pc.printf("Curr Val at %f\r\n",currVal);
            pc.printf("Average is %f\r\n\r\n",V[0]*(100));
        }
        
        wait(0.001);
    }
}