Diff: main.cpp

Revision:

47:959ef2792024

Parent:

46:ed8ada80ba17

Child:

48:07e3cf7dc819

--- a/main.cpp	Fri Oct 23 10:23:42 2015 +0000
+++ b/main.cpp	Fri Oct 23 13:37:26 2015 +0000
@@ -34,6 +34,7 @@
 //-------------------------------Variables--------------------------------------------------------------------- 
 int maxCounts = 13000;                      // max richt motor counts Aim motor
 int breakPerc = 0;
+volatile int modeL = 1, modeR = 1;
 const double servoBreak = 0.00165, servoFree = 0.002;    // Range servo,between servoL en servoR  (= pulsewidth pwm servo) 
 const double tControl = 0.05, tLcd = 2;      // ticker time (sec)  
 const double Fs = 200;                      //Sample frequency Hz
@@ -45,7 +46,6 @@
 volatile bool btn = false;                  // Button is pressed?
 volatile bool controlFlag = false, btnFlag = false, emgFlag = false, lcdFlag = false, runEmg = true;     // Go flags
 bool boolBrake = false;
-
 //----------------------------Functions-----------------------------------------------------------------------
 void PRINT(const char* text){                
     lcd.cls();                              // clear LCD scherm
@@ -92,11 +92,11 @@
     int mode = 1;
     if(side){
         double y = FilterdesignsLeft(u);                // filter signal left EMG                               
-        mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3)
+        mode = Mode(modeL, y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3)
         }
     else {
         double y = FilterdesignsRight(u);                                               
-        mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR);   // right EMG
+        mode = Mode(modeR, y, thresholdlowR, thresholdmidR, thresholdhighR);   // right EMG
         }          
     return mode;
     }    
@@ -160,8 +160,8 @@
     }    
 bool controlAim(){                  // Function to control aim motor with modes
     bool both = false;              // boolean if both modes are 3       
-    int modeL = defMode(emgL, potL, true);      // determine mode left
-    int modeR = defMode(emgR, potR, false);     // determine mode right
+    modeL = defMode(emgL, potL, true);      // determine mode left
+    modeR = defMode(emgR, potR, false);     // determine mode right
     
     scope.set(0, modeL);
     scope.set(1, modeR);    
@@ -330,8 +330,8 @@
                     if(emgFlag){            // Go flag EMG sampelen                        
                         
                         emgFlag = false;                        
-                        int modeL = defMode(emgL, potL, true);
-                        int modeR = defMode(emgR, potR, false);
+                        modeL = defMode(emgL, potL, true);
+                        modeR = defMode(emgR, potR, false);
                                                                                                
                         if((modeL > 1) && (modeR > 1) && L && R) {          // both modes  > 1                                                                                             
                             state = FIRE;                                                            
@@ -382,7 +382,7 @@
             case FIRE: {            
                 pc.printf("Shooting\r\n");      // terminal
                 PRINT("FIRING");                // lcd        
-                pwmM1.write(0.25);               // beam motor on
+                pwmM1.write(0.3);               // beam motor on
                 bool breaking = false;                 
                 if(boolBrake){
                     servo.pulsewidth(servoBreak);   // Servo to break position
@@ -400,7 +400,7 @@
                                 }
                             }
                         if(!breaking || !boolBrake){          // Not breaking
-                            if(counts%4200 < 100){                          // rotated 1 rev?
+                            if((counts-100)%4200 < 100){                          // rotated 1 rev?
                                 pwmM1.write(0);                             // motor beam off
                                 pc.printf("Beam on startposition\r\n");     // terminal
                                 state = AIM;                                // Next stage