File content as of revision 7:dd3cba61b34b:

#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                      1           //Factor proprotionele regelaar
#define SAMPLETIME_EMG          0.005       //Sampletijd ticker EMG meten

//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(PTD1);        //Richting van motor arm 1
Encoder puls_motor_arm1(PTD0, PTC9);    //Encoder pulsen tellen van motor arm 1
PwmOut pwm_motor_arm2(PTA12);           //PWM naar motor arm 2
DigitalOut dir_motor_arm2(PTD3);        //Ricting van motor arm 2
Encoder puls_motor_arm2(PTD4, PTC8);    //Encoder pulsen tellen van motor arm 2
AnalogIn EMG_bi(PTB1);                  //Meten EMG signaal biceps


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

//Gedefinieerde datatypen en naamgeving en beginwaarden
bool rust = false;                      //Bool voor controleren volgorde in programma
bool kalibratie_positie_arm1 = false;   //Bool voor controleren volgorde in programma
bool kalibratie_positie_arm2 = false;   //Bool voor controleren volgorde in programma
bool kalibratie_EMG_bi = false;         //Bool voor controleren volgorde in programma
bool kalibratie_EMG_tri = false;        //Bool voor controleren volgorde in programma

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

int puls_arm1_home = 363;           //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
int puls_arm2_home = 787;           //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
float pwm_to_motor_arm1;            //PWM output naar motor arm 1
float pwm_to_motor_arm2;            //PWM output naar motor arm 2
float xbk;                          //Gemeten EMG waarde biceps in de kalibratie
int i;                              //

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(){
    pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    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(abs(pwm_to_motor_arm1));   //Output PWM naar motor is de absolute waarde van de berekende PWM
            
    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)
        kalibratie_positie_arm1 = true;
        pc.printf("Arm 1 naar thuispositie compleet");     //Tekst voor controle Arm 1 naar thuispositie
    }  
}   

void arm2_naar_thuispositie(){
    pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP;        //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    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(abs(pwm_to_motor_arm2));   //Output PWM naar motor is de absolute waarde van de berekende PWM
            
    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)
        kalibratie_positie_arm2 = true;
        pc.printf("Arm 2 naar thuispositie compleet");     //Tekst voor controle Arm 2 naar thuispositie
    }
}


int main() {
    
    //PC baud rate instellen
    pc.baud(38400);             //PC baud rate is 38400 bits/seconde
    
    //Aanzetten
    if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
        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("Aanzetten compleet");    //Tekst voor controle Aanzetten
        rust = true;                        //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
    }
    
    
    
    //Arm 1 naar thuispositie
    if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
        wait(1);                            //Een seconde wachten
        
        ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
        
        while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
            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
    }
    
    //Arm 2 naar thuispositie
    if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
        wait(1);                            //Een seconde wachten
        
        //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
        
        while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
            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
    }
    
    //Ticker voor kalibratie
    if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
        
        ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);     //Ticker iedere zoveel seconden de flag laten opsteken
        pc.printf("Ticker voor kalibratie compleet");               //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
        wait(2);                            //Twee seconden wachten
        
        for (i=0; i<1000; i++){
            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
            
            xbk = EMG_bi.read();            //EMG signaal uitlezen
        }
            
    }
        
    
        
        
}