File content as of revision 0:46582bba07d2:

#include "mbed.h"
#include "QEI.h"

PwmOut Motor1_pwm(D5);
QEI Encoder(D12,D13,NC,64);
Serial pc(USBTX,USBRX);
Ticker Controller;

float Radius = 10;
float Frequency = 30;
float Multiplier = 1;
float k1 = 1;
float k2 = 0.1f;
float k3 = 0.1f;
float Start_slow = 100;
float Start_lock = 60;
float End_slow = 400;
float End_lock = 440;
float Frequency_PWM = 10000;
float Counts = 0;
float Revolutions = 0;
const double pi = 3.14159265359;

float Input_force = 0.1;
float Angle = 0;
float Curr_speed = 0;
float Desired_speed;
float Delta_speed;
float Int_delta_speed;
float Deriv_delta_speed = 0;
float Prev_delta_speed = 0;

void Slide_Controller();

int main()
{
    Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
   
    Controller.attach(&Slide_Controller,1/Frequency);
    pc.baud(9600);
    while (true) {
        Counts = Encoder.getPulses();
        Revolutions = Counts /(32*131);
        Angle = Revolutions*2*pi;
        char a = pc.getc();
        Input_force = (float)a/100;
            
        pc.printf("%f", Input_force);
        //Motor1_pwm.write(Slide_Controller(Input_force));
        
        wait(1/Frequency);
    }
    return 0;
}




float Position(float Angle){
    return Angle*Radius;
}

void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
    
    Desired_speed = Input_force*Multiplier;
    
    if (Position(Angle) < Start_slow && Desired_speed < 0){
        Desired_speed *= (Position(Angle)-Start_lock)/(Start_slow-Start_lock)*1.5-0.5;
    }
    if (Position(Angle) > End_slow && Desired_speed > 0){
        Desired_speed *= (End_lock-Position(Angle))/(End_lock-End_slow)*1.5-0.5;
    }
    Prev_delta_speed = Delta_speed;
    Delta_speed = Desired_speed-Curr_speed;                         // P
    Int_delta_speed += Delta_speed/Frequency;                       // I
    Deriv_delta_speed = (Delta_speed-Prev_delta_speed)*Frequency;   // D
    Motor1_pwm.write(k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed);
    //return k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed;
}