File content as of revision 0:bd171e51f222:

#include "mbed.h"
#include <string>
#include <sstream>

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.001 //1kHz Sample Time
#define KP 0.2078f //Proportional Gain
#define KD 1.6231f //Integral Gain

//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;
        
float errorSignal;
float errorLast;
        
int state=0;  // 0=off    1=on    2=wait;

Ticker sampler;

void sample()
{
    leftDist=leftWheelDSense.read();
    rightDist=rightWheelDSense.read();
    leftDat=(12.185*leftDist)-3.83;
    rightDat=(-15.77*rightDist)+4.75;
    average=(leftDat+rightDat)/2;
    errorLast=errorSignal;
    errorSignal=average;
    return;
}

int main() {
        
        //Control System Variables
        # define TI 0.001 //1kHz Sample Time
        # define KP 0.7299f //Proportional Gain
        # define KD 5.1302f //Integral Gain
        # define M_PI           3.14159265358979323846
        
        //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 leftDist=0.5f;
        float rightDist=0.5f;
        float leftDat=0.0f;
        float rightDat=0.0f;
        float duty=mid;
        float reference=0;
        
        float errorSignal = 0.0f;
        float errorCurr = 0.0f;
        float errorLast = 0.0f;
        float errorChange = 0.0f;
        sampler.attach(&sample,TI);
        float controllerOutput=3.14/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;
        
        
    while(1) {   //main loop that cycles forever
        
    //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;
        duty=min+(currVal*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
        }
        
        wait(0.0001);
    }
}