File content as of revision 0:b781e625aca4:

// reads input voltage from distance measurement circuit and outputs to console

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

//analog inputs to the microcontroller
AnalogIn Lin(PTB2);
AnalogIn Rin(PTB1);
//pwm output to servo
PwmOut output(PTB3);

// ticker to sample error
const float ti = 2e-2;
const float PI = 3.1415926535;
Ticker updater;

//initializing variables
float vL = 0;
float vR = 0;
float out = 0;
float turn_angle = 0;
float err = 0; 
float prev_error = 0;
float del_error = 0;
int i = 0;
float sample_count = 20;

float sum = 0;

// turning constants
const float kp = 36.8;
const float kd = 2.128;
const float full_left = .059f; // duty cycle for full left turn
const float full_right = .091f; // duty cycle for full right turn
float d = full_left;

void pwmUpdate()
{
    prev_error = err; 
    vL = Lin.read(); 
    vR = Rin.read();
    
    err = -(vL-vR) ; // voltage error
    del_error = (err - prev_error)/ti; //change in voltage error
    turn_angle = kp*err + kd*del_error; //calculates turn angle
    if(turn_angle < 0) 
        turn_angle = 0; 
    if(turn_angle > PI) 
        turn_angle = PI;  
    d= (turn_angle/PI) * (full_right - full_left) + full_left;
    if( abs(d - output.read()) > 0.01) //only updates the duty cycle if there is a change in the duty cycle greater than 0.3%
        {
         output.write(d);
         }
     cout << "del error: " << del_error << "\r" << std::endl;
    
}



int main(void)
{
    output.period(0.02f); // 50 Hz control signal
    // attaches pwmUpdate function to the ticker with a period of 20ms
    updater.attach(&pwmUpdate,ti);
    
    while(1) {} // do nothing
}