File content as of revision 16:f4df06bf58ab:

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


#define TSAMP 0.01
#define K_P (0.1)
#define K_I (0.03  *TSAMP)
#define K_D (0.001 /TSAMP)
#define I_LIMIT 1.

#define M1_PWM PTC8
#define M1_DIR PTC9
#define M2_PWM PTA5
#define M2_DIR PTA4

//Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting

Serial pc(USBTX, USBRX);
DigitalOut led1(LED_RED);
HIDScope scope(4);

//motor 25D
Encoder motor1(PTD3,PTD5,true); //wit, geel
PwmOut pwm_motor1(M2_PWM);
DigitalOut motordir1(M2_DIR);

//motor2 37D
Encoder motor2(PTD2, PTD0,true); //wit, geel
PwmOut pwm_motor2(M1_PWM);
DigitalOut motordir2(M1_DIR);

float speed1;
float hoek1;
float speed2;
float hoek2;

bool flip=false;

void attime()
{
    flip = !flip;
}

void looper()
{
    /*motordir1=0;
    pwm_motor1.write(0.06);
    scope.set(0, motor1.getPosition());
    scope.set(1, motor1.getSpeed());
    scope.set(2, motor2.getPosition());
    scope.set(3, motor2.getSpeed());
    scope.send();*/
}

void clamp(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}

float pi(float setpoint, float measurement)
{
    float error;
    float           out_p = 0;
    static float    out_i = 0;
    error  = setpoint - measurement;
    out_p  = error*K_P;
    out_i += error*K_I;
    clamp(&out_i,-I_LIMIT,I_LIMIT);

    return out_p + out_i;
}

int main()
{
    Ticker log_timer;
    Ticker timer;
    log_timer.attach(looper, TSAMP);
    timer.attach(&attime, 7);

    //motor1
    /*speed1 = 0.7;
    hoek1 = 0.09; //in seconde
    wait(1);
    pwm_motor1.write(0.1); 
    pwm_motor1.period_us(100);
    motordir1=0;            //aangeven van directie (0 = CCW)
    pwm_motor1.write(speed1);  //snelheid van de motor
    wait(hoek1);             //Hierdoor kun je het aantal graden bepalen die de as draait
    pwm_motor1.write(0);
    wait(1);
    motordir1=1;
    pwm_motor1.write(speed1);
    wait(hoek1);
    pwm_motor1.write(0);
    wait(1);
    motordir1=1;
    pwm_motor1.write(speed1);
    wait(hoek1);
    pwm_motor1.write(0);
    wait(1);
    motordir1=0;
    pwm_motor1.write(speed1);
    wait(hoek1);
    pwm_motor1.write(0);
    */

    //motor2
    wait(1);
    speed2 = 1;
    motordir1=1;
    pwm_motor1.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
    wait(0.3);             //naar 140 graden
    pwm_motor1.write(0);    //CCW
    wait(1);
    motordir1=0;
    pwm_motor1.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
    wait(0.20);             //balletje slaan, 160 graden
    pwm_motor1.write(0);
    wait(1);
    motordir1=1;            //CW
    pwm_motor1.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
    wait(1);            //terug naar begin positie, 20 graden
    pwm_motor1.write(0); 

    
    pc.baud(115200);
    while(1) {
        wait(0.2);
        pc.printf("%d\r\n", motor1.getPosition());
    }
}