Diff: main.cpp

Revision:

26:d9855716ced7

Parent:

25:c97d079e07f3

Child:

27:f62e450bb411

diff -r c97d079e07f3 -r d9855716ced7 main.cpp
--- a/main.cpp	Fri Oct 16 08:04:48 2015 +0000
+++ b/main.cpp	Fri Oct 16 09:12:51 2015 +0000
@@ -6,17 +6,18 @@
 #include "Filterdesigns.h"
 #include "Kalibratie.h"
 #include "Mode.h"
+#include <math.h>
 //--------------------Classes------------------------
 InterruptIn btnSet(PTC6);           // kalibreer knop
 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 emgL(A0), emgR(A1)         // Analog input van emg kabels links en rechts
-AnalogIn ptmrL(A2), ptmrR(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, tickerBtn, tickerRem;   // regelaar button en rem ticker
+Ticker EMGticker, tickerRegelaar, tickerRem;   // regelaar button en rem ticker
 
 // QEI Encoder(poort 1, poort 2, ,counts/rev
     QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
@@ -34,10 +35,9 @@
 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 double tRegelaar = 0.005, tBtn = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
+const double tRegelaar = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
 const double Fs = 200;                   //Sample frequentie Hz
 double t = 1/ Fs;                        // tijd EMGticker
 double ymin = 0, ymax = 0;
@@ -47,7 +47,6 @@
 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 flipLed(DigitalOut& led){              //function to flip one LED   
     led.write(on);
@@ -80,19 +79,41 @@
     
     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.set(w+2,mode);
     scope.send();                               //stuur de waardes naar HIDscope
     return mode;
-    }
+    }    
+int PotReader(AnalogIn& pot){                   // potmeter uitlezen en mode bepalen (thresholds)
+    double volt = pot.read();
+    int mode = 1;    
+    if(volt > 0.8){
+        mode = 3;
+        }
+    else if(volt>0.35 && volt<0.65){
+        mode = 2;
+        }    
+    return mode;    
+    }    
+int defMode(AnalogIn& emg, int w, AnalogIn& pot){       // bepaal mode uit emg en pot
+    int emgMode = EMGfilter(emg, w);
+    int potMode = PotReader(pot);
+    int mode = 1;    
+    if(!(emgMode==1) != !(potMode==1)){         // emgMode = 1 XOR potMode = 1
+        if(emgMode > potMode){                 // maximum van emg en pot
+            mode = emgMode;
+            }
+        else{
+            mode = potMode;
+            }
+        }
+    return mode;       
+    }        
 void setEmgFlag(){
     emgFlag = true;
     }   
 void btnSetAction(){                // Set knop K64F
     btn = true;                     // GoFlag setknop 
-    }  
-void setBtnFlag(){                  // Go flag regelaar Aim motor 
-    btnFlag = true;
-    }
+    } 
 void setRegelaarFlag(){             // Go flag button controle    
     regelaarFlag = true;
     }
@@ -102,7 +123,7 @@
 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
+        pwmM2.write(0);                                 // Aim motor stilzetten
         pc.printf("BOEM! CRASH! KASTUK! \r\n");        
         }
     }
