Diff: main.cpp

Revision:

28:8c90a46b613e

Parent:

27:d37b3a0e0f2b

Child:

29:5a846abba59e

diff -r d37b3a0e0f2b -r 8c90a46b613e main.cpp
--- a/main.cpp	Tue Oct 29 14:22:05 2019 +0000
+++ b/main.cpp	Tue Oct 29 17:14:23 2019 +0000
@@ -30,6 +30,8 @@
 
 
 // variables
+const float pi = 3.1416;
+const float l = 0.535;
 Ticker          TickerStateMachine;
 Ticker          motor_control;
 Ticker          write_scope;
@@ -47,10 +49,10 @@
 volatile bool    pressed_2    = false;
 HIDScope scope(6);
 
-volatile float theta_1;
+//volatile float theta_1;
 //volatile float  theta_error_1;
 volatile float  theta_reference_1;
-volatile float theta_2;
+//volatile float theta_2;
 //volatile float  theta_error_2;
 volatile float  theta_reference_2;
 float Ts    = 0.01;
@@ -58,6 +60,15 @@
 float Ki;
 float Kd;
 
+float theta;
+float thetav_1;
+float thetav_2;
+float theta_1 = (90.0f*pi)/180.0f;
+float theta_2 = (90.0f*pi)/180.0f;
+float x;
+float y;
+volatile float EMGx_velocity=0.02;
+volatile float EMGy_velocity=0;
 
 BiQuad Lowpass_bl ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
 BiQuad Highpass_bl ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
@@ -87,6 +98,14 @@
 float mvc_value_br;
 float mvc_value_leg;
 
+float treshold_bl = 0.5;
+float treshold_br = 0.5;
+float treshold_leg = 0.5;
+
+bool previous_value_emg_leg;
+bool current_value_emg_leg;
+
+
 // functies
 void ledred()
 {
@@ -183,6 +202,25 @@
     return torque;
 }
 
+void AngleVelocity()
+{
+    float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2)));  
+    thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity;
+    thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity;
+}
+
+void JointAngle()
+{
+    float theta_ref1=theta_1+thetav_1*Ts;
+    float theta_ref2=theta_2+thetav_2*Ts;
+    x=cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l;
+    y=sin(theta_ref1)*l+sin(theta_ref1+theta_ref2)*l;  
+    if (sqrt(pow(x,2)+pow(y,2))<1.0f) {
+        theta_1=theta_ref1;
+        theta_2=theta_ref2;
+    }
+}
+
 void CalculateDirectionMotor()
 {
     direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
@@ -216,6 +254,21 @@
 
 bool emg_switch(float treshold, float emg_input) {
     if(emg_input > treshold){
+        current_value_emg_leg = true;
+    } else {
+        current_value_emg_leg = false;
+    }
+    if(current_value_emg_leg == true && previous_value_emg_leg == false) {
+        previous_value_emg_leg = current_value_emg_leg;
+        return true;
+    } else {
+        previous_value_emg_leg = current_value_emg_leg;
+        return false;
+    }
+}
+
+bool emg_trigger(float treshold, float emg_input) {
+    if(emg_input > treshold) {
         return true;
     } else {
         return false;
@@ -496,7 +549,16 @@
 void while_vertical_movement()
 {
     // Do vertical movement stuff
-    if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
+    if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) {
+        EMGy_velocity = -0.02f;
+    }
+    else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) {
+        EMGy_velocity = 0.02f;
+    }    
+    else {
+        EMGy_velocity = 0.0f;
+    }
+    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken
         CurrentState = horizontal_movement;
         StateChanged = true;
     }
@@ -505,7 +567,16 @@
 void while_horizontal_movement()
 {
     // Do horizontal movement stuff
-    if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
+    if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) {
+        EMGx_velocity = -0.02f;
+    }
+    else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) {
+        EMGx_velocity = 0.02f;
+    }    
+    else {
+        EMGx_velocity = 0.0f;
+    }
+    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken
         CurrentState = vertical_movement;
         StateChanged = true;
     }