File content as of revision 15:3ebd0e666a9c:

#include "mbed.h"           //Mbed bibliotheek inladen, standaard functies
#include "MODSERIAL.h"      //MODSERIAL bibliotheek inladen, communicatie met PC
#include "encoder.h"        //Encoder bibliotheek inladen, communicatie met encoder
#include "TextLCD.h"        //LCD scherm bibliotheek inladen, communicatie met LCD scherm
#include "arm_math.h"       //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst

//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR         0.005       //Sampletijd ticker regelaar motor
#define KP_arm1                     0.01       //Factor proprotionele regelaar arm 1
#define KI_arm1                     0         //Factor integraal regelaar arm 1
#define KD_arm1                     0        //Factor afgeleide regelaar arm 1
#define KP_arm2                     0.01       //Factor proprotionele regelaar arm 2
#define KI_arm2                     0           //Factor integraal regelaar arm 2
#define KD_arm2                     0          //Factor afgeleide regelaar arm 2
#define SAMPLETIME_EMG              0.005       //Sampletijd ticker EMG meten
#define PULS_ARM1_HOME_KALIBRATIE   180         //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
#define PULS_ARM2_HOME_KALIBRATIE   393         //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft

//High Pass filter          Filtercoëfficiënten
#define A1 1
#define A2 -1.5610
#define A3 0.6414
#define B1 0.0201
#define B2 0.0402
#define B3 0.0201

//Notch filter          Filtercoëfficiënten
#define C1 1
#define C2 -1.1873E-16
#define C3 0.9391
#define D1 0.9695
#define D2 -1.1873E-16
#define D3 0.9695

//Low pass filter           Filtercoëfficiënten
#define E1 1
#define E2 -1.9645
#define E3 0.9651
#define F1 1.5515E-4
#define F2 3.1030E-4
#define F3 1.5515E-4


//Pinverdeling en naamgeving variabelen
TextLCD         lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2);      //LCD scherm
MODSERIAL       pc(USBTX, USBRX);                               //PC

PwmOut pwm_motor_arm1(PTA5);            //PWM naar motor arm 1
DigitalOut dir_motor_arm1(PTA4);        //Richting van motor arm 1
Encoder puls_motor_arm1(PTD0, PTD2);    //Encoder pulsen tellen van motor arm 1, (geel, wit)
PwmOut pwm_motor_arm2(PTC8);            //PWM naar motor arm 2
DigitalOut dir_motor_arm2(PTC9);        //Ricting van motor arm 2
Encoder puls_motor_arm2(PTD5, PTA13);   //Encoder pulsen tellen van motor arm 2, (geel, wit)
AnalogIn EMG_bi(PTB1);                  //Meten EMG signaal biceps
AnalogIn EMG_tri(PTB2);
//Blauw op 3,3 V en groen op GND

Ticker ticker_regelaar;                 //Ticker voor regelaar motor
Ticker ticker_EMG;                      //Ticker voor EMG meten
Timer biceps_kalibratie;
Timer triceps_kalibratie;

//States definiëren
enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, BEGIN_METEN, B, T};     //Alle states benoemen, ieder krijgt een getal beginnend met 0
uint8_t state = RUST;                   //State is rust aan het begin

//Gedefinieerde datatypen en naamgeving en beginwaarden
char *lcd_r1 = new char[16];            //Char voor tekst op eerste regel LCD scherm
char *lcd_r2 = new char[16];            //Char voor tekst op tweede regel LCD scherm

float pwm_to_motor_arm1;                //PWM output naar motor arm 1
float pwm_to_motor_arm2;                //PWM output naar motor arm 2
float error_arm1_kalibratie;            //Error in pulsen arm 1
float vorige_error_arm1 = 0;            //Derivative actie van regelaar arm 1
float integraal_arm1 = 0;               //Integraal actie van regelaar arm 1
float afgeleide_arm1;                   //Afgeleide actie van regelaar arm 1
float error_arm2_kalibratie;            //Error in pulsen arm 2
float vorige_error_arm2 = 0;            //Derivative actie van regelaar arm 2
float integraal_arm2 = 0;               //Integraal actie van regelaar arm 2
float afgeleide_arm2;                   //Afgeleide actie van regelaar arm 2
float xb;                               //Gemeten EMG waarde biceps
float xt;

arm_biquad_casd_df1_inst_f32 highpass_biceps;
float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3};
float highpass_biceps_states[4];

arm_biquad_casd_df1_inst_f32 notch_biceps;
float notch_biceps_const[] = {D1, D2, D3, -C2, -C3};
float notch_biceps_states[4];

arm_biquad_casd_df1_inst_f32 lowpass_biceps;
float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3};
float lowpass_biceps_states[4];

