Diff: main.cpp

Revision:

13:0adbf6a5de37

Parent:

12:12c162dc8893

Child:

14:a27de0ddf09f

diff -r 12c162dc8893 -r 0adbf6a5de37 main.cpp
--- a/main.cpp	Fri Oct 28 11:46:44 2016 +0000
+++ b/main.cpp	Fri Oct 28 11:58:56 2016 +0000
@@ -77,11 +77,40 @@
 void Lift_Controller();
 void Ticker_Flag();
 
+float statechange_threshold = 0.6;        /////// moet gekopieerd worden  t/m-->
+bool binary_norm_emg_2 = 0;        
+bool binary_norm_emg_2_previous = 0;
+states scoopstate = STATE_MOVE;
+bool state_resettime_value = 1;
+Timeout state_resettime;
+DigitalOut LED_STATE_MOVE(D15);
+DigitalOut LED_STATE_GRAB(D14);
+Timeout controlstarterTimeout;
+
+void set2true(){
+    state_resettime_value = 1;
+}
+void change_scoopstate(){
+    switch(scoopstate){
+        case STATE_MOVE:
+            scoopstate = STATE_GRAB;
+            break;
+        case STATE_GRAB:
+            scoopstate = STATE_MOVE;
+            break;
+    }
+    state_resettime.attach(&set2true, 1);
+    state_resettime_value = 0;
+}
+void start_controlling(){
+    Controller.attach(&Ticker_Flag,1/Frequency);
+}                                       ///////////////hier
+
 int main()
 {
     Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
     Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
-    Controller.attach(&Ticker_Flag,1/Frequency);
+
     pc.baud(9600);
     
     led.write(1);
@@ -100,6 +129,7 @@
     
     change_state.attach( &calibrate,1);
     change_state2.attach( &run,11);
+    controlstarterTimeout.attach(&start_controlling,11); ///// ook nieuw
     emgSampleTicker.attach( &emgSample, 0.005); //200Hz
     
 //    treshold = (cali_max-cali_min)*treshold_multiplier;
@@ -114,10 +144,30 @@
         }
         
         if (Controller_Flag == true){
-            Slide_Controller();
-            Lift_Controller();
-            control_servo(Norm_EMG_0);
+            if(Norm_EMG_2 > statechange_threshold){
+                binary_norm_emg_2 = 1;
+            }else{
+                binary_norm_emg_2 = 0;
+            }
+            if((binary_norm_emg_2 != binary_norm_emg_2_previous)&&binary_norm_emg_2&&state_resettime_value){
+                change_scoopstate();
+            }
+            binary_norm_emg_2_previous = binary_norm_emg_2;
             
+            switch(scoopstate){
+                case STATE_MOVE:
+                    Slide_Controller();
+                    LED_STATE_MOVE = 1;
+                    LED_STATE_GRAB = 0;
+                    break;
+                case STATE_GRAB:
+                    Lift_Controller();
+                    control_servo(Norm_EMG_0);
+                    LED_STATE_GRAB = 1;
+                    LED_STATE_MOVE = 0;
+                    break;
+            }
+                
             Controller_Flag = false;
         }
 
@@ -177,7 +227,7 @@
     Lift_Position =     Lift_Angle*Lift_Radius;
     
     if(Norm_EMG_0 > 0.6 && Lift_Timeout_Switch == true){
-        //Lift_Switch = !Lift_Switch;
+        Lift_Switch = !Lift_Switch;
         Lift_Timeout_Switch = false;
         Lift_Timeout.attach(Lift_Timeout_Return, 1);
     }