File content as of revision 11:0793a78109a2:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
 
Serial pc(USBTX, USBRX); // tx, rx
DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
PwmOut motor2speed(D5);
DigitalIn button1(SW3);
DigitalIn encoderA(D3);
DigitalIn encoderB(D2);
AnalogIn potmeter2(A5);
QEI Encoder(D3, D2, NC, 128);
HIDScope scope(3);
Ticker ScopeTime;
Ticker myControllerTicker;

double Aantal_Degs;
double Aantal_pulses;
double reference;
double position;

void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
{
    scope.set(0, motor2direction.read());
    scope.set(1, motor2speed.read());
    scope.set(2, Aantal_Degs);
    Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131);

    scope.send();
    
}
// Controller gain
const double m2_Kp = 0.07,m2_Ki = 0.005;
const double m2_Ts = 0.01;
double m2_err_int = 0;

// Reusable PI controller
double P( double error, const double Kp, const double Ki, double Ts, double &e_int) 
{
    e_int = e_int + Ts*error;
    return Kp * error + Ki*e_int;
}

// Next task, measure the error and apply the output to the plant
void motor2_Controller() 
{
    reference = potmeter2.read()*360;
    position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
    double P2 = P( reference - position, m2_Kp, m2_Ki, m2_Ts, m2_err_int);
    motor2speed = abs(P2);
    if(P2 > 0)
    {  
        motor2direction = 0;
    }
    else
    {
        motor2direction = 1;
    }

}

int main()
{   
    pc.baud(115200);
    pc.printf("Tot aan loop werkt\n");
    
    ScopeTime.attach_us(&ScopeSend, 10e4);
    myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
    while(true)
    {
       pc.printf("position = %f aantal degs = %f \n",reference,position);
       wait(0.2f);
    }

}