float xbf;
float xbfmax = 0;

arm_biquad_casd_df1_inst_f32 highpass_triceps;
float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3};
float highpass_triceps_states[4];

arm_biquad_casd_df1_inst_f32 notch_triceps;
float notch_triceps_const[] = {D1, D2, D3, -C2, -C3};
float notch_triceps_states[4];

arm_biquad_casd_df1_inst_f32 lowpass_triceps;
float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3};
float lowpass_triceps_states[4];

float xtf;
float xtfmax = 0;

float xbt;
float xtt;

volatile bool regelaar_ticker_flag;     //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_ticker_flag()          //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
{
    regelaar_ticker_flag = true;
}

volatile bool regelaar_EMG_flag;        //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_EMG_flag()             //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
{
    regelaar_EMG_flag = true;
}

void keep_in_range(float * in, float min, float max)        //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
{
    if (*in < min) {                    //Als de waarde kleiner is als het minimum wordt de waarde het minimum
        *in = min;
    }
    if (*in > max) {                    //Als de waarde groter is dan het maximum is de waarde het maximum
        *in = max;
    } else {                            //In alle andere gevallen is de waarde de waarde zelf
        *in = *in;
    }
}

void arm1_naar_thuispositie()
{
    error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition());        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR;              //Integraal deel van regelaar
    afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR;         //Afgeleide deel van regelaar
    pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;      //Output naar motor na PID regelaar
    keep_in_range(&pwm_to_motor_arm1, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)

    if (pwm_to_motor_arm1 > 0) {                        //Als PWM is positief, dan richting 1
        dir_motor_arm1.write(1);
    } else {                                            //Anders richting nul
        dir_motor_arm1.write(0);
    }
    pwm_motor_arm1.write(fabs(pwm_to_motor_arm1));      //Output PWM naar motor is de absolute waarde van de berekende PWM
    pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
    pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);

    if (pwm_to_motor_arm1 == 0) {                       //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
        state = KALIBRATIE_ARM2;                        //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
        pc.printf("KALIBRATIE_ARM1 afgerond\n");          //Tekst voor controle Arm 1 naar thuispositie
    }
}

void arm2_naar_thuispositie()
{
    error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition());        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR;              //Integraal deel van regelaar
    afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR;         //Afgeleide deel van regelaar
    pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;      //Output naar motor na PID regelaar
    keep_in_range(&pwm_to_motor_arm2, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)

    if (pwm_to_motor_arm2 > 0) {                        //Als PWM is positief, dan richting 1
        dir_motor_arm2.write(1);
    } else {                                            //Anders richting nul
        dir_motor_arm2.write(0);
    }
    pwm_motor_arm2.write(fabs(pwm_to_motor_arm2));      //Output PWM naar motor is de absolute waarde van de berekende PWM
    pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition());
    pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2);

    if (pwm_to_motor_arm2 == 0) {                       //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
        state = EMG_KALIBRATIE_BICEPS;                      //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
        pc.printf("KALIBRATIE_ARM2 afgerond\n\r");          //Tekst voor controle Arm 2 naar thuispositie
    }
}

void filter_biceps()
{
    arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1);

    arm_biquad_cascade_df1_f32(&notch_biceps, &xbf, &xbf, 1);

    xbf = fabs(xbf);

    arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1);
    
    pc.printf("xbf is %f\n\r", xbf);


}

void filter_triceps()
{
    arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1);

    arm_biquad_cascade_df1_f32(&notch_triceps, &xtf, &xtf, 1);

    xtf = fabs(xtf);

    arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1);

     pc.printf("xtf is %f\n\r", xtf);

}

