File content as of revision 8:5dab7ea40bc1:

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

#define TSAMP 0.001

volatile bool looptimerflag;
HIDScope scope(4);

void setlooptimerflag(void)
{
    looptimerflag = true;
}

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

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

float integral1 = 0;
float setpoint1 = 8; // te behalen speed van motor1 (37D)
float dt1 = 0.01;
float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
float Ki1 = 0.20;
float error1 = 0;
float pwm1 = 0;
float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)

float integral2 = 0;
float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
float dt2 = 0.01;
float Kp2 = 0.30;
float Ki2 = 0.20;
float error2 = 0;
float pwm2 = 0;
float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);

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

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

    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);

    float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
    float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd

    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
        while(!looptimerflag);
        looptimerflag = false; //clear flag

        //regelaar motor2, bepaalt positie
        error2 = setpoint2 - motor2.getPosition()*omrekenfactor2;
        integral2 = integral2 + error2*TSAMP;
        pwm2 = Kp2*error2 + Ki2*integral2;
        keep_in_range(&pwm2, -1,1);
        pwm_motor2.write(abs(pwm2));
        if(pwm2 > 0)
            motor2dir = 1;
        else
            motor2dir = 0;

        //controleert of batje positie heeft bepaald
        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 5% voor 2 seconde, dan naar volgende motorcontrol
            if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
                batjeset = 0;
            } else {
                batjeset++;
            }
        } else {
            pwm_motor2.write(0);
            goto motor1control;
        }
    }

motor1control:
    while(1) {      // loop voor het goed plaatsen van motor1 (batje snelheid)
        while(!looptimerflag);
        looptimerflag = false; //clear flag

        //regelaar motor1, bepaalt snelheid
        if (balhit == 0) {
            error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
            integral1 = integral1 + error1*TSAMP;
            pwm1 = Kp1*error1 + Ki1*integral1;
            keep_in_range(&pwm1, -1,1);
            pwm_motor1.write(abs(pwm1));
        } else {
            pwm_motor1.write(0);
            goto motorcontroldone;
        }

        if(pwm1 > 0)
            motor1dir = 1;
        else
            motor1dir = 0;

        //scope set
        scope.set(0, pwm1);
        scope.set(1, motor1.getSpeed()*omrekenfactor1);
        scope.set(2, motor1.getPosition()*omrekenfactor1);
        scope.set(3, balhit);
        scope.send();

        //controleert of batje balletje heeft bereikt
        if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) {
            balhit = 1;
        }
    }
motorcontroldone:
    wait(1);
}