Diff: main.cpp

Revision:

27:f62e450bb411

Parent:

26:d9855716ced7

Child:

28:e6d2fe0e593e

diff -r d9855716ced7 -r f62e450bb411 main.cpp
--- a/main.cpp	Fri Oct 16 09:12:51 2015 +0000
+++ b/main.cpp	Fri Oct 16 12:37:26 2015 +0000
@@ -13,8 +13,8 @@
 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 emgL(A0), emgR(A1);         // Analog input van emg kabels links en rechts
-AnalogIn potL(A2), potR(A3);      // Intensiteit 'EMG' signaal door potmeter
+AnalogIn emgL(A0), emgR(A1);        // Analog input van emg kabels links en rechts
+AnalogIn potL(A2), potR(A3);        // Intensiteit 'EMG' signaal door potmeter
 AnalogIn KS(A4);                    // Killswitch
 HIDScope scope(6);                  // 3 scopes
 Ticker EMGticker, tickerRegelaar, tickerRem;   // regelaar button en rem ticker
@@ -27,8 +27,8 @@
 // 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 
+enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE};  // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
+uint8_t state = KALIBRATE_EMG;     // eerste spelfase 
 enum aimFase {OFF, CW, CCW};         // Aim motor mogelijkheden
 uint8_t aimState = OFF;              // mogelijkheid begin
 //-------------------------------Variables--------------------------------------------------------------------- 
@@ -38,12 +38,12 @@
 
 const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R  (= pulsewidth pwm servo) 
 const double tRegelaar = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
-const double Fs = 200;                   //Sample frequentie Hz
+const double Fs = 50;                   //Sample frequentie Hz
 double t = 1/ Fs;                        // tijd EMGticker
-double ymin = 0, ymax = 0;
-double thresholdlow = 0, thresholdmid = 0, thresholdhigh= 0;
+double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
+double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
 
-volatile bool L = false, R = false;      // Booleans voor knop link en knop rechts
+volatile bool L = false, R = false, RH = false;      // Booleans voor knop link en knop rechts
 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;
@@ -58,29 +58,51 @@
     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
+void EMGkalibratieL(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
+    LedB = 1;
+    Init();    
+    double ymin = KalibratieMin(emgL);
+    wait(1);
+    double ymax = KalibratieMax(emgL);
+    
+    if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
+        ymin = 10;
+        ymax = 10;
+        }    
+    thresholdlowL = 8 * ymin; // standaardwaarde
+    thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
+    thresholdhighL = 0.8 * ymax;
+
+    pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); //bugfix
+    }
+void EMGkalibratieR(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
     LedB = 1;
     Init();    
-    ymin = KalibratieMin(emgL);
+    double ymin = KalibratieMin(emgR);
     wait(1);
-    ymax = KalibratieMax(emgL);
-
+    double ymax = KalibratieMax(emgR);
+    
+    if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
+        ymin = 10;
+        ymax = 10;
+        }    
     // bepalen van thresholds voor aan/uit
-    thresholdlow = 8 * ymin; // standaardwaarde
-    thresholdmid = 0.5 * ymax; // afhankelijk van max output gebruiker
-    thresholdhigh = 0.8 * ymax;
+    thresholdlowR = 8 * ymin; // standaardwaarde
+    thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
+    thresholdhighR = 0.8 * ymax;
 
-    pc.printf("ymax = %f en ymin = %f \n",ymax, ymin); //bugfix
+    pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
     }
-int EMGfilter(AnalogIn& emg, int w){    
+int EMGfilter(AnalogIn& emg, bool side){    
     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,mode);
-    scope.send();                               //stuur de waardes naar HIDscope
+    double y = Filterdesigns(u);                // signaal filteren
+    int mode = 1;
+    if(side){                                   // linker EMG  
+        mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
+        }
+    else {                                      // rechter EMG
+        mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
+        }          
     return mode;
     }    
 int PotReader(AnalogIn& pot){                   // potmeter uitlezen en mode bepalen (thresholds)
@@ -94,8 +116,8 @@
         }    
     return mode;    
     }    
