Diff: main.cpp

Revision:

54:062b9f36edf2

Parent:

53:4a0857fbbb34

Child:

55:a435e46299ea

--- a/main.cpp	Wed Oct 28 13:10:26 2015 +0000
+++ b/main.cpp	Wed Oct 28 13:22:20 2015 +0000
@@ -30,8 +30,10 @@
 uint8_t state = CALIBRATE_EMG;       // first state program
 enum aimFase {OFF, CW, CCW};         // Aim motor possible states
 uint8_t aimState = OFF;              // first state aim motor 
+enum calFase {cR, cL, mid};         
+uint8_t calState = cR;              
 //-------------------------------Variables--------------------------------------------------------------------- 
-int maxCounts = 13000;                      // max richt motor counts Aim motor
+int maxCounts = 0;                      // max richt motor counts Aim motor
 int breakPerc = 0;
 volatile int modeL = 1, modeR = 1;
 const double servoBreak = 0.0016, servoFree = 0.002;    // Range servo,between servoL en servoR  (= pulsewidth pwm servo) 
@@ -232,28 +234,39 @@
                 dirM2.write(0);                         // direction aim motor
                 pwmM2.write(0.2);                       // percentage motor power
                 bool calibrated = false;                                       
-                while(state==CALIBRATE_AIM){               
+                while(state==CALIBRATE_AIM){
                     if(controlFlag){       
                         controlFlag = false;
-                        if(!calibrated){                        // Not calibrated
-                            if((ksRight.read()>0.8)){           // Killswitch? 
-                                pwmM2.write(0);                 // Aim motor freeze
-                                enc2.reset();                   // Reset encoder 
-                                PRINT("Aim calibrated");        // LCD
-                                pc.printf("Aim calibrated\r\n");// Terminal                        
-                                calibrated = true;                          
-                                pc.printf("Go to midi position\r\n");
-                                dirM2.write(1);                 // direction aim motor
-                                pwmM2.write(0.2);               // percentage motor power, turn it on                     
+                        
+                        switch(calState){
+                            case cR:{                               // calibrate right killswitch
+                                if((ksRight.read()>0.95)){           // Killswitch? 
+                                    pwmM2.write(0);                 // Aim motor freeze
+                                    enc2.reset();                   // Reset encoder                            
+                                    dirM2.write(1);                 // direction aim motor
+                                    pwmM2.write(0.2);               // percentage motor power, turn it on
+                                    calState = Cl;
+                                    break;                                                         
+                                    }
                                 }
-                            }                                    
-                        if(calibrated) {                        // Calibrated                           
-                            //checkAim();                         // Check killswitches                                               
-                            if(abs(enc2.getPulses()) > 700){    // rotate aim motor to midi position
-                                pwmM2.write(0);                 // Aim motor freeze
-                                state = CALIBRATE_BEAM;         // next state
-                                }                     
-                            }     
+                            case cL:{
+                                if((ksLeft.read()>0.95)){           // calibrate left killswitch
+                                    pwmM2.write(0);                 // Aim motor freeze
+                                    maxCounts = abs(enc2.getPulses());                            
+                                    dirM2.write(0);                 // direction aim motor
+                                    pwmM2.write(0.2);               // percentage motor power, turn it on
+                                    calState = mid;
+                                    break;                                                         
+                                    }
+                                }
+                            case mid:{
+                                if(abs(enc2.getPulses()) < (maxCounts/2)){    // rotate aim motor to midi position
+                                    pwmM2.write(0);                 // Aim motor freeze
+                                    state = CALIBRATE_BEAM;         // next state
+                                    break;
+                                    } 
+                                }
+                            }              
                         }
                     }
                 lcd.cls();