Diff: main.cpp

Revision:

33:1da600f06862

Parent:

32:7355524d862f

diff -r 7355524d862f -r 1da600f06862 main.cpp
--- a/main.cpp	Wed Oct 30 18:17:00 2019 +0000
+++ b/main.cpp	Thu Oct 31 10:05:31 2019 +0000
@@ -53,7 +53,7 @@
 
 volatile float  theta_ref1;
 volatile float  theta_ref2;
-float Ts    = 0.01;
+float Ts    = 0.01f;
 float Kp;
 float Ki;
 float Kd;
@@ -65,10 +65,8 @@
 float torque_2;
 float x;
 float y;
-volatile float EMGx_velocity=0.02;
-volatile float EMGy_velocity=0;
-char beweging;
-
+volatile float EMGx_velocity = 0.0f;
+volatile float EMGy_velocity = 0.0f;
 
 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);
@@ -105,7 +103,7 @@
 bool previous_value_emg_leg;
 bool current_value_emg_leg;
 
-
+volatile char command;
 // functies
 void ledred()
 {
@@ -156,6 +154,36 @@
     led_red=1;
 }
 
+bool get_command_a() {
+    command = pc.getcNb();
+    if (command == 'a') {
+        pc.printf("a is ingedrukt! \n\r");   
+        return true;
+    } else {
+    return false;
+    }
+}
+
+bool get_command_d () {
+    command = pc.getcNb();
+    if (command == 'd') {
+        pc.printf("d is ingedrukt! \n\r");   
+        return true;
+    } else {
+    return false;
+    }
+}
+
+bool get_command_s () {
+    command = pc.getcNb();
+    if (command == 's') {
+        pc.printf("s is ingedrukt! \n\r");   
+        return true;
+    } else {
+    return false;
+    }
+}
+
 void Controller()
 {
     float K     = 1;
@@ -469,8 +497,11 @@
 void while_demo_mode()
 {
     // Do demo mode stuff
+    treshold_bl = 1.1f;
+    treshold_br = 1.1f;
+    treshold_leg = 1.1f;
     if ((pressed_1) || (pressed_2)) {
-        CurrentState = emg_calibration;
+        CurrentState = vertical_movement;
         StateChanged = true;
     }
 }
@@ -571,7 +602,7 @@
     
     else {
         EMGy_velocity = 0.0f;
-        pc.printf("beweging %f \n\r", EMGy_velocity);
+        //pc.printf("beweging %f \n\r", EMGy_velocity);
     }
 
     if ((pressed_1) || (pressed_2) /*|| (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))*/) { // EMG gebaseerde threshold aanmaken
@@ -584,22 +615,25 @@
 void while_horizontal_movement()
 {
     // Do horizontal movement stuff
-    /*
-    if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) {
+    
+    if ((emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) || (get_command_a())){
         EMGx_velocity = -0.02f;
+        pc.printf("beweging %f \n\r", EMGx_velocity);
     }
-    else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) {
+    if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)) || (get_command_d())) {
         EMGx_velocity = 0.02f;
+        pc.printf("beweging %f \n\r", EMGx_velocity);
     }    
-    else {
+    if ((!(emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) && !(emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)))) && (!get_command_a() && !get_command_d())) {
         EMGx_velocity = 0.0f;
+        pc.printf("beweging %f \n\r", EMGx_velocity);
     }
-    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken
+    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg))) || (get_command_s())) { // EMG gebaseerde threshold aanmaken
         CurrentState = vertical_movement;
         StateChanged = true;
     }
-    */
     
+    /*
     if (beweging == 'a') {
         EMGx_velocity = -0.02f;
         pc.printf(" you pressed %c \n\r" , beweging);
@@ -613,10 +647,10 @@
         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
+    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;
-    }
+    }*/
 }    
 
 
@@ -692,13 +726,6 @@
     PWM_motor_2.period_ms(10);
     motor_control.attach(&MotorControl, Ts);
     write_scope.attach(&WriteScope, 0.01);
-    
-    void CalculateDirectionMotor();
-    void MotorControl();
-    
-    
-    TickerStateMachine.attach(StateMachine,1.00f);
-    beweging = pc.getc();
     while(true) {
         StateMachine();
     }