File content as of revision 1:d5e81fb97eca:

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

DigitalOut  motor1_direction(D4);
PwmOut      motor1_speed(D5);
PwmOut      led(D9);
DigitalIn   button_1(PTC6); //counterclockwise
DigitalIn   button_2(PTA4); //clockwise
Encoder     motor1(D12,D13);
HIDScope    scope(1);

AnalogIn PotMeter1(A1);
Ticker controller;

// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
const double g = 360; // Aantal graden 1 rotatie
const double c = 4200; // Aantal counts 1 rotatie
const double q = c/(g);

//PI-controller constante
const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
const double motor1_Ki = 0.002; // Integrating gain m1.
const double motor1_Ts = 0.01; // Time step m1
double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1

// Reusable P controller
double Pc (double error, const double Kp)
{
    return motor1_Kp * error;
}

// Measure the error and apply output to the plant
void motor1_controlP()
{
    double referenceP1 = PotMeter1.read();
    double positionP1 = q*motor1.getPosition();
    double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
}

// Reusable PI controller
double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
{
    err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
    return motor1_Kp*error + motor1_Ki*err_int;
} // De totale fout die wordt hersteld met behulp van PI control.

void motor1_controlPI()
{
    double referencePI1 = PotMeter1.read();
    double positionPI1 = q*motor1.getPosition();
    double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
}

const int pressed = 0;

double H;
double P;
double D;

void sethome(){
    motor1.setPosition(0);
    H = motor1.getPosition();
}

void move_motor1_ccw (){
    motor1_direction = 0;
    motor1_speed = 1;
}

void move_motor1_cw (){
    motor1_direction = 1;
    motor1_speed = 1;
}

void movetohome(){
    P = motor1.getPosition();
    
    if (P >= -28 && P <= 28){
        motor1_speed = 0;
    }
    else if (P > 24){
        motor1_direction = 1;
        motor1_speed = 0.1;
    }
    else if (P < -24){
        motor1_direction = 0;
        motor1_speed = 0.1;
    }
}


void move_motor1(){
    P = motor1.getPosition();
    D = (P - H);
    
    if (button_1 == pressed) {
        move_motor1_cw ();
    }
    else if (button_2 == pressed) {
        move_motor1_ccw ();
    }
    else {
        movetohome();
    }
}


void read_encoder1 ()    // aflezen van encoder via hidscope??
{
    scope.set(0,motor1.getPosition());
    led.write(motor1.getPosition()/100.0);
    scope.send();
    wait(0.2f);
}

int main()
{

    controller.attach_us(&motor1_controlP, 1e5);
    sethome();
    while (true) {
        read_encoder1();
        move_motor1();
    }
}