Diff: main.cpp

Revision:

38:08c8fd55a3bb

Parent:

37:090ba5b1e655

Child:

39:f0f9b3432f43

diff -r 090ba5b1e655 -r 08c8fd55a3bb main.cpp
--- a/main.cpp	Tue Oct 20 13:54:54 2015 +0000
+++ b/main.cpp	Wed Oct 21 10:17:10 2015 +0000
@@ -35,7 +35,7 @@
 int maxCounts = 13000;                   // max richt motor counts Aim motor
 int breakPerc = 0;
 const double servoL = 0.001, servoR = 0.0011;    // Range servo,between servoL en servoR  (= pulsewidth pwm servo) 
-const double tControl = 0.005, tBreak = 0.1, tLcd = 2;     // tickers  
+const double tControl = 0.1, tBreak = 0.1, tLcd = 2;     // tickers  
 const double Fs = 50;                           //Sample frequency Hz
 double t = 1/ Fs;                               // time EMGticker
 double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
@@ -145,15 +145,15 @@
     }  
 void checkAim(){                    // check if Killswitch is on or max counts is reached
     double volt = KS.read();    
-    if(volt> 0.5 || abs(enc2.getPulses()) > (maxCounts -50) || enc2.getPulses() < 0){
+    if(volt> 0.5 /*|| abs(enc2.getPulses()) > (maxCounts -50) || enc2.getPulses() < 0*/){
         pwmM2.write(0);                                 // Aim motor freeze
-        pc.printf("BOEM! CRASH! KASTUK! \r\n");         // Terminal
+        pc.printf("BOEM! CRASH! KASTUK! %f\r\n", KS.read());         // Terminal
         PRINT("BOEM! CRASH!");                          // LCD
         }
     }
 void motorAim(int dir){            // Rotate Aim motor with given direction
     dirM2.write(dir);              
-    pwmM2.write(0.25);              
+    pwmM2.write(0.1);              
     }    
 bool controlAim(){                  // Function to control aim motor with modes
     bool both = false;              // boolean if both modes are 3       
@@ -228,25 +228,29 @@
                 dirM2.write(0);                         // direction aim motor
                 pwmM2.write(0.1);                      // percentage motor power
                 bool calibrated = false;                //                       
-                while(state==CALIBRATE_AIM){                    
-                    pc.printf("Killswitch: %f\r\n", KS.read()); // terminal
-                    if(KS.read() > 0.5){                // Killswitch? //2* gedaan, zodat breuk uit de if-statement is
-                        pwmM2.write(0);                 // Aim motor freeze
-                        enc2.reset();                   // Reset encoder 
-                        PRINT("Aim calibrated");        // LCD                        
-                        calibrated = true;              // 
-                        state = CALIBRATE_PEND;       // next state
-                        //dirM2.write(1);                 // direction aim motor
-                        //pwmM2.write(0.25);              // percentage motor power, turn it on                     
-                        }  
+                while(state==CALIBRATE_AIM){               
+                    if(controlFlag){       // motor regelen op GoFlag
+                        controlFlag = false;
+                        if(KS.read() > 0.8){                // Killswitch? //2* gedaan, zodat breuk uit de if-statement is
+                            pwmM2.write(0);                 // Aim motor freeze
+                            enc2.reset();                   // Reset encoder 
+                            PRINT("Aim calibrated");        // LCD
+                            pc.printf("Aim calibrated\r\n");                        
+                            calibrated = true;              // 
+                            state = CALIBRATE_PEND;       // next state
+                            wait(2);
+                            //dirM2.write(1);                 // direction aim motor
+                            //pwmM2.write(0.25);              // percentage motor power, turn it on                     
+                            }
+                        pc.printf("Killswitch: %f\r\n", KS.read()); // terminal  
                         /*                  
                     if(controlFlag && calibrated){       // motor regelen op GoFlag
                         controlFlag = false;                    
                         if(enc1.getPulses() > maxCounts/2){  // rotate aim motor to midi position
                             pwmM2.write(0);                 // Aim motor freeze
                             state = CALIBRATE_PEND;       // next state
-                            }                      
-                        }    */ 
+                            } */                     
+                        }     
                     }
                 break;
                 }            
@@ -279,16 +283,19 @@
                             pwmM1.write(0);             // Motor 1 freeze
                             enc1.reset();               // encoder 1 reset
                             PRINT("Beam calibrated");
+                            pc.printf("Beam calibrated\r\n");
                             btn = false;                // button op false
                             state = AIM;                // next state                          
                             }
                         else if(modeL == 2){
                             pwmM1.write(0);             // beam freeze
                             PRINT("Flex both full  to continue");               // LCD
+                            pc.printf("Beam freeze\r\n");
                             }
                         else if(modeR == 2){
                             pwmM1.write(0.025);         // beam rotate
                             PRINT("Flex left half  to stop beam");              // LCD
+                            pc.printf("Beam move\r\n");
                             }
                         }             
                     }