File content as of revision 12:b09b7fe5550c:

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

#define TSAMP 0.001 // sample freq encoder motor
#define TIMEB4NEXTCHOICE 1  // sec keuzelampje blijft aan
#define TIMEBETWEENBLINK 100 // sec voor volgende blink
#define TSAMP_EMG 0.002 //sample frequency emg
#define KALIBRATIONTIME 500 // 10 sec voor bepalen van maximale biceps/triceps waarde
#define FACTOR 0.6 //factor*max_waarde = threshold emg

//Define objects

HIDScope scope(5);

AnalogIn    emg0(PTB1);         //Analog input biceps
AnalogIn    emg1(PTB2);         //Analog input triceps

Ticker log_timer;       //sample emg
Ticker blink;           //ledjes aan/uit
Ticker blink2;          //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen
Ticker looptimer;       //motor regelaar

MODSERIAL pc(USBTX,USBRX);

arm_biquad_casd_df1_inst_f32 bihighpass;
float bihighpass_const[] = {0.8751821104711265,  -1.750364220942253,  0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
float bihighpass_states[4];

arm_biquad_casd_df1_inst_f32 binotch;
float binotch_const[] = {0.9714498065192796,  -1.5718388053127037,  0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
float binotch_states[4];

arm_biquad_casd_df1_inst_f32 trihighpass;
float trihighpass_const[] = {0.8751821104711265,  -1.750364220942253,  0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
float trihighpass_states[4];

arm_biquad_casd_df1_inst_f32 trinotch;
float trinotch_const[] = {0.9714498065192796,  -1.5718388053127037,  0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
float trinotch_states[4];

float bi_result = 0;
float tri_result = 0;

float bi_max = 0;
float tri_max = 0;

// variables for biceps MAF
float y0 = 0;
float y1 = 0;
float y2 = 0;
float y3 = 0;
float y4 = 0;
float y5 = 0;
float y6 = 0;
float y7 = 0;
float y8 = 0;
float y9 = 0;

// variables for triceps MAF
float x0 = 0;
float x1 = 0;
float x2 = 0;
float x3 = 0;
float x4 = 0;
float x5 = 0;
float x6 = 0;
float x7 = 0;
float x8 = 0;
float x9 = 0;

//LED interface
DigitalOut dir1(PTA1);
DigitalOut dir2(PTA2);
DigitalOut dir3(PTD4);
DigitalOut for1(PTA12);
DigitalOut for2(PTA13);
DigitalOut for3(PTD1);

uint8_t         direction = 0;
uint8_t         force = 0;

//motorcontrol objects

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

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

float integral = 0;
float derivative = 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 kalibratie = 0;
float controlerror = 0;
float previouserror = 0;
float pwm = 0;

float pwm1 =0;
float integral1 = 0;
float derivative1 = 0;
float controlerror1 = 0;
float previouserror1 = 0;

int state = 1;
int count = 0;
float angle = 0;

float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);

float setpoint1 = 0; //te behalen speed van motor1 (37D)
float setpoint2 = 0; //te behalen hoek van motor2 (25D)

float Kp1 = 12.0; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag
float Kd1 = 0.0;

float Kp2 = 8.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
float Ki2 = 0.0; //0.30 en 0.20
float Kd2 = 0.0;

volatile bool looptimerflag; //voor motorcontrol TSAMP

//functies

void setlooptimerflag(void)
{
    looptimerflag = true;
}

Ticker hid; 

void hidscope(void){
  
    scope.send();    
}

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

void looper()
{
    //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively
    float emg_biceps;         //Float voor EMG-waarde biceps
    float emg_triceps;        //Float voor EMG-waarde triceps

    emg_biceps = emg0.read();         // read float value (0..1 = 0..3.3V) biceps
    emg_triceps = emg1.read();        // read float value (0..1 = 0..3.3V) triceps

    //process emg biceps
    arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 );
    arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 );
    y0 = fabs(emg_biceps);
    bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1);
    y9=y8;
    y8=y7;
    y7=y6;
    y6=y5;
    y5=y4;
    y4=y3;
    y3=y2;
    y2=y1;
    y1=y0;

    //process emg triceps
    arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 );
    arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 );
    x0 = fabs(emg_triceps);
    tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1);
    x9=x8;
    x8=x7;
    x7=x6;
    x6=x5;
    x5=x4;
    x4=x3;
    x3=x2;
    x2=x1;
    x1=x0;
}

void kalbi() //blinking three lights, first row - 2nd row unlit
{
    if(dir1==0) {
        dir1 = dir2 = dir3 = 1;
    } else    {
        dir1 = dir2 = dir3 = 0;
    }
}

