File content as of revision 11:d31b03b05f59:

#include "mbed.h"
// #include "MODSERIAL.h"
#include "encoder.h"
#include "HIDScope.h"

// Serial pc(USBTX,USBRX);
HIDScope scope(1);

Ticker          mod;

//motor 1 gegevens
PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
DigitalOut motor1_rich(D7); // digitaal signaal voor richting
// einde motor 1

// ENCODER
Encoder motor1_enc(D12,D11);

//POTMETERS
AnalogIn potright(A0);


double setpoint;
const double K = 2 ;

// counts 2 radians
double get_radians(double counts)
{
    double pi = 3.14159265359;
    double radians = (counts/4200)*2*pi;
    return radians;
}

double setpoint_f(double input)
{
    double offset = 0.5;
    double gain = 4;
    double output = (input-offset)*gain;
    return output;
}

double K_control()
{
    double setpoint = setpoint_f(potright.read());
    double rads = get_radians(motor1_enc.getPosition());
    double error = (setpoint - rads);
    double output = K*error;
    return output;
}


void send()
{
    scope.set(0,setpoint);
    scope.send();
}

void motor1_control()
{
    double output = K_control();
    if(output > 0) {
        motor1_rich.write(0);
        motor1_aan.write(1);
    } else if(output < 0) {
        motor1_rich.write(1);
        motor1_aan.write(1);
    }
}


int main()
{
    double output ;
    mod.attach(&send,0.01);
    mod.attach(&motor1_control, 0.1);
    while(true) 
    {
    }
}