File content as of revision 6:f260176f704b:

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

#define TSAMP 0.005
#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 //blauw
#define M1_DIR PTC9 //groen
#define M2_PWM PTA5 //blauw
#define M2_DIR PTA4 //groen

Serial pc(USBTX, USBRX);
DigitalOut led1(LED_RED);


void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
volatile bool looptimerflag;
HIDScope scope(2);


void setlooptimerflag(void)
{
    looptimerflag = true;
}



int main()
{
    //Let op dat de jumpers goed staan als het motortje niet wilt draaien. De E1 jumper moet onder de nummer 7 pin. De locatie van de M1 pin
    //bepaalt of de motor CW of CCW draait.
    //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter

    //motor 25D
    Encoder motor1(PTD3,PTD5); //wit, geel
    PwmOut pwm_motor1(M2_PWM); //
    pwm_motor1.period_us(75); //10kHz PWM frequency
    DigitalOut motordir1(M2_DIR);

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

    //char c ='0';
    while(1) {
        wait(13);
        pwm_motor1.write(1);




        wait(0.01);
        scope.set(0, motor1.getPosition());
        scope.set(1, motor2.getPosition());
        scope.send();
        /*do {

            if(pc.readable()) {
                c = pc.getc();
            }
        } while((c !='1'));

        c = '0';
        pwm_motor1.write(0.5);
        motordir1.write(0);
        wait(3);

        }*/
    }
}