Diff: main.cpp

Revision:

24:ddd69385b55f

Parent:

23:855c4bcb2284

Child:

25:c97d079e07f3

--- a/main.cpp	Thu Oct 15 10:16:44 2015 +0000
+++ b/main.cpp	Thu Oct 15 11:13:32 2015 +0000
@@ -1,34 +1,69 @@
 #include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "TextLCD.h"
 #include "HIDScope.h"
 #include "Filterdesigns.h"
 #include "Kalibratie.h"
-#include "MODSERIAL.h" //bugfix
 #include "Mode.h"
-
-AnalogIn    emg(A0); //Analog input van emg kabels
+//--------------------Classes------------------------
+InterruptIn btnSet(PTC6);           // kalibreer knop
+DigitalOut ledR(LED_RED);           // 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 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;
-MODSERIAL   pc(USBTX, USBRX); //bugfix
 DigitalOut  LedBlue(LED3);
 DigitalIn   button(PTA4);
 
+// QEI Encoder(poort 1, poort 2, ,counts/rev
+    QEI enc1 (D13, D12, NC, 64);
+    QEI 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
+ 
+enum aimFase {OFF, CW, CCW};         // Aim motor mogelijkheden
+uint8_t aimState = OFF;              // mogelijkheid begin
+//-------------------------------Variables---------------------------------------------------------------------   
+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;
+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;
-bool readymin = 0;
-double ymin;
-double ymax;
-double thresholdlow;
-double thresholdmid;
-double thresholdhigh;
-bool go_flag = 0;
+bool readymax = 0, readymin = 0;
+double ymin, ymax;
+double thresholdlow, thresholdmid, thresholdhigh;
+bool go_flag = false;
 
-void EMGkalibratie()
-{
+//----------------------------Functions-----------------------------------------------------------------------
+void EMGkalibratie(){
     LedBlue = 1;
-    Init();
+    Init();    
     ymin = KalibratieMin(readymin);
     wait(1);
     ymax = KalibratieMax(readymax);
@@ -39,43 +74,274 @@
     thresholdhigh = 0.8 * ymax;
 
     pc.printf("ymax = %f en ymin = %f \n",ymax, ymin); //bugfix
-}
+    }
 
-void EMGfilter()
-{
+void EMGfilter(){
     //uitlezen emg signaal
     double u = emg.read();
     double y = Filterdesigns(u);
     //pc.printf("%f \n",y); //bugfix
     // Plotten in HIDscope
-    int mode = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (aan, uit, middel)
+    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,mode);
+    scope.set(2,emgModeL);
     scope.send(); //stuur de waardes naar HIDscope
-}
-
-void Go_flag(){
-    go_flag = 1;
     }
 
