Diff: main.cpp

Revision:

40:35c7c60d7262

Parent:

39:f0f9b3432f43

Child:

41:91c8c39d7a33

diff -r f0f9b3432f43 -r 35c7c60d7262 main.cpp
--- a/main.cpp	Wed Oct 21 13:23:30 2015 +0000
+++ b/main.cpp	Wed Oct 21 14:17:22 2015 +0000
@@ -34,7 +34,7 @@
 const int on = 0, off = 1;               // on off
 int maxCounts = 13000;                   // max richt motor counts Aim motor
 int breakPerc = 0;
-const double servoL = 0.001, servoFree = 0.002;    // Range servo,between servoL en servoR  (= pulsewidth pwm servo) 
+const double servoBreak = 0.0017, servoFree = 0.002;    // Range servo,between servoL en servoR  (= pulsewidth pwm servo) 
 const double tControl = 0.1, tBreak = 0.1, tLcd = 2;     // tickers  
 const double Fs = 200;                           //Sample frequency Hz
 double t = 1/ Fs;                               // time EMGticker
@@ -406,26 +406,25 @@
                 pc.printf("Shooting\r\n");
                 PRINT("FIRING");
                 servo.pulsewidth(servoL);       // BREAK release
-                pwmM1.write(0.25);              // beam motor on               
-                bool servoPos = true;                                                
-                while(state == FIRE){           // while in FIRE state
-                    if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){             // BREAK goFlag half rotation beam = breaking
-                        breakFlag = false;
-                        if(servoPos){                        
-                            servoPos = false;
-                            servo.pulsewidth(servoL);       // left   
+                pwmM1.write(0.25);              // beam motor on
+                servo.pulsewidth(servoBreak);   // Servo to break position              
+                bool breaking = true;           // boolean breaking?                                             
+                while(state == FIRE){           // while in FIRE state                                      
+                    if(controlFlag{             // BREAK goFlag half rotation beam = breaking
+                        controlFlag = false;
+                        if(breaking){ // encoder is 0 on 100 counts before 12 o'clock  
+                            if((abs(enc1.getPulses())-100)%4200 > (2100*abs(breakPerc)/10)){
+                                servo.pulsewidth(servoFree);   // Servo to free position
+                                breaking = false;
+                                }
                             }
-                        else{
-                            servoPos = true;
-                            servo.pulsewidth(servoR);       // right
-                            }            
-                        }                                    
-                    if(controlFlag){
-                        controlFlag = false;                                                                                                                        
-                        if((abs(enc1.getPulses())+50)%4200 < 150){    // rotate beam one revolution
-                            pwmM1.write(0);                      // motor beam off
-                            pc.printf("Beam on startposition\r\n");
-                            state = AIM;                        // Next stage
+                            else{       // Not breaking
+                                if((abs(enc1.getPulses())+150)%4200 < 150){     // rotated 1 rev?
+                                    pwmM1.write(0);                             // motor beam off
+                                    pc.printf("Beam on startposition\r\n");     // terminal
+                                    state = AIM;                                // Next stage
+                                    }
+                                }                       
                             }                           
                         }                       
                     }