Diff: main.cpp

Revision:

11:08ba9cfc9c8f

Parent:

10:193942c3900a

Child:

12:20ae1d20148d

--- a/main.cpp	Fri Apr 19 09:40:17 2019 +0000
+++ b/main.cpp	Fri Apr 19 10:45:45 2019 +0000
@@ -81,6 +81,7 @@
 //------------- EMG gloabals -------------//
 // Tickers
 Ticker sample_ticker; //ticker voor filteren met 1000Hz
+Ticker sample_timer;
 Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
 Timer timer_calibration; //timer voor EMG Kalibratie
 double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
@@ -110,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.5;
+double Palmair_p_t = 0.6;
 
 //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
 double Duim_p_tL = 0.5; 
@@ -131,10 +132,10 @@
 volatile double threshold4L;
 
 // thresholdreads bools
-int Duim;
-int Bicep;
-int Dorsaal;
-int Palmair;
+bool Duim;
+bool Bicep;
+bool Dorsaal;
+bool Palmair;
 
 // filters
 //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
@@ -354,18 +355,18 @@
         return PolsReference;
     }
 }
-float Kinematics2(float EMG2)
+float Kinematics2(bool Dorsaal)
 {
 
-    if (EMG2 > 0.45f) {
-        stap2 = EMG2*300*Ts;
+    if (Dorsaal == 1 && Duim == 0) {
+        stap2 = 300*Ts;
         Hoeknieuw2 = ElbowReference + stap2;
         return Hoeknieuw2;
     }
 
-    else if (EMG2 < -0.45f) {
-        stap2 = EMG2*300*Ts;
-        Hoeknieuw2 = ElbowReference + stap2;
+    else if (Dorsaal == 1 && Duim == 1) {
+        stap2 = 300*Ts;
+        Hoeknieuw2 = ElbowReference - stap2;
         return Hoeknieuw2;
     }
 
@@ -488,8 +489,8 @@
     Pot1 = pot0.read();
     pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
     pwm1 =(Pot1*2)-1;
-    Input1 = pwm1;
-    Input2 = pwm2;
+    Input1 = Bicep;
+    Input2 = Dorsaal;
 
     float Polshoek1 = Kinematics1(Input1);
     float Polshoek4 = Limits1(Polshoek1);
@@ -566,12 +567,11 @@
     pwm1 =(Pot1*2)-1;
 }
 
-void sample()
-{
-pc.printf("Duim Right = %i\r\n", Duim);
-pc.printf("Bicep Right = %i\r\n",Bicep);
-pc.printf("Dorsaal Left = %i\r\n", Dorsaal);
-pc.printf("Palmair Left = %i\r\n", Palmair);           
+void sample(){
+pc.printf("Duim = %i\r\n", Duim);
+pc.printf("Bicep = %i\r\n",Bicep);
+pc.printf("Dorsaal = %i\r\n", Dorsaal);
+pc.printf("Palmair = %i\r\n", Palmair);           
         
 }
 
@@ -812,7 +812,8 @@
                     Input2 = pwm2;
                     Pwm.attach (PwmMotor, Ts);
                     sample_ticker.attach(&emgsample, 0.001);            // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
-                    
+                    threshold_check_ticker.attach(&threshold_check, 0.01);
+                    sample_timer.attach(&sample, 0.5);    
                     servo_position1 = Duim_krom;
                     servo_position2 = MWP_krom;
                     servo_position3 = Wijsvinger_krom;
@@ -824,7 +825,7 @@
                 
                 // Servo positie
 
-                if (!But1) {
+                if (Palmair == 1 %% Duim = 0) {
                     servo_position1 = Duim_krom;
                     servo_position2 = MWP_krom;
                     servo_position3 = Wijsvinger_krom;
@@ -862,6 +863,8 @@
                 Pwm.detach ();
                 ServoTick.detach();
                 sample_ticker.detach();
+                threshold_check_ticker.detach();
+                sample_timer.detach();
                 pwmpin2 = 0;
                 pwmpin1 = 0;
                 counts = 0;
@@ -873,6 +876,8 @@
                 Pwm.detach ();
                 ServoTick.detach();
                 sample_ticker.detach();
+                sample_timer.detach();
+                threshold_check_ticker.detach();
                 counts = 0;
                 currentState = HOMING1  ;
                 stateChanged = true;