-int main()
-{
-    //pc.baud(115200);
-    EMGticker.attach(&Go_flag, t); //500H
-    while(1) {
-        if(go_flag) {   // als deze true is dan gaat hij de onderstaande gebeuren aan
-            go_flag = 0;
-            if(button == 0) {
-                readymax = 0;
-                readymin = 0;
-            } 
-            else if(readymax == 0 || readymin == 0) {
+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 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;
+    }
+void setRemFlag(){                  // Go flag rem 
+    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
+        pc.printf("BOEM! CRASH! KASTUK! \r\n");        
+        }
+    }
+void motorAim(int dir){            // Aim motor laten draaien met gegeven direction 
+    dirM2.write(dir);              // richting
+    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
+        R = false;
+        L = false;
+        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
+        aimState = OFF; 
+        L = false;        
+        } 
+    else if((!(ptmrR.read() > 0.8) != !(emgModeR == 3)) && aimState!=OFF)  {      // Potmeter R vol ingedrukt XOR emgModeR == 3 
+        R = false;
+        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
+        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
+        PRINT("Turn positive (CW)\r\n");
+        motorAim(1);
+        }
+    if(ptmrR.read() < 0.1 && emgModeR == 1 && !R){           // R is niet aan
+        R = true;
+        }
+    if(ptmrL.read() < 0.1 && emgModeL == 1 && !L){           // L is niet aan
+        L = true;
+        }
+    return both;            
+    }               
+int main(){
+    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(&Go_flag, t);                      // ticker EMG, 500H
+    
+    lcd.cls();
+    lcd.printf("NEW GAME \n HELLO!");
+     
+    pc.printf("\n\n\n\n\n");
+    pc.printf("--- 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.");
+                
                 EMGkalibratie();
-            } 
-            else if(readymax == 1 && readymin == 1) {
-                EMGfilter();
+                                   
+                state = KALIBRATE_AIM;
+                break;
+                }
+            case KALIBRATE_AIM: {
+                pwmM2.period(1/100000);            // aanstuurperiode motor 2
+                PRINT("Kalibrate aim motor\r\n");
+                wait(1);
+                pc.printf("Use L and R to move pendulum to equilibruim position\r\n L+R = OK\r\n");                                
+                while(state==KALIBRATE_AIM){ 
+                    if(regelaarFlag){       // motor regelen op GoFlag
+                        regelaarFlag = false;                  
+                        checkAim();     // Controleer positie                       
+                        }
+                        
+                    if(go_flag){            // Go flag EMG sampelen
+                        go_flag = false;
+                        EMGfilter();
+                        }        
+                    if(btnFlag){                // Go flag button controle?                        
+                        btnFlag = 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());
+                            state = KALIBRATE_PEND;         // volgende state                        
+                            }
+                        }
+                    }
+                break;
+                }            
+            case KALIBRATE_PEND: {
+                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
+                btn = false;                        
+                while(state==KALIBRATE_PEND){                    
+                    if(btnFlag){                        
+                        pc.printf("");          // lege regel printen, anders doet setknop het niet
+                        btnFlag = 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");
+                            btn = false;                // knop op false
+                            state = AIM;                // volgende fase                            
+                            }
+                        }             
+                    }
+                break;
+                }
+            case AIM: {                  
+                pc.printf("Aim with EMG\r\n");            
+                while(state == AIM){               
+                    if(regelaarFlag){       // motor regelen op GoFlag
+                        regelaarFlag = false;                  
+                        checkAim();     // Controleer positie                       
+                        }
+                    if(go_flag){            // Go flag EMG sampelen
+                        go_flag = false;
+                        EMGfilter();
+                        }                   
+                    if(btnFlag){                // Go flag button controle?                                                
+                        btnFlag = false;                                                
+                        if(controlAim()){                                                                   
+                            pc.printf("Position choosen, adjust shoot velocity\r\n");                            
+                            state = REM;         // volgende state (REM)                       
+                            }                      
+                        }
+                    }            
+                break;
+                }
+            case REM: {                
+                pc.printf("Set break percentage, current is: %.0f\r\n", remPerc);
+                L = false;
+                R = false;
+                while(state == REM){
+                    if(go_flag){                    // Go flag EMG sampelen
+                        go_flag = false;
+                        EMGfilter();
+                        }
+                    if(btnFlag){                    // Go flag button controle?  == rem aansturen                      
+                        btnFlag = false;
+                                       
+                        if((ptmrR.read() < 0.1)) {           // R is niet aan
+                            R = true;
+                            }
+                        if((ptmrL.read() < 0.1)) {           // 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;                                                                                                
+                                state = FIRE;
+                                }
+                            else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3))) {                 // links 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
+                                if(remPerc>1){
+                                    remPerc--;          // Rem percentage verlagen met 1
+                                    }
+                                L = false;
+                                
+                                } 
+                            else if((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && R) {      // Rechts half ingedrukt = rem verhogen
+                                if(remPerc<9){
+                                    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)); 
+                                }
+                            }
+                        }
+                    }
+                L = false;
+                R = false;                              
+                break;
+                }                    
+            case FIRE: {            
+                pc.printf("Shooting\r\n");
+                servo.pulsewidth(servoL);       // schrijfrem release
+                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
+                        remFlag = false;
+                        if(servoPos){                       // 
+                            servoPos = false;
+                            servo.pulsewidth(servoL);       // links   
+                            }
+                        else{
+                            servoPos = true;
+                            servo.pulsewidth(servoR);       // rechts
+                            }            
+                        }                                    
+                    if(regelaarFlag){
+                        regelaarFlag = false;
+                        //pc.printf("Slinger encoder: %i\r\n", enc1.getPulses()%4200);                                                                                                 
+                        if((abs(enc1.getPulses())+50)%4200 < 150){    // Slinger 1 rondje laten draaien 
+                            pwmM1.write(0);                      // motor uitzetten
+                            pc.printf("Pendulum on startposition\r\n");
+                            state = AIM;                        // Aim is volgende fase
+                            }                           
+                        }                       
+                    }                                                     
+                break;
+                }
             }
         }
-    }
-}
+    }          
\ No newline at end of file