File content as of revision 3:b17ae88bfa54:

#include "mbed.h"
#include <iostream>
#define TI  .001f       //1kHz sample time
#define dKP  5.1491f     //Proportional Gain
#define dKI  119.3089f   //Integral Gain
#define FPWM .00004f    //PWM Frequency#define TI 0.001 //1kHz sample time
#define sKP 3.50f //Proportional Gain
#define sKD 0.000f //Derivative Gain was .01
#define DCINTERCEPT 0.061f //duty cycle intercept
#define DCSLOPE 0.05f //duty cycle slope
#define TOL 0.005f               //tolerance
#ifndef M_PI
#define M_PI 3.14f
#endif

//Pins
DigitalOut myRled(LED1);
DigitalOut myBled(LED2);
DigitalOut myBled(LED2);

AnalogIn _SpeedReference(PTB0);
AnalogIn _Tacho(PTB2);
PwmOut _MotorControl(PTB1);
DigitalOut _BreakSignal(PTB3);

AnalogIn _LeftSensors(A0);
AnalogIn _RightSensors(A1);
//AnalogIn _ReferenceSignal(A2);
PwmOut _servo(PTE20);

Ticker Update;

//Integral Value 
float _errorArea;
float _error;
float _controllerOutput;
float _ref;
float _feedbackPrev;

//States
enum States {RUN, WAIT, STOP, EMPTY};
States _State;

//Methods
void Init();
void Loop();
void UpdateMethod();
void UpdateDriveControl();
void UpdateSteeringControl();
void SetMotor(float);
void BreaksOn();
void BreaksOff();

//State Methods
void WaitState();
void RunState();
void StopState();

int main() {
    Init();
    while(1) {Loop();}
}

void Init() //Initializer
{
    _MotorControl.period(FPWM);
    SetMotor(0.0f); //Turns off the motor
    _error = 0.0f;
    _errorArea = 0.0f;
    _ref = 0.0f;
    _State = STOP;
    _feedbackPrev = 0.0f;
    _servo.period(0.02);
    BreaksOff();
    Update.attach(&UpdateMethod, TI);
    //SetMotor(_ref);
}

void Loop() //Repeated Loop
{
    wait(5);
    cout<< _ref << endl;
}

void UpdateMethod() //
{
    UpdateDriveControl();
    UpdateSteeringControl();
} 
    
void UpdateDriveControl()
{
    if(_State == RUN)
    {
    _ref = _SpeedReference.read();
    _error = _ref - _Tacho.read();
    _errorArea += TI*_error;
    _controllerOutput = dKP*_error+ dKI*_errorArea;
    SetMotor(_controllerOutput);
    }
}

void UpdateSteeringControl(void)
{
    //float feedbackPrev = 0;
    float feedback = _LeftSensors.read() - _RightSensors.read();
    float reference = 0.0f;//_ReferenceSignal.read();
    float err = reference - feedback;
    float feedbackChange = (0.0f/*feedback*/ - _feedbackPrev)/TI;
    float controllerOut = sKP*err + sKD*feedbackChange;
    float controllerOutNorm = (controllerOut + M_PI/2.0f)/M_PI;
    float dutyCycle = DCSLOPE*controllerOutNorm + DCINTERCEPT;
    if (abs(dutyCycle-_servo.read()) > TOL)
        _servo.write(dutyCycle);
    _feedbackPrev = feedback;
    
}
void SetMotor(float speed)
{
    float invD = 0.0f;
    if(speed<0.0f)
    invD =0.0f;
    else if(speed> .75f)
    invD = .75f;
    else
    invD = speed;   
 
    _MotorControl.write(/*1.0f-*/invD);
}

void BreaksOn()
{
    _controllerOutput = 0;
    _errorArea = 0;
    _BreakSignal = 0;
}

void BreaksOff()
{
    _controllerOutput = 0;
    _errorArea = 0;
    _BreakSignal = 1;
}

void WaitState()
{
    _State = WAIT;
}
void RunState()
{
    _State = RUN;
}
void StopState()
{
    _State = STOP;
}