File content as of revision 2:29daa03e4f62:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include <iostream>
#include "FastPWM.h"
#include "QEI.h"
#include <math.h>
//#include "HIDScope.h"


Serial pc(USBTX, USBRX);
volatile float width_percent;

float k_p = 17.5;  
float k_i = 1.02;
float k_d = 23.2;
float t_s = 0.0025; //sample time in sec


//inputs for LPF; still to fill in!
float b1;
float b2;
float b3;
float b4;
float b5;



float getError(){
    float a; //error
    cin >> a;
    return a;
    }
    
    
/*PID controller code
Proportional part general formula: u_k = k_p * e 
Integral part general formula: u_i = k_i * integral e dt
Derivative part general formula: u_d = k_d * derivative e */



/*float PIDControl(float error){
    static float error_integral = 0;
    static float error_prev = error;
    static BiQuad LPF (b1, b2, b3, b4, b5); 
    
    //proportional
    float u_k = k_p * error;
    
    //integral
    error_integral = error_integral + error * t_s;
    float u_i = k_i * error_integral;
    
    //differential
    float error_derivative = (error - error_prev) / t_s;
   // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
   // float u_d = k_d * filtered_error;
    float u_d = k_d * error_derivative; //this is very sensitive to noise, hence the LPF above
    error_prev = error;
    
    return u_k + u_i + u_d;
    }
    

*/


int main()
{
    pc.baud(115200);
    pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
    
    startmain:
    pc.printf("Please input your error.\r\n");
    float er = getError();
    pc.printf("Your error is: %f\r\n",er);
    pc.printf("-------\r\n-------\r\n");
    wait(0.5);
    goto startmain;

    
    
    //float outcome = PIDControl(error);
    //pc.printf("The outcome is %f\n",outcome);
   // pc.printf("Your input is: %f\n", error);

}