File content as of revision 2:fbda25b453b3:

#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

#define SAMPLETIME_EMG  0.005   //Constanten definiëren en een waarde toekennen
#define A1              2
#define A2              -157.9
#define B0              1
#define B1              -1.964
#define B2              0.9651
#define C1              1.158
#define C2              -0.4112
#define D0              1
#define D1              -1.709
#define D2              0.9625

//Pinverdeling en naamgeving variabelen
Encoder motor_arm1(             //Encoder motor arm 1
Encoder motor_arm2(             //Encoder motor arm 2   

PwmOut pwm_motor_arm1(          //PWM naar motor arm 1
PwmOut pwm_motor_arm2(          //PWM naar motor arm 2

DigitalOut dir_motor_arm1(      //Richting motor arm 1
DigitalOut dir_motor_arm2(      //Richting motor arm 2

AnalogIn emg_bi(                //EMG signalen biceps 
AnalogIn emg_tri(               //EMG signalen triceps

Ticker ticker_sample_EMG(              //Uitlezen EMG iedere zoveel seconden


//Gedefinieerde datatypes en variabelen
bool rust = false
bool kalibratie_positie = false
bool kalibratie_EMG = false
char lcd_r1 = new char[16];     //LCD regel 1, 16 karakters
char lcd_r2 = new char[16];     //LCD regel 2, 16 karakters
void sample_bi_EMG              //Acties voor inladen EMG
void sample_tri_EMG             //Acties voor inladen EMG
float xbk                       //x(n) biceps
float xbk1                      //x(n-1) biceps
float xbk2                      //x(n-2) biceps
float ybk                       //y(n) biceps
float ybk1                      //y(n-1) biceps
float ybk2                      //y(n-2) biceps

float xtk                       //x(n) triceps
float xtk1                      //x(n-1) triceps
float xtk2                      //x(n-2) triceps
float ytk                       //y(n) triceps
float ytk1                      //y(n-1) triceps
float ytk2                      //y(n-2) triceps    


//Beginwaarden voor variabelen

xbk1 = 0                        //xbk(n-1) = 0
xbk2 = 0                        //xbk(n-2) = 0
ybk1 = 0                        //ybk(n-1) = 0
ybk2 = 0                        //ybk(n-2) = 0

xtk1 = 0                        //xtk(n-1) = 0
xtk2 = 0                        //xtk(n-2) = 0
ytk1 = 0                        //ytk(n-1) = 0
ytk2 = 0                        //ytk(n-2) = 0


   //PC baud rate
   pc.baud(38400)

int main{
       
       if (rust = false && kalibratie_positie = false && kalibratie_EMG = false){
            lcd_r1 = " BMT M9   GR. 4 ";    //Tekst in LCD scherm
            lcd_r2 = "Hoi! Ik ben PIPO";    //Tekst in LCD scherm  
            wait(2);                        //2 seconden wachten
            rust = true
       } 
    
       if (rust = true && kalibratie_positie = false && kalibratie_EMG = false){
            wait(1);                        //1 seconde wachten
        
            pwm_motor_arm1 = 0.02036        //Dutycycle motor arm 1/100
            dir_motor_arm1 = // 1 of 0      //Draairichting motor arm 1
        
            while(motor_arm1 < 363){        //Zolang aantal encoderpulsen < 363 --> PWM motor arm 1 blijft gelijk
                pwm_motor_arm1 = 0.02036
            }
        
            pwm_motor_arm1 = 0              //Na while loop PWM motor arm 1 op nul
            motor_arm1 = 0                  //Encoderpulsen op nul zetten
        
            pwm_motor_arm2 = 0.02398        //Dutycycle motor arm 2/100
            dir_motor_arm2 = // 1 of 0      //Draairichting motor arm 2
        
            while(motor_arm_2 < 787){       //Zolang aantal encoderpulsen < 787 --> PWM motor arm 3 blijft gelijk
                pwm_motor_arm2 = 0.02398
            }
        
            pwm_motor_arm2 = 0              //Na while loop PWM motor arm 2 op nul
            motor_arm2 = 0                  //Encoderpulsen op nul zetten
        
            lcd_r1 = "  Beginpositie  "     //Tekst in LCD scherm
            lcd_r2 = "    ingesteld   "     //Tekst in LCD scherm   
            wait(2);                        //2 seconden wachten
            kalibratie_positie = true
       }
    
       if (rust = true && kalibratie_positie = true && kalibratie_EMG = false){
            wait(1);                        //1 seconde wachten
            
            //EMG inladen
            void sample_EMG{
                xbk = emg_bi.read();            //Uitlezen EMG signaal biceps
                xtk = emg_tri.read();           //Uitlezen EMG signaal triceps
            }
            
            ticker_sample_EMG.attach(sample_EMG,SAMPLETIME_EMG)     //Ticker gaat de void sample_EMG uitvoeren en dus de EMG uitlezen iedere 0.005 seconden
            
            //Detrend
            
        
            //Hoogdoorlaatfilter 2Hz
            while(true){
                ybk = A1*ybk1-A2*ybk2+B0*xbk-B1*xbk1+B2*xbk2        //Overdracht 2e orde hoogdoorlaatfilter
                
                xbk2 = xbk1                                         //xbk(n-2) wordt xbk(n-1) in de volgende loop
                xbk1 = xbk                                          //xbk(n-1) wordt xbk(n) in de volgende loop
                ybk2 = ybk1                                         //ybk(n-2) wordt ybk(n-1) in de volgende loop
                ybk1 = ybk                                          //ybk(n-1) wordt ybk(n) in de volgende loop
                
                ytk = A1*ytk1-A2*ytk2+B0*xtk-B1*xtk1+B2*xtk2        //Overdracht 2e orde hoogdoorlaatfilter
                
                xtk2 = xtk1                                         //xtk(n-2) wordt xtk(n-1) in de volgende loop
                xtk1 = xtk                                          //xtk(n-1) wordt xtk(n) in de volgende loop
                ytk2 = ytk1                                         //ytk(n-2) wordt ytk(n-1) in de volgende loop
                ytk1 = ytk                                          //ytk(n-1) wordt ytk(n) in de volgende loop
                
                wait(SAMPLETIME_EMG);                               //0.005 seconden wachten
            }
            
            //Bandstop 40 - 60Hz
            while(true){
                blabla = 
            
            //Rectifier
            
            
            //Laagdoorlaatfilter 10Hz
            
            
            //Moving average 4e orde