int main()
{
    while(1) {                      //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
        //PC baud rate instellen
        pc.baud(38400);             //PC baud rate is 38400 bits/seconde

        switch(state) {             //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case

            case RUST: {            //Aanzetten
                lcd_r1 = " BMT M9   GR. 4 ";        //Tekst op eerste regel van LCD scherm
                lcd_r2 = "Hoi! Ik ben PIPO";        //Tekst op tweede regel van LCD scherm
                wait(2);                            //Twee seconden wachten
                pc.printf("RUST afgerond\n\r");         //Tekst voor controle Aanzetten
                state = KALIBRATIE_ARM1;            //State wordt kalibratie arm 1, zo door naar volgende onderdeel
                break;                              //Stopt acties in deze case
            }

            case KALIBRATIE_ARM1: {                 //Arm 1 naar thuispositie
                pc.printf("KALIBRATIE_ARM1\n\r");
                wait(1);                            //Een seconde wachten

                ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken

                while(state == KALIBRATIE_ARM1) {
                    while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    arm1_naar_thuispositie();                       //Voer acties uit om arm 1 naar thuispositie te krijgen
                }
                wait(1);                                            //Een seconde wachten
                break;                                              //Stopt acties in deze case
            }

            case KALIBRATIE_ARM2: {                 //Arm 2 naar thuispositie
                pc.printf("KALIBRATIE_ARM1\n\r");
                wait(1);                            //Een seconde wachten

                //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken

                while(state == KALIBRATIE_ARM2) {
                    while(regelaar_ticker_flag != true) ;           //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    arm2_naar_thuispositie();                       //Voer acties uit om arm 2 naar thuispositie te krijgen
                }
                wait(1);                                            //Een seconde wachten
                ticker_regelaar.detach();                           //Ticker detachten, ticker doet nu niks meer
                break;                              //Stopt acties in deze case
            }

            case EMG_KALIBRATIE_BICEPS: {
                pc.printf("EMG__KALIBRATIE_BICEPS\n\r");

                lcd_r1 = " SPAN IN 5 SEC. ";
                lcd_r2 = " 2 X BICEPS AAN ";
                pc.printf("span biceps aan\n\r");
                pc.printf("EMG biceps %f\n\r",xb);

                arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states);
                arm_biquad_cascade_df1_init_f32(&notch_biceps, 1, notch_biceps_const, notch_biceps_states);
                arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states);

                ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
                biceps_kalibratie.start();

                while(biceps_kalibratie.read() <= 5) {
                    
                    while(regelaar_EMG_flag != true) ;              //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    xb = EMG_bi.read();         //EMG meten van biceps

                    filter_biceps();

                    if(int(biceps_kalibratie.read()) == 1) {
                        lcd_r1 = "       4        ";
                        pc.printf("4");
                    }
                    if(biceps_kalibratie.read() == 2) {
                        lcd_r1 = "       3        ";
                        pc.printf("3");
                    }
                    if(biceps_kalibratie.read() == 3) {
                        lcd_r1 = "       2        ";
                        pc.printf("2");
                    }
                    if(biceps_kalibratie.read() == 4) {
                        lcd_r1 = "       1        ";
                        pc.printf("1");
                    }

                }
                if(xbf >= xbfmax) {
                    xbfmax = xbf;
                }
                xbt = xbfmax * 0.8;
                pc.printf("threshold is %f\n\r", xbt);
                state = EMG_KALIBRATIE_TRICEPS;
                break;
            }

            case EMG_KALIBRATIE_TRICEPS: {
                pc.printf("EMG__KALIBRATIE_TRICEPS\n\r");

                lcd_r1 = " SPAN IN 5 SEC. ";
                lcd_r2 = " 2 X TRICEPS AAN";
                pc.printf("span triceps aan\n\r");
                pc.printf("EMG biceps %f\n\r",xt);

                arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states);
                arm_biquad_cascade_df1_init_f32(&notch_triceps, 1, notch_triceps_const, notch_triceps_states);
                arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1, lowpass_triceps_const, lowpass_triceps_states);

                ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
                triceps_kalibratie.start();

                while(triceps_kalibratie.read() <= 5) {
                   
                    while(regelaar_EMG_flag != true) ;              //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan

                    xt = EMG_tri.read();         //EMG meten van biceps

                    filter_triceps();

                    if(triceps_kalibratie.read() == 1) {
                        lcd_r1 = "       4        ";
                        pc.printf("4");
                    }
                    if(triceps_kalibratie.read() == 2) {
                        lcd_r1 = "       3        ";
                        pc.printf("3");
                    }
                    if(triceps_kalibratie.read() == 3) {
                        lcd_r1 = "       2        ";
                        pc.printf("2");
                    }
                    if(triceps_kalibratie.read() == 4) {
                        lcd_r1 = "       1        ";
                    }

                }
                if(xtf >= xtfmax) {
                    xtfmax = xtf;
                }
                xtt = xtfmax * 0.8;
                pc.printf("threshold is %f\n\r", xtt);
                state = BEGIN_METEN;
                break;
            }

            case BEGIN_METEN: {
                lcd_r1 = "      BEGIN     ";
                pc.printf("KEUZE1\n\r");

                while(state == BEGIN_METEN) {
                    while(regelaar_EMG_flag != true);              //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                    regelaar_EMG_flag = false;

                    xb = EMG_bi.read();
                    filter_biceps();
                    xt = EMG_tri.read();
                    filter_triceps();

                    if(xb >= xbt) {
                        state = B;
                    }
                    if(xt >= xtt) {
                        state = T;
                    }
                }




                default: {                              //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
                    state = RUST;                       //Als dat gebeurt wordt de state rust en begint hij weer vooraan
                }
            }
            pc.printf("state: %u\n\r",state);
        }
    }
}