void kaltri() //blinking three lights, 2nd row - first row lit
{
    if(for1==0) {
        for1 = for2 = for3 = 1;
    } else    {
        for1 = for2 = for3 = 0;
    }
}

void okay()  //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P)
{
    if(direction == 1 && force == 1) {            // links zwak
        if(for1 == 0 && dir1 == 0) {
            for1 = dir1 = 1;
        } else    {
            for1 = dir1 = 0;
        }
    }
    if(direction == 1 && force == 2) {            // links normaal
        if(for2 == 0 && dir1 == 0) {
            for2 = dir1 = 1;
        } else    {
            for2 = dir1 = 0;
        }
    }
    if(direction == 1 && force == 3) {            // links sterk
        if(for3 == 0 && dir1 == 0) {
            for3 = dir1 = 1;
        } else    {
            for3 = dir1 = 0;
        }
    }
    if(direction == 2 && force == 1) {            // mid zwak
        if(for1 == 0 && dir2 == 0) {
            for1 = dir2 = 1;
        } else    {
            for1 = dir2 = 0;
        }
    }
    if(direction == 2 && force == 2) {            // mid normaal
        if(for2 == 0 && dir2 == 0) {
            for2 = dir2 = 1;
        } else    {
            for2 = dir2 = 0;
        }
    }
    if(direction == 2 && force == 3) {            // mid sterk
        if(for3 == 0 && dir2 == 0) {
            for3 = dir2 = 1;
        } else    {
            for3 = dir2 = 0;
        }
    }
    if(direction == 3 && force == 1) {            // rechts zwak
        if(for1 == 0 && dir3 == 0) {
            for1 = dir3 = 1;
        } else    {
            for1 = dir3 = 0;
        }
    }
    if(direction == 3 && force == 2) {            // rechts normaal
        if(for2 == 0 && dir3 == 0) {
            for2 = dir3 = 1;
        } else    {
            for2 = dir3 = 0;
        }
    }
    if(direction == 3 && force == 3) {            // rechts sterk
        if(for3 == 0 && dir3 == 0) {
            for3 = dir3 = 1;
        } else    {
            for3 = dir3 = 0;
        }
    }
}

int main()
{
    hid.attach(hidscope, 0.01);
    pc.baud(115200);                        //baudrate instellen
    log_timer.attach(looper, TSAMP_EMG);    //EMG, Fsample 500 Hz
    looptimer.attach(setlooptimerflag,TSAMP);
    pwm_motor1.period_us(100);              //10kHz PWM frequency
    pwm_motor2.period_us(100);              //10kHz PWM frequency

    //set up filters
    arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states);
    arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states);

    arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states);
    arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states);

    //kalibratie

    //motorarm naar nul-positie
    blink.attach(kalbi, 0.2);
    blink2.attach(kaltri, 0.2);

    //calibration motor 2
    pwm_motor2.write(0.65); //lage PWM
    motor2dir = 0; //rechtsom
    wait(1);                // anders wordt de while(1) meteen onderbroken
    while(1) {
        if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // motor2.getSpeed()*omrekenfactor2 > -0.70), 0.70 is nog aan te passen
            pwm_motor2.write(0);
            motor2.setPosition(0);
            goto motor1cal;
        }
        wait(0.01);
    }
motor1cal:
    //calibration motor 1
    pwm_motor1.write(0.65); //lage PWM
    motor1dir = 0; //linksom
    wait(1);                // anders wordt de while(1) meteen onderbroken
    while(1) {
        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen
            pwm_motor1.write(0);
            motor1.setPosition(0);
            goto emgcal;
        }
        wait(0.01);
    }
emgcal:
    blink.detach();
    blink2.detach();
    dir1 = dir2 = dir3 = 1;
    for1 = for2 = for3 = 1;
    pc.printf("kalmoarm ");
    wait (1);
    for1 = for2 = for3 = 0;

    if(kalibratie==0) {
        //biceps kalibratie
        blink.attach(kalbi, 0.2);
        for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
            if (bi_max < bi_result) {
                bi_max = bi_result;
            }
            wait (0.01);
        }
        blink.detach();
        dir1 = dir2 = dir3 = 1;
        pc.printf("kalbi ");
        wait (1);

        //triceps kalibratie
        blink.attach(kaltri, 0.2);
        for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
            if (tri_max < tri_result) {
                tri_max = tri_result;
            }
            wait (0.01);
        }
        blink.detach();
        for1 = for2 = for3 = 1;
        pc.printf("kaltri ");
        wait (1);
        for1 = for2 = for3 = 0;
        kalibratie = 1;
    }

