File content as of revision 13:54ee98850a15:

#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

//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR         0.005       //Sampletijd ticker regelaar motor
#define KP_arm1                     0.3       //Factor proprotionele regelaar arm 1
#define KI_arm1                     0         //Factor integraal regelaar arm 1
#define KD_arm1                     0.02        //Factor afgeleide regelaar arm 1
#define KP_arm2                     0.001       //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 -3.1806
#define A3 3.8612
#define A4 -2.1122
#define A5 0.4383
#define B1 4.1660E-4
#define B2 0.0017
#define B3 0.0025
#define B4 0.0017
#define B5 4.1660E-4

//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 3.9179
#define E3 5.7571
#define E4 3.7603
#define E5 0.9212
#define F1 0.9598
#define F2 3.8391
#define F3 5.7587
#define F4 3.8391
#define F5 0.9598

//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
//Blauw op 3,3 V en groen op GND

Ticker ticker_regelaar;                 //Ticker voor regelaar motor
Ticker ticker_EMG;                      //Ticker voor EMG meten

//States definiëren
enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_OFFSET_BICEPS, EMG_BICEPS, KALIBRATIE_BICEPS, EMG_OFFSET_TRICEPS, EMG_TRICEPS, KALIBRATIE_TRICEPS};     //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 xbk = 0;                          //Gemeten EMG waarde biceps in de kalibratie
float xbk1 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk2 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk3 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk4 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk5 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk6 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk7 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk8 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk9 = 0;                         //Gemeten EMG waarde biceps in de kalibratie
float xbk10 = 0;                        //Gemeten EMG waarde biceps in de kalibratie
float meanxbk;                          //Offset van biceps kalibratie
float xb;                               //Gemeten EMG waarde biceps
float xbo;                              //Gemeten EMG waarde biceps min de offset, deze gaat in de filters

//High Pass filter          Y(n-x) waarden instellen op nul en definiëren als float
float xbhp;                             //Zelfde als xbo, maar makkelijker in notatie in filter
float xbhp1 = 0;
float xbhp2 = 0;
float xbhp3 = 0;
float xbhp4 = 0;
float xbhp5 = 0;
float ybhp;
float ybhp1 = 0;
float ybhp2 = 0;
float ybhp3 = 0;
float ybhp4 = 0;
float ybhp5 = 0;

//Notch filter
float xbn;
float xbn1 = 0;
float xbn2 = 0;
float ybn;
float ybn1 = 0;
float ybn2 = 0;

//Rectifier
float rb;
float rbr;

//Low pass filter
float xblp;
float xblp1 = 0;
float xblp2 = 0;
float xblp3 = 0;
float xblp4 = 0;
float xblp5 = 0;
float yblp;
float yblp1 = 0;
float yblp2 = 0;
float yblp3 = 0;
float yblp4 = 0;
float yblp5 = 0;

//Gefilterde biceps EMG
float xbf;
float ybf;

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_OFFSET_BICEPS;                      //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
        pc.printf("KALIBRATIE_ARM2 afgerond\n");          //Tekst voor controle Arm 2 naar thuispositie
    }
}

void filter_biceps()
{
    //High pass
    xbhp = xbo;             //Input in filter
    ybhp = -A2*ybhp1 - A3*ybhp2 - A4*ybhp3 - A5*ybhp4 + B1*xbhp + B2*xbhp1 + B3*xbhp2 + B4*xbhp3 + B5*xbhp4;        //Filterformule in z-domein
    
    //Waarden voor de volgende ronde benoemen
    xbhp4 = xbhp3;
    xbhp3 = xbhp2;
    xbhp2 = xbhp1;
    xbhp1 = xbhp;
    xbn = xbhp;
 
    ybhp4 = ybhp3;
    ybhp3 = ybhp2;
    ybhp2 = ybhp1;
    ybhp1 = ybhp;
    ybn1 = ybhp;        //De vorige waarde in het notchfilter is de waarde uit het high pass filter
    
    //Notch
    ybn = -C2*ybn1 - C3*ybn2 + D1*xbn + D2*xbn1 + D3*xbn2;              //Filterfunctie in z-domein
    
    //Waarden voor de volgende ronde benoemen
    xbn2 = xbn1;
    xbn1 = xbn;
    xblp = xbn;
    
    ybn2 = ybn1;
    ybn1 = ybn;
    rb = ybn;
    
    //Rectify
    rbr = fabs(rb);             //Absolute waarde van de waarde uit notchfilter
    
    yblp1 = rbr;
    
    //Low pass
    yblp = -E2*yblp1 - E3*yblp2 - E4*yblp3 - E5*yblp4 + F1*xblp + F2*xblp1 + F3*xblp2 + F4*xblp3 + F5*xblp4;        //Filterfunctie in z-domein
    
    xblp4 = xblp3;
    xblp3 = xblp2;
    xblp2 = xblp1;
    xblp1 = xblp;
    xbf = xblp;
 
    yblp4 = yblp3;
    yblp3 = yblp2;
    yblp2 = yblp1;
    yblp1 = yblp;
    ybf = yblp;    
}


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");         //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");
                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");
                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_OFFSET_BICEPS: {               //Kalibratie EMG signaal biceps
                pc.printf("EMG_OFFSET_BICEPS\n");
                wait(1);                            //Een seconde wachten

                ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);     //Ticker iedere zoveel seconden de flag laten opsteken

                pc.printf("Ticker voor kalibratie compleet\n");               //Tekst voor controle Ticker voor kalibratie

                //5 seconden EMG biceps meten

                wait(1);                            //Een seconde wachten
                lcd_r1 = " EMG kalibratie ";        //Tekst op eerste regel van LCD scherm
                lcd_r2 = " Span biceps aan";        //Tekst op tweede regel van LCD scherm

                while(state == EMG_OFFSET_BICEPS){
                    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
                    
                    pc.printf("Ga EMG meten\n");

                    xbk = EMG_bi.read();            //EMG signaal uitlezen
                    xbk10 = xbk9;
                    xbk9 = xbk8;
                    xbk8 = xbk7;
                    xbk7 = xbk6;
                    xbk6 = xbk5;
                    xbk5 = xbk4;
                    xbk4 = xbk3;
                    xbk3 = xbk2;
                    xbk2 = xbk1;
                    xbk1 = xbk;
                    pc.printf("xbk10 is %f\n", xbk10);
                    
                    if(xbk10 > 0){
                        pc.printf("10 waarden gemeten\n");
                        state = EMG_BICEPS;
                    }  
                }
                break;
            }
            
            case EMG_BICEPS:
            {
                pc.printf("EMG_BICEPS\n");
                meanxbk = (xbk1 + xbk2 + xbk3 + xbk4 + xbk5 + xbk6 + xbk7 + xbk8 + xbk9 + xbk10)/10;        //Offset bepalen van de eerste 10 gemeten waarden
                
                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
                xbo = xb - meanxbk;         //Gemeten waarden zonder offset
                
                filter_biceps();

            }
            
            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",state);
    }
}