File content as of revision 9:58f9e4f8229c:

#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

        if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
            error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
            integral1 = integral1 + error1*TSAMP;
            pwm1 = Kp1*error1 + Ki1*integral1;
        } else {            //regelaar motor1, bepaalt positie
            batjeset = integral1 = integral2 = 0;
            goto resetposition;
        }

        keep_in_range(&pwm1, -1,1);
        pwm_motor1.write(abs(pwm1));

        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) { vrij specifieke if-statement ter controle
        if (motor1.getPosition()*omrekenfactor1 > 1.10) {
            balhit = 1;
        }

    }
resetposition:

    while(1) {      // batje en slagarm worden weer in oorspronkelijke positie geplaatst
        while(!looptimerflag);
        looptimerflag = false; //clear flag

        //regelaar motor2, bepaalt positie
        error2 = 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;
        
        //regelaar motor1, bepaalt positie
        error1 = motor1.getPosition()*omrekenfactor1;
        integral1 = integral1 + error1*TSAMP;
        pwm1 = Kp2*error1 + Ki2*integral1; //kp2 en ki2 worden hier gekozen (voorlopig), goede insteling voor motor2 om goed positie te bepalen, miss ook voor motor1
        
        keep_in_range(&pwm1, -1,1);
        pwm_motor1.write(abs(pwm1));
        if(pwm1 > 0)
            motor1dir = 1;
        else
            motor1dir = 0;

        //controleert of robot terug in oorspronkelijke positie is
        if(batjeset < 200) {                            
            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03 || motor1.getPosition()*omrekenfactor1 < -0.03 || motor1.getPosition()*omrekenfactor1 >0.03) {
                batjeset = 0;
            } else {
                batjeset++;
            }
        } else {
            pwm_motor2.write(0);
            pwm_motor2.write(0);
            //direction = force = 0;
            //goto directionchoice;
            goto test;            
        }        
    }
test:
wait(2);

}