directionchoice:
    log_timer.attach(looper, TSAMP_EMG);
    direction = 1;
    force = 1;
    goto motorcontrol;

    while(1) { //Loop keuze DIRECTION
        for(int i=1; i<4; i++) {
            if(i==1) {           //red
                dir1=1;
                dir2=0;
                dir3=0;
                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                    if(bi_result>FACTOR*bi_max) {
                        direction = 1;
                        pc.printf("links ");
                        wait(TIMEB4NEXTCHOICE);                 // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
                        goto forcechoice;                       // goes to second while(1) for the deciding the force
                    } else {
                        wait(0.01);
                    }
                }
            }
            if(i==2) {           //green
                dir1 =0;
                dir2 =1;
                dir3 =0;
                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                    if(bi_result>FACTOR*bi_max) {
                        direction = 2;
                        pc.printf("mid ");
                        wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
                        goto forcechoice;
                    } else {
                        wait(0.01);
                    }
                }
            }
            if(i==3) {           //blue
                dir1 =0;
                dir2 =0;
                dir3 =1;
                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                    if(bi_result>FACTOR*bi_max) {
                        direction = 3;
                        pc.printf("rechts ");
                        wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
                        goto forcechoice;
                    } else {
                        wait(0.01);
                    }
                }
            }
        }
    }
forcechoice:
    while(1) { //Loop keuze FORCE
        for(int j=1; j<4; j++) {
            if(j==1) {           //red
                for1=1;
                for2=0;
                for3=0;
                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                    if(tri_result>FACTOR*tri_max) {
                        for1 = for2 = for3 = 0;
                        pc.printf("reset ");
                        goto directionchoice;
                    } else {
                        if(bi_result>FACTOR*bi_max) {
                            force = 1;
                            pc.printf("zwak ");
                            wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
                            goto choicesmade;
                        } else {
                            wait(0.01);
                        }
                    }
                }
            }
            if(j==2) {           //green
                for1=0;
                for2=1;
                for3=0;
                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                    if(tri_result>FACTOR*tri_max) {
                        for1 = for2 = for3 = 0;
                        pc.printf("reset ");
                        goto directionchoice;
                    } else {
                        if(bi_result>FACTOR*bi_max) {
                            force = 2;
                            pc.printf("normaal ");
                            wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
                            goto choicesmade;
                        } else {
                            wait(0.01);
                        }
                    }
                }
            }
            if(j==3) {           //blue
                for1=0;
                for2=0;
                for3=1;
                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
                    if(tri_result>FACTOR*tri_max) {
                        for1 = for2 = for3 = 0;
                        pc.printf("reset ");
                        goto directionchoice;
                    } else {
                        if(bi_result>FACTOR*bi_max) {
                            force = 3;
                            pc.printf("sterk ");
                            wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
                            goto choicesmade;
                        } else {
                            wait(0.01);
                        }
                    }
                }
            }
        }
    }

choicesmade:
    blink.attach(okay, 0.2);
    while(1) {
        if(tri_result>FACTOR*tri_max) {
            blink.detach();
            pc.printf("reset ");
            switch (direction) {
                case 1:
                    dir1 = 1;
                    for1 = 1;
                    for2 = for3 = 0;
                    break;
                case 2:
                    dir2 = 1;
                    for1 = 1;
                    for2 = for3 = 0;
                    break;
                case 3:
                    dir3 = 1;
                    for1 = 1;
                    for2 = for3 = 0;
                    break;
            }

            wait(1);                                // 1 sec wait, anders reset je meteen ook de biceps keuze
            goto forcechoice;
        } else {
            if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
                blink.detach();
                log_timer.detach();
                goto motorcontrol;
            } else {
                wait(0.01);                          // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
            }
        }
    }

motorcontrol:

    /* Vanaf hier komt de aansturing van de motor */


