File content as of revision 5:e5ca53305b87:

#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 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
#define FACTOR 0.6 //factor*max_waarde = threshold emg
//Define objects
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 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 controlerror = 0;
float pwm = 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 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag

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

float Kp3 = 0.09; //Kp en Ki van motor1, voor de return
float Ki3 = 0.05;

volatile bool looptimerflag; //voor motorcontrol TSAMP

//functies

void setlooptimerflag(void)
{
    looptimerflag = true;

}

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()
{
    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.6); //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.55); //lage PWM
    motor1dir = 1; //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;

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

directionchoice:
    log_timer.attach(looper, TSAMP_EMG);

    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 */

    switch (direction) {
        case 1:
            setpoint2 = -1*0.436332313+0.197222205;    //25 graden + 11,3 graden, slag naar linkerdoel
            break;
        case 2:
            setpoint2 = -1*0.436332313;                //25 graden vanaf nul-punt (precies midden)
            break;
        case 3:
            setpoint2 = -1*0.436332313-0.197222205;    // 25 graden - 11,3 graden, slag naar rechterdoel
            break;
    }

    switch (force) {
        case 1:
            setpoint1 = 6.8;
            break;
        case 2:
            setpoint1 = 7.4;
            break;
        case 3:
            setpoint1 = 8.0;
            break;
    }

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

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

        keep_in_range(&pwm, -1,1);
        pwm_motor2.write(abs(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.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 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;
            pwm = Kp1*controlerror + 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(&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.10) {
            balhit = 1;
        }
    }

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;
        pwm = Kp3*controlerror + Ki3*integral;

        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.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
        controlerror = -1*motor2.getPosition()*omrekenfactor2;
        integral = integral + controlerror*TSAMP;
        pwm = Kp2*controlerror + Ki2*integral;

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