Diff: main.cpp

Revision:

30:8ae855348d22

Parent:

29:9610df479f89

Child:

31:074b9d03d816

--- a/main.cpp	Sat Oct 17 14:13:57 2015 +0000
+++ b/main.cpp	Mon Oct 19 10:09:42 2015 +0000
@@ -15,7 +15,7 @@
 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
+HIDScope scope(6);                  
 Ticker EMGticker, tickerRegelaar, tickerRem;   // regelaar button en rem ticker
 
 // QEI Encoder(poort 1, poort 2, ,counts/rev
@@ -41,6 +41,7 @@
 double t = 1/ Fs;                        // tijd EMGticker
 double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
 double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
+double yL = 0, yR = 0;                  // y waarden EMG links en 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
@@ -60,15 +61,15 @@
 void EMGkalibratieL(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
     LedB = 1;
     Init();    
-    double ymin = KalibratieMin(emgL);
+    double ymin = KalibratieMin(emgL, true);
     wait(1);
-    double ymax = KalibratieMax(emgL);
+    double ymax = KalibratieMax(emgL, true);
     
     if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
         ymin = 10;
         ymax = 10;
         }    
-    thresholdlowL = 8 * ymin; // standaardwaarde
+    thresholdlowL = 4 * ymin; // standaardwaarde
     thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
     thresholdhighL = 0.8 * ymax;
 
@@ -77,28 +78,29 @@
 void EMGkalibratieR(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
     LedB = 1;
     Init();    
-    double ymin = KalibratieMin(emgR);
+    double ymin = KalibratieMin(emgR, false);
     wait(1);
-    double ymax = KalibratieMax(emgR);
+    double ymax = KalibratieMax(emgR, false);
     
     if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
         ymin = 10;
         ymax = 10;
         }        
-    thresholdlowR = 8 * ymin; // standaardwaarde
+    thresholdlowR = 4 * ymin; // standaardwaarde
     thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
     thresholdhighR = 0.8 * ymax;
 
     pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
     }
 int EMGfilter(AnalogIn& emg, bool side){    
-    double u = emg.read();                      // uitlezen emg signaal
-    double y = Filterdesigns(u);                // signaal filteren
+    double u = emg.read();                      // uitlezen emg signaal (linker of rechter EMG)    
     int mode = 1;
-    if(side){                                   // linker EMG  
+    if(side){
+        double y = FilterdesignsLeft(u);                // signaal filteren                                   // linker EMG  
         mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
         }
-    else {                                      // rechter EMG
+    else {
+        double y = FilterdesignsRight(u);                // signaal filteren                                   // rechter EMG
         mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
         }          
     return mode;
@@ -149,10 +151,11 @@
     }
 void motorAim(int dir){            // Aim motor laten draaien met gegeven direction 
     dirM2.write(dir);              // richting
-    pwmM2.write(0.5);              // width aanpassen  
+    pwmM2.write(0.25);              // width aanpassen  
     }    
 bool controlAim(){                  // Function om aim motor aan te sturen
     bool both = false;              // boolean of beide knoppen ingedrukt zijn   
+       
     int modeL = defMode(emgL, potL, true);
     int modeR = defMode(emgR, potR, false);    
     
@@ -167,7 +170,7 @@
         L = true;        
         }
                 
-    if((modeL>2) && (modeR >2)) {                       // mode L en mode R beide > 2 
+    if((modeL>2) && (modeR >2 && R && L)) {                       // mode L en mode R beide > 2 
         both = true;                                    // Return true
         pwmM2.write(0);                                 // Aim motor stilzetten
         aimState = OFF;        
@@ -245,12 +248,14 @@
                 pc.printf("Kalibrate pendulum motor with setknop\r\n");            
                 pwmM1.period(1/100000);         // aanstuurperiode motor 1      
                 servo.period(0.02);             // 20ms period aanstuurperiode
-                pwmM1.write(0.5);               // Motor aanzetten, laag vermogen
+                pwmM1.write(0.25);               // Motor aanzetten, laag vermogen
                 btn = false;                        
                 while(state==KALIBRATE_PEND){                    
                     if(regelaarFlag){                        
                         pc.printf("");          // lege regel printen, anders doet setknop het niet
-                        regelaarFlag = false;                    
+                        regelaarFlag = false; 
+                        
+                                           
                         if (btn){               // Als setknop ingedrukt is reset
                             pwmM1.write(0);             // Motor 1 stilzetten
                             enc1.reset();               // encoder 1 resetten
@@ -298,9 +303,13 @@
                         if(L && R){                    
                             if((modeL > 1) && modeR > 1) {     // beide aangespannenR = false;                                                                                                
                                 state = FIRE;
+                                R = false;
+                                L = false;
                                 }
                             else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten                      
                                 state = FIRE;
+                                R = false;
+                                L = false;
                                 }                            
                             else if((modeL == 2) && L) {      // links ingedrukt = rem verlagen
                                 if(remPerc>1){
@@ -327,10 +336,10 @@
             case FIRE: {            
                 pc.printf("Shooting\r\n");
                 servo.pulsewidth(servoL);       // schrijfrem release
-                pwmM1.write(0.5);              // motor aanzetten               
+                pwmM1.write(0.25);              // 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;