// FORMAT_CODE_START
    setpoint1=0;
    setpoint2=0;
    integral1 = integral = 0;
    previouserror1 = previouserror = 0;


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

            scope.set(0, motor2.getPosition()*omrekenfactor2);
            scope.set(1, setpoint2);
            scope.set(2, motor1.getPosition()*omrekenfactor1);
            scope.set(3, setpoint1);
            scope.set(4, state);

            switch(state) {
                case 1: {
                    setpoint1=0;
                    setpoint2 += 0.4*TSAMP;
                    switch (direction) {
                        case 1:
                            angle = 0.436332313+0.197222205;  //(0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
                            break;
                        case 2:
                            angle = 0.436332313;
                            break;
                        case 3:
                            angle = 0.436332313-0.197222205;
                            break;
                    }
                    if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1
                        setpoint2 = angle;
                        count = 0;
                        state=2;
                    }
                    /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) {
                        state = 2;
                    }*/
                    break;
                }
                case 2: {
                    setpoint1 = 0;
                    count++;
                    if(count>1000) {
                        count = 0;
                        state = 3;
                    }
                    break;
                }
                case 3: {
                    switch (force) {
                        case 1:
                            setpoint1 += 2.5*TSAMP; //6.8*TSAMP;
                            break;
                        case 2:
                            setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
                            break;
                        case 3:
                            setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
                            break;
                    }
                    if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
                        setpoint1 = 2.36;
                        state = 4;
                    }
                    break;
                }
                case 4: {
                    setpoint2 -= 0.25*TSAMP;
                    if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm
                        state = 5;
                    }
                    break;
                }
                case 5: {
                    setpoint1 -= 0.5*TSAMP;
                    if(setpoint1 < 0) {
                        state = 6;
                    }
                    break;
                }
                case 6: {
                    setpoint1 = 0;
                    count++;
                    if(count>3000) {
                        count = 0;
                        state = 1;
                        goto directionchoice;
                    }
                    break;
                }
            }

            //motor regeling

            //regelaar motor1, bepaalt positie
            controlerror1 = setpoint1 - (motor1.getPosition()*omrekenfactor1);
            integral1 = integral1 + (controlerror1*TSAMP);
            derivative1 = (controlerror1 - previouserror1)/TSAMP;
            pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
            previouserror1 = controlerror1;

            keep_in_range(&pwm1, -1,1);
            pwm_motor1.write(fabs(pwm1));
            if(pwm1 > 0) {
                motor1dir = 1;
            } else {
                motor1dir = 0;
            }


            //regelaar motor2, bepaalt positie
            controlerror = setpoint2 - (motor2.getPosition()*omrekenfactor2);
            integral = integral + (controlerror*TSAMP);
            derivative = (controlerror - previouserror)/TSAMP;
            pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
            previouserror = controlerror;

            keep_in_range(&pwm, -1,1);
            pwm_motor2.write(fabs(pwm));
            if(pwm > 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.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
                            batjeset = 0;
                        } else {
                            batjeset++;
                        }
                    } else {
                        pwm_motor2.write(0);
                        batjeset = integral = derivative = previouserror = 0;
                        wait(1);
                        //goto motor1control;
                    }
            */
        }
        /*
        motor1control:
            while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
                while(!looptimerflag);
                looptimerflag = false; //clear flag

                if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
                    controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
                    integral = integral + controlerror*TSAMP;
                    derivative = (controlerror - previouserror)/TSAMP;
                    pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
                    previouserror = controlerror;
                } else {            //regelaar motor1, bepaalt positie
                    balhit = integral = derivative = previouserror = 0;
                    goto resetpositionmotor1;
                }

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

                if(pwm > 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.60) {
                    balhit = 1;
                }
            }
        // FORMAT_CODE_END

        resetpositionmotor1:
            while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
                while(!looptimerflag);
                looptimerflag = false; //clear flag

                //regelaar motor1, bepaalt positie
                controlerror = -1*motor1.getPosition()*omrekenfactor1;
                integral = integral + controlerror*TSAMP;
                derivative = (controlerror - previouserror)/TSAMP;
                pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
                previouserror = controlerror;

                keep_in_range(&pwm, -1,1);
                if(pwm > 0) {
                    motor1dir = 1;
                } else {
                    motor1dir = 0; //1 = rechtsom, 0 = linksom
                }

                pwm_motor1.write(abs(pwm));

                //controleert of arm terug in positie is
                if(batjeset < 200) {
                    if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
                        batjeset = 0;
                    } else {
                        batjeset++;
                    }
                } else {
                    pwm_motor1.write(0);
                    batjeset = integral = derivative = previouserror = 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
                controlerror = -1*motor2.getPosition()*omrekenfactor2;
                integral = integral + controlerror*TSAMP;
                derivative = (controlerror - previouserror)/TSAMP;
                pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
                previouserror = controlerror;

                keep_in_range(&pwm, -1,1);

                if(pwm > 0) {
                    motor2dir = 1;
                } else {
                    motor2dir = 0;
                }

                pwm_motor2.write(abs(pwm));

                //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.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
                        batjeset = 0;
                    } else {
                        batjeset++;
                    }
                } else {
                    pwm_motor2.write(0);
                    batjeset = integral = derivative = previouserror = 0;
                    wait(1);
                    direction = force = 0;
                    goto motor1cal;
                }
            }*/
    } // end main