File content as of revision 0:cbabd9031b37:

#include "mbed.h"

DigitalOut gpo(D0);
DigitalOut led(LED_RED);
DigitalOut motor_dir(D7);

double Kp = 17.5;
double Ki = 1.02;
double Kd = 23.2;
double Ts = 0.01; //Sample time in seconds
    
    double GetReferenceVelocity() //Determine reference value
    {
        const float maxVelocity = 8.4;
        double referenceVelocity;
        
        if (botton1)
        {
            referenceVelocity = PotMeterIn * maxVelocity;
            }
            else
            {
                referenceVelocity = -1*PotMeterIn * maxVelocity;
                }
            return referenceVelocity;    
    
    }
    
    double GetActualPosition() // measure plant output
    {
        double pos_rad = - (pos_counts - pos_offset_counts) / (double)counts_per_rad;
        return pos_rad;
        }
    
    double PID_controller(double error)
    {
        static double error_integral = 0;
        static double error_prev = error;
        static Biquad LowpassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
        
        //P part
        double u_k = Kp * e;
        
        //I part
        error_integral = error_integral + error * Ts;
        double u_i = Ki * error_integral;
        
        //D part
        double error_derivative = (error - error_prev)/Ts;
        double filtered_error_derivatice = LowPassFilter.step(error_derivative);
        double u_d = Kd * filtered_error_derivative;
        error_prev = error;
        
        return u_k + u_i + u_d;
        }