-int defMode(AnalogIn& emg, int w, AnalogIn& pot){       // bepaal mode uit emg en pot
-    int emgMode = EMGfilter(emg, w);
+int defMode(AnalogIn& emg, AnalogIn& pot, bool side){       // bepaal mode uit emg en pot
+    int emgMode = EMGfilter(emg, side);
     int potMode = PotReader(pot);
     int mode = 1;    
     if(!(emgMode==1) != !(potMode==1)){         // emgMode = 1 XOR potMode = 1
@@ -105,7 +127,7 @@
         else{
             mode = potMode;
             }
-        }
+        }    
     return mode;       
     }        
 void setEmgFlag(){
@@ -121,11 +143,11 @@
     remFlag = true;
     }  
 void checkAim(){                    // controleer of killer switch geraakt is en of max aantal counts niet bereikt is 
-    double volt = KS.read();    
-    if(volt< 0.5 /*|| abs(enc2.getPulses()) > maxCounts*/){
-        pwmM2.write(0);                                 // Aim motor stilzetten
+    /*double volt = KS.read();    
+    if(volt> 0.5 /*|| abs(enc2.getPulses()) > maxCounts*///){
+        /*pwmM2.write(0);                                 // Aim motor stilzetten
         pc.printf("BOEM! CRASH! KASTUK! \r\n");        
-        }
+        }*/
     }
 void motorAim(int dir){            // Aim motor laten draaien met gegeven direction 
     dirM2.write(dir);              // richting
@@ -133,43 +155,48 @@
     }    
 bool controlAim(){                  // Function om aim motor aan te sturen
     bool both = false;              // boolean of beide knoppen ingedrukt zijn   
-    int modeL = defMode(emgL, 0, potL);
-    int modeR = defMode(emgR, 3, potR);
-   
+    int modeL = defMode(emgL, potL, true);
+    int modeR = defMode(emgR, potR, false);    
+    
+    scope.set(0, modeL);
+    scope.set(1, modeR);    
+    scope.send();                               //stuur de waardes naar HIDscope
+    
+    if(modeR == 1 && !R){                               // R is niet aan
+        R = true;        
+        }    
+    if(modeL == 1 && !L){                               // L is niet aan
+        L = true;
+        }
+            
     if(modeL>1 && modeR >1) {                           // mode L en mode R beide > 1 
         R = false;
         L = false;
         pwmM2.write(0);                                 // Aim motor stilzetten       
         aimState = OFF;        
         }
-    else if(modeL == 3) {                 
+    else if((modeL == 3)) {                 
         both = true;                                    // Return true
         pwmM2.write(0);                                 // Aim motor stilzetten
         aimState = OFF; 
         L = false;        
         } 
     else if(modeR == 3 && aimState!=OFF)  {      
-        R = false;
+        R = false;        
         pwmM2.write(0);                                 // Aim motor stilzetten        
         aimState = OFF; 
         PRINT("Motor freeze\r\n");
         } 
-    else if(modeL == 2 && aimState != CCW && L) {       // modeL ==2 AND richting is niet CCW
+    else if((modeL == 2) && (aimState != CCW) && L) {       // modeL ==2 AND richting is niet CCW
         aimState = CCW;                                 // draai CCW
         pc.printf("Turn negative (CCW)\r\n");
         motorAim(0);
         } 
-    else if((modeR == 2) && aimState != CW && R) {      // modeR == 2 AND richting is niet CW
+    else if((modeR == 2) && (aimState != CW) && R) {      // modeR == 2 AND richting is niet CW
         aimState = CW;                                  // draai CW
         PRINT("Turn positive (CW)\r\n");
         motorAim(1);
-        }
-    if(modeR == 1 && !R){                               // R is niet aan
-        R = true;
-        }
-    if(modeL == 1 && !L){                               // L is niet aan
-        L = true;
-        }
+        }    
     return both;            
     }               
 int main(){
@@ -184,12 +211,13 @@
     PRINT("--- NEW GAME ---\r\n");    
     while(1){                                           // Programma door laten lopen
         switch(state){
-            case KALIBRATE_EMG1: {
-                pc.printf("Kalibrate EMG signal in 5 seconds\r\n");
-                lcd.cls();
-                lcd.printf("Kalibrate EMG\n in 5 sec.");
+            case KALIBRATE_EMG: {
+                PRINT("Kalibrate EMG left in 5 seconds\r\n");
+                                
+                EMGkalibratieL();
                 
-                EMGkalibratie();
+                PRINT("Kalibrate EMG right in 5 seconds\r\n");
+                EMGkalibratieR();                
                                    
                 state = KALIBRATE_AIM;
                 break;
@@ -261,9 +289,9 @@
                 while(state == REM){                      
                     if(emgFlag){            // Go flag EMG sampelen
                         emgFlag = false;                        
-                        int modeL = defMode(emgL, 0, potL);
-                        int modeR = defMode(emgR, 3, potR);
-                                                                                    
+                        int modeL = defMode(emgL, potL, true);
+                        int modeR = defMode(emgR, potR, false);
+                                                                                  
                         if(modeR < 2) {           // R is niet aan
                             R = true;
                             }
@@ -271,20 +299,20 @@
                             L = true;
                             }
                         if(L && R){                    
-                            if(modeL > 1 && modeR > 1) {     // beide aangespannenR = false;                                                                                                
+                            if((modeL > 1) && modeR > 1) {     // beide aangespannenR = false;                                                                                                
                                 state = FIRE;
                                 }
                             else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten                      
                                 state = FIRE;
                                 }                            
-                            else if(modeL == 2 && L) {      // links ingedrukt = rem verlagen
+                            else if((modeL == 2) && L) {      // links ingedrukt = rem verlagen
                                 if(remPerc>1){
                                     remPerc--;              // Rem percentage verlagen met 1
                                     }
                                 L = false;                                
                                 } 
-                            else if(modeR == 2 && R) {      // Rechts half ingedrukt = rem verhogen
-                                if(remPerc<9){
+                            else if((modeR == 2) && R) {      // Rechts half ingedrukt = rem verhogen
+                                if(remPerc<10){
                                     remPerc++;              // rem percentage verhogen met 1
                                     }
                                 R = false;                        
@@ -305,7 +333,7 @@
                 pwmM1.write(0.5);              // motor aanzetten               
                 bool servoPos = true;                                                
                 while(state == FIRE){           // Zolang in FIRE state
-                    if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(remPerc)/10)){             // Rem goFlag en half rondje = remmen
+                    if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100/**abs(remPerc)/10*/)){             // Rem goFlag en half rondje = remmen
                         remFlag = false;
                         if(servoPos){                       // 
                             servoPos = false;