File content as of revision 6:2887ad4c5d97:

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


HIDScope scope(6);

void keep_in_range(float * in, float min, float max);

int main()
{
    //motor 1, 25D
    Encoder motor1(PTD3, PTD5);
    DigitalOut motor1dir(PTC9);
    PwmOut pwm_motor1(PTC8);
    pwm_motor1.period_us(100); //10kHz PWM frequency

    //motor 2, 25D
    Encoder motor2(PTD2,PTD0);
    DigitalOut motor2dir(PTA4);
    PwmOut pwm_motor2(PTA5);
    pwm_motor2.period_us(100); //10kHz PWM frequency


    float integral1 = 0;
    float setpoint1 = 3;
    float dt1 = 0.01;
    float Kp1 = 2;
    float Ki1 = 0.5;
    float error1 = 0;
    float output1 = 0;
    float omrekenfactor1 = (32*70)/6.28;

    float integral2 = 0;
    float setpoint2 = 3.14;
    float dt2 = 0.01;
    float Kp2 = 0.55;
    float Ki2 = 0.45;
    float error2 = 0;
    float output2 = 0;
    float omrekenfactor2 = (24*172)/6.28;

    while(1) {
        if(motor1.getPosition()/omrekenfactor1 < 1.3) {
            error1 = setpoint1 - motor1.getSpeed()/omrekenfactor1;
            //motorpositie omgerekend naar rad/s
            integral1 = integral1 + error1*dt1;
            output1 = Kp1*error1 + Ki1*integral1;
            keep_in_range(&output1,-1,1);
            pwm_motor1.write(abs(output1));
            wait(dt1);
        }

        else {
            output1=0;
        }


        if(output1 > 0) {
            motor1dir = 1;
        } else {
            motor1dir = 0;
        }

        error2 = setpoint2 - motor2.getPosition()/omrekenfactor2;     // (omrekenfactor)/dt1;
        //motorpositie omgerekend naar rad/s
        integral2 = integral2+ error2*dt2;
        output2 = Kp2*error2 + Ki2*integral2;
        keep_in_range(&output2,-1,1);
        pwm_motor2.write(abs(output2));
        wait(dt2);


        if(output2 > 0) {
            motor2dir = 1;
        } else {
            motor2dir = 0;
        }
        scope.set(0, output1);
        scope.set(1, motor1.getSpeed()/omrekenfactor1);
        scope.set(2, motor1.getPosition()/(omrekenfactor1));
        scope.set(3, output2);
        scope.set(4, motor2.getSpeed()/omrekenfactor2);
        scope.set(5, motor2.getPosition()/(omrekenfactor2));
        scope.send();
    }
}
void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = max;
}