@@ -111,40 +132,42 @@
     pwmM2.write(0.5);              // width aanpassen  
     }    
 bool controlAim(){                  // Function om aim motor aan te sturen
-    bool both = false;              // boolean of beide knoppen ingedrukt zijn       
-
-    if(!(ptmrL.read() >0.3 && ptmrR.read()>0.3) != !(emgModeL!=1 && emgModeR!=1)) {     // Potmeters L en R beide aan XOR EMG beide aangespannen
+    bool both = false;              // boolean of beide knoppen ingedrukt zijn   
+    int modeL = defMode(emgL, 0, potL);
+    int modeR = defMode(emgR, 3, potR);
+   
+    if(modeL>1 && modeR >1) {                           // mode L en mode R beide > 1 
         R = false;
         L = false;
-        pwmM2.write(0);                // Aim motor stilzetten       
+        pwmM2.write(0);                                 // Aim motor stilzetten       
         aimState = OFF;        
         }
-    else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3)) && L) {                 // Potmeter L vol ingedrukt XOR emgModeL is 3
-        both = true;                   // Return true
-        pwmM2.write(0);                // Aim motor stilzetten
+    else if(modeL == 3) {                 
+        both = true;                                    // Return true
+        pwmM2.write(0);                                 // Aim motor stilzetten
         aimState = OFF; 
         L = false;        
         } 
-    else if((!(ptmrR.read() > 0.8) != !(emgModeR == 3)) && aimState!=OFF)  {      // Potmeter R vol ingedrukt XOR emgModeR == 3 
+    else if(modeR == 3 && aimState!=OFF)  {      
         R = false;
-        pwmM2.write(0);                // Aim motor stilzetten        
+        pwmM2.write(0);                                 // Aim motor stilzetten        
         aimState = OFF; 
         PRINT("Motor freeze\r\n");
         } 
-    else if((!(ptmrL.read() > 0.3 && ptmrL.read() < 0.6) != !(emgModeL == 2)) && aimState != CCW && L) {      // Potmeter L half XOR emgModeL ==2 AND richting is niet CCW
-        aimState = CCW;                 // draai 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((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && aimState != CW && R) {      // Potmeter R half XOR emgModeR == 2 AND richting is niet CW
-        aimState = CW;                  // draai 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(ptmrR.read() < 0.1 && emgModeR == 1 && !R){           // R is niet aan
+    if(modeR == 1 && !R){                               // R is niet aan
         R = true;
         }
-    if(ptmrL.read() < 0.1 && emgModeL == 1 && !L){           // L is niet aan
+    if(modeL == 1 && !L){                               // L is niet aan
         L = true;
         }
     return both;            
@@ -153,15 +176,13 @@
     flipLed(ledR);                                      // test of code begint    
     btnSet.mode(PullUp);                                // Button mode
     btnSet.rise(&btnSetAction);                         // Setknop aan functie koppellen    
-    tickerRegelaar.attach(&setRegelaarFlag,tRegelaar);   // ticker regelaar motor
-    tickerBtn.attach(&setBtnFlag,tBtn);                  // ticker knop controle
-    tickerRem.attach(&setRemFlag,tRem);                  // ticker rem
-      
-    EMGticker.attach(&setEmgFlag, t);                      // ticker EMG, 500H
+    tickerRegelaar.attach(&setRegelaarFlag,tRegelaar);  // ticker regelaar motor    
+    tickerRem.attach(&setRemFlag,tRem);                 // ticker rem      
+    EMGticker.attach(&setEmgFlag, t);                   // ticker EMG, 500H
         
     pc.printf("\n\n\n\n\n");
     PRINT("--- NEW GAME ---\r\n");    
-    while(1){                   // Programma door laten lopen
+    while(1){                                           // Programma door laten lopen
         switch(state){
             case KALIBRATE_EMG1: {
                 pc.printf("Kalibrate EMG signal in 5 seconds\r\n");
@@ -181,16 +202,11 @@
                 while(state==KALIBRATE_AIM){ 
                     if(regelaarFlag){       // motor regelen op GoFlag
                         regelaarFlag = false;                  
-                        checkAim();     // Controleer positie                       
+                        checkAim();         // Controleer positie                       
                         }
                         
                     if(emgFlag){            // Go flag EMG sampelen
-                        emgFlag = false;
-                        emgModeL = EMGfilter(emgL,0);
-                        emgModeR = EMGfilter(emgR,3);
-                        }        
-                    if(btnFlag){                // Go flag button controle?                        
-                        btnFlag = false;                                                
+                        emgFlag = false;                                                                        
                         if(controlAim()){           // Buttons met control, if true = beide knoppen = bevestigen
                             enc2.reset();           // resetknop = encoder 1 resetten                        
                             pc.printf("Positie gekalibreerd, kalibreer nu slinger, encoder counts: %i\r\n", enc2.getPulses());
@@ -207,13 +223,13 @@
                 pwmM1.write(0.5);               // Motor aanzetten, laag vermogen
                 btn = false;                        
                 while(state==KALIBRATE_PEND){                    
-                    if(btnFlag){                        
+                    if(regelaarFlag){                        
                         pc.printf("");          // lege regel printen, anders doet setknop het niet
-                        btnFlag = false;                    
+                        regelaarFlag = false;                    
                         if (btn){               // Als setknop ingedrukt is reset
                             pwmM1.write(0);             // Motor 1 stilzetten
                             enc1.reset();               // encoder 1 resetten
-                            pc.printf("Pendulum kalibrated\r\n");
+                            PRINT("Pendulum kalibrated\r\n");
                             btn = false;                // knop op false
                             state = AIM;                // volgende fase                            
                             }
@@ -229,12 +245,7 @@
                         checkAim();     // Controleer positie                       
                         }
                     if(emgFlag){            // Go flag EMG sampelen
-                        emgFlag = false;
-                        emgModeL = EMGfilter(emgL,0);
-                        emgModeR = EMGfilter(emgR,3);
-                        }                   
-                    if(btnFlag){                // Go flag button controle?                                                
-                        btnFlag = false;                                                
+                        emgFlag = false;                                                            
                         if(controlAim()){                                                                   
                             pc.printf("Position choosen, adjust shoot velocity\r\n");                            
                             state = REM;         // volgende state (REM)                       
@@ -247,49 +258,41 @@
                 pc.printf("Set break percentage, current is: %.0f\r\n", remPerc);
                 L = false;
                 R = false;
-                while(state == REM){
-                    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;
-                                       
-                        if((ptmrR.read() < 0.1)) {           // R is niet aan
+                while(state == REM){                      
+                    if(emgFlag){            // Go flag EMG sampelen
+                        emgFlag = false;                        
+                        int modeL = defMode(emgL, 0, potL);
+                        int modeR = defMode(emgR, 3, potR);
+                                                                                    
+                        if(modeR < 2) {           // R is niet aan
                             R = true;
                             }
-                        if((ptmrL.read() < 0.1)) {           // L is niet aan
+                        if(modeL < 2) {           // L is niet aan
                             L = true;
                             }
                         if(L && R){                    
-                            if(!(ptmrL.read() >0.3 && ptmrR.read()>0.3) != !(emgModeL!=1 && emgModeR!=1)) {     // beide aangespannenR = false;                                                                                                
+                            if(modeL > 1 && modeR > 1) {     // beide aangespannenR = false;                                                                                                
                                 state = FIRE;
                                 }
-                            else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3))) {                 // links vol ingedrukt = schieten                      
+                            else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten                      
                                 state = FIRE;
-                                } 
-                            else if(!(ptmrR.read() > 0.8) != !(emgModeR == 3))  {               //rechts vol ingedrukt = schieten                        
-                                state = FIRE;
-                                } 
-                            else if((!(ptmrL.read() > 0.3 && ptmrL.read() < 0.6) != !(emgModeL == 2)) && L) {      // links ingedrukt = rem verlagen
+                                }                            
+                            else if(modeL == 2 && L) {      // links ingedrukt = rem verlagen
                                 if(remPerc>1){
-                                    remPerc--;          // Rem percentage verlagen met 1
+                                    remPerc--;              // Rem percentage verlagen met 1
                                     }
-                                L = false;
-                                
+                                L = false;                                
                                 } 
-                            else if((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && R) {      // Rechts half ingedrukt = rem verhogen
+                            else if(modeR == 2 && R) {      // Rechts half ingedrukt = rem verhogen
                                 if(remPerc<9){
-                                    remPerc++;          // rem percentage verhogen met 1
+                                    remPerc++;              // rem percentage verhogen met 1
                                     }
                                 R = false;                        
-                                }               
-                            
-                            if((ptmrL.read() > 0.3 || ptmrR.read() > 0.3) || (emgModeL != 1 || emgModeR !=1)){               // Print rem scale als een of beide knoppen aan zijn
-                                pc.printf("Current breaking scale: %i\r\n", abs(remPerc)); 
-                                }
-                            }
+                                }                         
+                            if(modeL > 1 || modeR > 1){               // Print rem scale als een of beide knoppen aan zijn
+                                pc.printf("Current breaking scale: %i\r\n", remPerc); 
+                                }                     
+                            }                    
                         }
                     }
                 L = false;
@@ -314,11 +317,10 @@
                             }            
                         }                                    
                     if(regelaarFlag){
-                        regelaarFlag = false;
-                        //pc.printf("Slinger encoder: %i\r\n", enc1.getPulses()%4200);                                                                                                 
+                        regelaarFlag = false;                                                                                                                        
                         if((abs(enc1.getPulses())+50)%4200 < 150){    // Slinger 1 rondje laten draaien 
                             pwmM1.write(0);                      // motor uitzetten
-                            pc.printf("Pendulum on startposition\r\n");
+                            PRINT("Pendulum on startposition\r\n");
                             state = AIM;                        // Aim is volgende fase
                             }                           
                         }