Diff: main.cpp

Revision:

25:c97d079e07f3

Parent:

24:ddd69385b55f

Child:

26:d9855716ced7

diff -r ddd69385b55f -r c97d079e07f3 main.cpp
--- a/main.cpp	Thu Oct 15 11:13:32 2015 +0000
+++ b/main.cpp	Fri Oct 16 08:04:48 2015 +0000
@@ -8,65 +8,63 @@
 #include "Mode.h"
 //--------------------Classes------------------------
 InterruptIn btnSet(PTC6);           // kalibreer knop
-DigitalOut ledR(LED_RED);           // Led op moederbord
+DigitalOut ledR(LED_RED), LedB(LED3); // Led op moederbord
 MODSERIAL pc(USBTX, USBRX);         // Modserial voor Putty
 TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD scherm op binnenste rij van K64F, rs, e, d4-d7
 PwmOut servo(D3);                   // PwmOut servo
-AnalogIn btnL(A2), btnR(A3);        // push buttons, links en rechts
+AnalogIn emgL(A0), emgR(A1)         // Analog input van emg kabels links en rechts
 AnalogIn ptmrL(A2), ptmrR(A3);      // Intensiteit 'EMG' signaal door potmeter
 AnalogIn KS(A4);                    // Killswitch
-Ticker tickerRegelaar, tickerBtn, tickerRem;   // regelaar button en rem ticker
-Timer EMGTimer;                     // timer voor EMG meting
-// EMG spul:
-AnalogIn    emg(A0); //Analog input van emg kabels Links
-AnalogIn    emgR(A1); //Analog input van emg kabels Rechts
-HIDScope    scope(3); //3 scopes
-Ticker      EMGticker;
-DigitalOut  LedBlue(LED3);
-DigitalIn   button(PTA4);
+HIDScope scope(6);                  // 3 scopes
+Ticker EMGticker, tickerRegelaar, tickerBtn, tickerRem;   // regelaar button en rem ticker
 
 // QEI Encoder(poort 1, poort 2, ,counts/rev
-    QEI enc1 (D13, D12, NC, 64);
-    QEI enc2 (D11, D10, NC, 64);
+    QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
 // Motor1 met PWM aansturen en richting aangeven
     PwmOut pwmM1(D6);       
     DigitalOut dirM1(D7);
 // Motor2 met PWM aansturen en richting aangeven
     PwmOut pwmM2(D5);       
     DigitalOut dirM2(D4);
-
 enum spelfase {KALIBRATE_EMG1, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE};  // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
-uint8_t state = KALIBRATE_EMG1; // eerste spelfase
- 
+uint8_t state = KALIBRATE_EMG1;     // eerste spelfase 
 enum aimFase {OFF, CW, CCW};         // Aim motor mogelijkheden
 uint8_t aimState = OFF;              // mogelijkheid begin
-//-------------------------------Variables---------------------------------------------------------------------   
+//-------------------------------Variables--------------------------------------------------------------------- 
+const int on = 0, off = 1;               // aan / uit
+int maxCounts = 2100;                    // max richt motor counts Aim motor
+int remPerc = 0;
+volatile int emgModeL = 1, emgModeR = 1 potModeL = 1, potModeR = 1; // emg signaal waarden (modes)
+
 const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R  (= pulsewidth pwm servo) 
-const int on = 0, off = 1;                    // aan / uit
-volatile bool btn = false;                       // boolean om programma te runnen, drukknop ingedrukt
-int maxCounts = 2100;                   // max richt motor counts Aim motor
-volatile bool regelaarFlag = false, btnFlag = false, remFlag = false;     // Go flags
 const double tRegelaar = 0.005, tBtn = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
-int remPerc = 0;
+const double Fs = 200;                   //Sample frequentie Hz
+double t = 1/ Fs;                        // tijd EMGticker
+double ymin = 0, ymax = 0;
+double thresholdlow = 0, thresholdmid = 0, thresholdhigh= 0;
+
 volatile bool L = false, R = false;      // Booleans voor knop link en knop rechts
-volatile int emgModeL = 1, emgModeR = 1;   // emg signaal waarden 
-
-//Sample frequentie
-double Fs = 200; //Hz
-double t = 1/ Fs; // voor EMGticker
-
-bool readymax = 0, readymin = 0;
-double ymin, ymax;
-double thresholdlow, thresholdmid, thresholdhigh;
-bool go_flag = false;
+volatile bool btn = false;               // boolean om programma te runnen, drukknop ingedrukt
+volatile bool regelaarFlag = false, btnFlag = false, remFlag = false;     // Go flags
+volatile bool emgFlag = false;
 
 //----------------------------Functions-----------------------------------------------------------------------
