Diff: main.cpp

Revision:

12:20ae1d20148d

Parent:

11:08ba9cfc9c8f

Child:

13:5295c4d4be9d

Child:

14:f8e84d287a22

--- a/main.cpp	Fri Apr 19 10:45:45 2019 +0000
+++ b/main.cpp	Fri Apr 19 11:51:14 2019 +0000
@@ -111,7 +111,7 @@
 double Duim_p_t = 0.5; 
 double Bicep_p_t = 0.4; 
 double Dorsaal_p_t = 0.6; 
-double Palmair_p_t = 0.6;
+double Palmair_p_t = 0.5;
 
 //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
 double Duim_p_tL = 0.5; 
@@ -339,15 +339,15 @@
 float Kinematics1(float EMG1)
 {
 
-    if (EMG1 > 0.45f) {
-        stap1 = EMG1*450*Ts;
+    if (Dorsaal == 1 && Duim == 0) {
+        stap1 = 450*Ts;
         Hoeknieuw1 = PolsReference + stap1;
         return Hoeknieuw1;
     }
 
-    else if (EMG1 < -0.45f) {
-        stap1 = EMG1*450*Ts;
-        Hoeknieuw1 = PolsReference + stap1;
+    else if (Dorsaal == 1 && Duim == 1) {
+        stap1 = 450*Ts;
+        Hoeknieuw1 = PolsReference - stap1;
         return Hoeknieuw1;
     }
 
@@ -490,7 +490,7 @@
     pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
     pwm1 =(Pot1*2)-1;
     Input1 = Bicep;
-    Input2 = Dorsaal;
+    Input2 = Palmair;
 
     float Polshoek1 = Kinematics1(Input1);
     float Polshoek4 = Limits1(Polshoek1);
@@ -658,6 +658,7 @@
 
             }
             sample_ticker.detach();
+            timer_calibration.stop();
             // State transition logic: automatisch terug naar motors off.
 
             currentState = HOMING1;
@@ -696,6 +697,8 @@
             // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF
 
             if (!button3) {
+                t.stop();
+                t.reset();
                 currentState = HOMING2  ;
                 stateChanged = true;
                 wait(1);
@@ -712,6 +715,7 @@
 
         case HOMING2:
             // Actions
+            
             if (stateChanged) {
                 // state initialization: white
                 t.start();
@@ -743,10 +747,13 @@
             if (!button2) {
                 currentState = DEMO;
                 stateChanged = true;
+                t.stop();
+                t.reset();
             } else if (!button3) {
                 currentState = MOVEMENT  ;
                 stateChanged = true;
-
+                t.stop();
+                t.reset();
             } else if (t>300) {
                 t.stop();
                 t.reset();
@@ -825,13 +832,13 @@
                 
                 // Servo positie
 
-                if (Palmair == 1 %% Duim = 0) {
+                if (Dorsaal == 1 && Duim == 0) {
                     servo_position1 = Duim_krom;
                     servo_position2 = MWP_krom;
                     servo_position3 = Wijsvinger_krom;
                     led1 = !led1;
                 }
-                if (!But2) {
+                if (Dorsaal == 1 && Duim == 1) {
                     servo_position1 = Duim_recht;
                     servo_position2 = MWP_recht;
                     servo_position3 = Wijsvinger_recht;
@@ -859,12 +866,17 @@
             if (!button2) {
                 currentState = FREEZE;
                 stateChanged = true;
+                threshold_check_ticker.detach();
+                t.stop();
+                t.reset();
             } else if (!button3) {
                 Pwm.detach ();
                 ServoTick.detach();
                 sample_ticker.detach();
                 threshold_check_ticker.detach();
                 sample_timer.detach();
+                t.stop();
+                t.reset();
                 pwmpin2 = 0;
                 pwmpin1 = 0;
                 counts = 0;
@@ -891,19 +903,28 @@
             // Actions
             if (stateChanged) {
                 // state initialization: blue
+                Duim = 0;
+                Bicep = 0;
+                Dorsaal = 0;
+                Palmair = 0;
                 led1 = 1;
                 led2 = 1;
                 led3 = 0;
-
-                wait (1);
-
+                wait(1);
                 stateChanged = false;
             }
 
             // State transition logic: automatisch terug naar MOVEMENT.
-
-            currentState = MOVEMENT;
-            stateChanged = true;
+            if (!button2) {
+                currentState = MOVEMENT;
+                stateChanged = true;
+                counts = 0;
+                        } 
+            else {
+                currentState = FREEZE  ;
+                stateChanged = false;
+                }
+            
             break;
 
     }