File content as of revision 13:017aa9316f92:

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

#define TSAMP 0.001

HIDScope scope(2);
volatile bool looptimerflag;
Ticker looptimer;

void setlooptimerflag(void)
{
    looptimerflag = true;
    
}

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

//motor 1, voltage pins op M2
Encoder motor1(PTD3, PTD5);
DigitalOut motor1dir(PTC9);
PwmOut pwm_motor1(PTC8);

//motor 2,
Encoder motor2(PTD2,PTD0);
DigitalOut motor2dir(PTA4);
PwmOut pwm_motor2(PTA5);

float integral = 0;
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
float calibration = 0; //heeft er een kalibratie plaatsgevonden?

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

float Kp3 = 0.09;
float Ki3 = 0.05;

int main()
{
    looptimer.attach(setlooptimerflag,TSAMP);
    pwm_motor1.period_us(100); //10kHz PWM frequency
    pwm_motor2.period_us(100); //10kHz PWM frequency

if(calibration==0){
    //calibration motor 2
    pwm_motor2.write(0.6); //lage PWM
    motor2dir = 1;
    wait(2);                // anders wordt de while(1) meteen onderbroken 
    while(1) {
        if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
            pwm_motor2.write(0); 
            motor2.setPosition(0);
            goto motor1cal;
        } 
        wait(0.01);               
    }
motor1cal:     
    //kalibration motor 1
    pwm_motor1.write(0.55); //lage PWM
    motor1dir = 1;
    wait(2);                // anders wordt de while(1) meteen onderbroken
    while(1) {
        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
            pwm_motor1.write(0); 
            motor1.setPosition(0);
            calibration = 1;
            goto motor2control;
        }   
        scope.set(0, motor1.getSpeed()*omrekenfactor1);
        scope.send();
        wait(0.01);          
    }
}

motor2control:
    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;
        integral = integral + error2*TSAMP;
        pwm2 = Kp2*error2 + Ki2*integral;
        
        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 1% 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);
            batjeset = integral = 0;
            wait(1);
            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;
            integral = integral + error1*TSAMP;
            pwm1 = Kp1*error1 + Ki1*integral;
        } else {            //regelaar motor1, bepaalt positie
            pwm_motor1.write(0);
            balhit = integral = 0;
            wait(1); // wait voordat arm weer naar beginpositie terugkeert
            goto resetpositionmotor1;
        }

        keep_in_range(&pwm1, -1,1);        
        pwm_motor1.write(abs(pwm1));
                
        if(pwm1 > 0) {
            motor1dir = 1;
        } else {
            motor1dir = 0;
        }
        
        //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;            
        }
    }
    
resetpositionmotor1:
     while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
        while(!looptimerflag);
        looptimerflag = false; //clear flag

        //regelaar motor1, bepaalt positie
        error1 = -1*motor1.getPosition()*omrekenfactor1;
        integral = integral + error1*TSAMP;
        pwm1 = Kp3*error1 + Ki3*integral;
        
        scope.set(0, pwm1);
        scope.set(1, error1);
        scope.send();
        
        keep_in_range(&pwm1, -1,1);                 
        if(pwm1 > 0) { //HIER MOET NAAR GEKEKEN WORDEN!! de if-loop moet motor1dir op 0 zetten, maar doet dit niet, waarom?
            motor1dir = 1;
        } else {
            motor1dir = 0; //1 = rechtsom, 0 = linksom
        }
        
        pwm_motor1.write(abs(pwm1));

        //controleert of arm terug in positie is
        if(batjeset < 200) {
            if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
                batjeset = 0;
            } else {
                batjeset++;
            }
        } else {
            pwm_motor1.write(0);
            batjeset = integral = 0;
            wait(1);
            goto resetpositionmotor2;
        }
    }
    
resetpositionmotor2:
 while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
        while(!looptimerflag);
        looptimerflag = false; //clear flag

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

        //controleert of batje positie heeft bepaald
        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
                batjeset = 0;
            } else {
                batjeset++;
            }
        } else {
            pwm_motor2.write(0);
            batjeset = integral = 0;
            wait(1);
            goto test;
            //direction = force = 0;
            //goto directionchoice;
        }
    }
test:
wait(2);
}