File content as of revision 28:1e3fe7204529:

#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

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 = 2.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 = 0.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;
}

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

motor2cal:

    //motorarm naar nul-positie
    blink.attach(kalbi, 0.4);
    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;
    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;
        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;
        wait (1);
        for1 = for2 = for3 = 0;
        kalibratie = 1;
    }

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;
                        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;
                        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;
                        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;
                        goto directionchoice;
                    } else {
                        if(bi_result>FACTOR*bi_max) {
                            force = 1;
                            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;
                        goto directionchoice;
                    } else {
                        if(bi_result>FACTOR*bi_max) {
                            force = 2;
                            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;
                        goto directionchoice;
                    } else {
                        if(bi_result>FACTOR*bi_max) {
                            force = 3;
                            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();
            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 */

    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

            switch(state) {
                case 1: 
                    switch (direction) {
                        case 1:
                            pwm_motor2.write(0.8);
                            motor2dir = 1;
                            count++;
                            if (count>170) { 
                                state=2;
                                count = 0;
                                pwm_motor2.write(0);
                            }
                            break;
                
                        case 2:
                            pwm_motor2.write(0.8);
                            motor2dir = 1;
                            count++;
                            if (count>100) {
                                state=2;
                                count = 0;
                                pwm_motor2.write(0);
                            }
                            break;
                        case 3:
                            pwm_motor2.write(0.8);
                            motor2dir = 1;
                            count++;
                            if (count>50) {
                                state=2;
                                count = 0;
                                pwm_motor2.write(0);
                            }
                            break;                         
                    }  
                case 2: {                    
                    count++;
                    if(count>1000) {
                        count = 0;
                        state = 3;
                    }
                    break;
                }
                case 3: {
                    switch (force) {
                        case 1:
                            pwm_motor1.write(1);
                            motor1dir = 1;
                            break;
                        case 2:
                            pwm_motor1.write(1); //net niet haalbaar
                            motor1dir = 1;
                            break;
                        case 3:
                            pwm_motor1.write(1); //niet haalbaar
                            motor1dir = 1;
                            break;
                    }
                    if(fabs(motor1.getPosition()*omrekenfactor1)>7.0) {
                        pwm_motor1.write(0); 
                        state = 4;
                    }
                    break;
                }
                case 4: {
                    count++;
                    if(count>4000) {
                        count = 0;
                        state = 1;
                        goto motor2cal;
                    }
                    break;
                }                
            }
        }        
    } // end main