File content as of revision 5:17aa878564d0:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"

// Button and potmeter control
InterruptIn button1(D11);
InterruptIn button2(D10);
AnalogIn potmeter(A0);

// Encoder
DigitalIn encA(D13);
DigitalIn encB(D12);
QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
float Ts = 0.01;
float angle;
float omega;


// Motor
DigitalOut motor2Direction(D4);
FastPWM motor2Power(D5);
DigitalOut motor1Direction(D7);
FastPWM motor1Power(D6);

volatile int motor1Toggle = 1;

//Motorcontrol
bool motordir;
double motorpwm;
float u1;
double u2;
double potValue;
double pi2= 6.283185;
float e; //e = error
float Kp=17.5;
float Ki=1;
float Kd=21.5;
float u_k;
float u_i;
float u_d;

//Hidscope
HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
// PC connection
MODSERIAL pc(USBTX, USBRX);

// Intializing tickers
Ticker motorTicker;
Ticker controlTicker;
Ticker directionTicker;
Ticker encoderTicker;
Ticker scopeTicker;

const float PWM_period = 1e-6;

volatile int counts; // Encoder counts
volatile int countsPrev = 0;
volatile int deltaCounts;

float factorin = 6.23185/64; // Convert encoder counts to angle in rad
float gearratio = 131.25; // Gear ratio of gearbox


float PID_controller(float e){
    static float error_integral=0;
    static float e_prev=e;
    static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
    
    //Proportional part:
    u_k=Kp*e;
    
    //Integral part
    error_integral=error_integral+e*Ts;
    u_i=Ki*error_integral;
    
    //Derivative part
    float error_derivative =(e-e_prev)/Ts;
    float filtered_error_derivative = LowPassFilter.step(error_derivative);
    u_d=Kd*filtered_error_derivative;
    e_prev=e;
    
    // Sum and return
    return u_k+u_i+u_d;    
}


void readEncoder()
{
    counts = encoder.getPulses();
    deltaCounts = counts - countsPrev;

    countsPrev = counts;
}
void motorControl()
{
    angle = counts * factorin / gearratio; // Angle of motor shaft in rad
    omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    potValue= potmeter.read();
    u1= (potValue*2*pi2)-pi2;
    e=u1-angle;
    
    u2=PID_controller(e);
    
        motorpwm= abs(u2);
    if (u2<0){
        motordir= 0;}
    else {
         motordir= 1;}
    motor1Power.pulsewidth(motorpwm * PWM_period*motor1Toggle );
    motor1Direction= motordir;
}

void Plotje()
{
    scope.set(0,u1); //gewenste hoek
    scope.set(1,angle); //Gemeten hoek
    scope.set(2,e); //verschil in gewenste en gemeten hoek

    scope.send(); //send what's in scope memory to PC
}

void toggleMotor()
{
    motor1Toggle = !motor1Toggle;
}

int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    
    motor1Power.period(PWM_period);
    motorTicker.attach(motorControl, 0.01);
    scopeTicker.attach(Plotje, 0.01);
    encoderTicker.attach(readEncoder, Ts);
    
    button2.fall(&toggleMotor);
    while (true) {

        pc.printf("Potmeter: %d \r\n", potValue);
        pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
        pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
        pc.printf("U1: %f   Error:  %f     \r\n",u1, e);
        
        wait(0.5);
    }
}