File content as of revision 7:59116528d895:

#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 = 8;
    float dt1 = 0.001;
    float Kp1 = 1.5;
    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.001;
    float Kp2 = 0.55;
    float Ki2 = 0.45;
    float error2 = 0;
    float output2 = 0;
    float omrekenfactor2 = (24*172)/6.28;

    float setpointrem1 = 0;
    float integralrem1 = 0;
    float Kprem1 = 1.5;
    float Kirem1 = 0.5;
    float errorrem1 = 0;
    float outputrem1 = 0;

    
    while(1) {
        error2 = setpoint2 - motor2.getPosition()/omrekenfactor2;
        integral2 = integral2+ error2*dt2;
        output2 = Kp2*error2 + Ki2*integral2;
        keep_in_range(&output2,-1,1);
        pwm_motor2.write(abs(output2));
        if(output2 > 0) {
            motor2dir = 1;
        } else {
            motor2dir = 0;
        }
        wait(dt2);

        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));
        if(output1 > 0) {
            motor1dir = 1;
        } else {
            motor1dir = 0;
        }
        wait(dt1);

        if(motor1.getPosition()/omrekenfactor1 < 1.3) {
            setpoint1=8;
        }

        else {
            setpoint1=0;

            /*errorrem1 = setpointrem1 - motor1.getSpeed()/omrekenfactor1;
            //motorpositie omgerekend naar rad/s
            integralrem1 = integralrem1 + errorrem1*dt1;
            outputrem1 = Kprem1*errorrem1 + Kirem1*integralrem1;
            keep_in_range(&outputrem1,-1,1);
            pwm_motor1.write(abs(outputrem1));
            if(outputrem1 > 0) {
                motor1dir = 1;
            } else {
                motor1dir = 0;
            }
            wait(dt1);*/
        }

        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;
}