-void EMGkalibratie(){
-    LedBlue = 1;
+void flipLed(DigitalOut& led){              //function to flip one LED   
+    led.write(on);
+    wait(0.2);
+    led.write(off);
+    }
+void PRINT(const char* text){                // putty en lcd print
+    lcd.cls();                              // clear LCD scherm
+    lcd.printf(text);                       // print text op lcd
+    pc.printf(text);                        // print text op terminal
+    }
+void EMGkalibratie(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
+    LedB = 1;
     Init();    
-    ymin = KalibratieMin(readymin);
+    ymin = KalibratieMin(emgL);
     wait(1);
-    ymax = KalibratieMax(readymax);
+    ymax = KalibratieMax(emgL);
 
     // bepalen van thresholds voor aan/uit
     thresholdlow = 8 * ymin; // standaardwaarde
@@ -75,34 +73,19 @@
 
     pc.printf("ymax = %f en ymin = %f \n",ymax, ymin); //bugfix
     }
-
-void EMGfilter(){
-    //uitlezen emg signaal
-    double u = emg.read();
-    double y = Filterdesigns(u);
-    //pc.printf("%f \n",y); //bugfix
-    // Plotten in HIDscope
-    emgModeL = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (aan, uit, middel)
-    scope.set(0,u); //ongefilterde waarde naar scope 1
-    scope.set(1,y); //gefilterde waarde naar scope 2
-    scope.set(2,emgModeL);
-    scope.send(); //stuur de waardes naar HIDscope
+int EMGfilter(AnalogIn& emg, int w){    
+    double u = emg.read();                      // uitlezen emg signaal
+    double y = Filterdesigns(u);                // signaal filteren    
+    int mode = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (1, 2 ,3 )    
+    
+    scope.set(w,u);                             //ongefilterde waarde naar scope 1
+    scope.set(w+1,y);                            //gefilterde waarde naar scope 2
+    scope.set(w+2,emgModeL);
+    scope.send();                               //stuur de waardes naar HIDscope
+    return mode;
     }
-
-void Go_flag(){
-    go_flag = true;
-    }
-// -----
-
-void flipLed(DigitalOut& led){                  //function to flip one LED   
-    led.write(on);
-    wait(0.2);
-    led.write(off);
-    }
-void PRINT(const char* text){                // putty en lcd print
-    lcd.cls();                              // clear LCD scherm
-    lcd.printf(text);                       // print text op lcd
-    pc.printf(text);                        // print text op terminal
+void setEmgFlag(){
+    emgFlag = true;
     }   
 void btnSetAction(){                // Set knop K64F
     btn = true;                     // GoFlag setknop 
@@ -174,13 +157,10 @@
     tickerBtn.attach(&setBtnFlag,tBtn);                  // ticker knop controle
     tickerRem.attach(&setRemFlag,tRem);                  // ticker rem
       
-    EMGticker.attach(&Go_flag, t);                      // ticker EMG, 500H
-    
-    lcd.cls();
-    lcd.printf("NEW GAME \n HELLO!");
-     
+    EMGticker.attach(&setEmgFlag, t);                      // ticker EMG, 500H
+        
     pc.printf("\n\n\n\n\n");
-    pc.printf("--- NEW GAME ---\r\n");    
+    PRINT("--- NEW GAME ---\r\n");    
     while(1){                   // Programma door laten lopen
         switch(state){
             case KALIBRATE_EMG1: {
@@ -204,9 +184,10 @@
                         checkAim();     // Controleer positie                       
                         }
                         
-                    if(go_flag){            // Go flag EMG sampelen
-                        go_flag = false;
-                        EMGfilter();
+                    if(emgFlag){            // Go flag EMG sampelen
+                        emgFlag = false;
+                        emgModeL = EMGfilter(emgL,0);
+                        emgModeR = EMGfilter(emgR,3);
                         }        
                     if(btnFlag){                // Go flag button controle?                        
                         btnFlag = false;                                                
@@ -247,9 +228,10 @@
                         regelaarFlag = false;                  
                         checkAim();     // Controleer positie                       
                         }
-                    if(go_flag){            // Go flag EMG sampelen
-                        go_flag = false;
-                        EMGfilter();
+                    if(emgFlag){            // Go flag EMG sampelen
+                        emgFlag = false;
+                        emgModeL = EMGfilter(emgL,0);
+                        emgModeR = EMGfilter(emgR,3);
                         }                   
                     if(btnFlag){                // Go flag button controle?                                                
                         btnFlag = false;                                                
@@ -266,9 +248,10 @@
                 L = false;
                 R = false;
                 while(state == REM){
-                    if(go_flag){                    // Go flag EMG sampelen
-                        go_flag = false;
-                        EMGfilter();
+                    if(emgFlag){                    // Go flag EMG sampelen
+                        emgFlag = false;
+                        emgModeL = EMGfilter(emgL,0);
+                        emgModeR = EMGfilter(emgR,3);
                         }
                     if(btnFlag){                    // Go flag button controle?  == rem aansturen                      
                         btnFlag = false;