Diff: main.cpp

Revision:

42:cafa56da9ed2

Parent:

41:8640b5782bc7

Child:

44:0056118c01b2

--- a/main.cpp	Thu Nov 01 16:05:41 2018 +0000
+++ b/main.cpp	Thu Nov 01 16:15:30 2018 +0000
@@ -230,7 +230,10 @@
         debug_forward_kinematics = true;
     }
     
-    if (emg_1.get_is_envelope_over_threshold()) {
+    bool emg_1_state = emg_1.get_is_envelope_over_threshold();
+    bool emg_2_state = emg_2.get_is_envelope_over_threshold();
+    
+    if (emg_1_state) {
         led_green = 0;
 
         if (control_goes_up) {
@@ -242,7 +245,7 @@
         led_green = 1;
     }
     
-    if (emg_2.get_is_envelope_over_threshold()) {
+    if (emg_2_state) {
         led_blue = 0;
         if (control_goes_right) {
             speed_x = max_speed;
@@ -316,12 +319,17 @@
     main_motor.set_target_angle(main_target);
     sec_motor.set_target_angle(sec_target);
 
+    /*
     screen.get_screen_handle()->setTextCursor(0, 0);
     screen.get_screen_handle()->printf("M_a: %.6f    \n", main_cur_angle);
     screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_cur_angle);
     screen.get_screen_handle()->printf("Vx:  %.6f \n", main_target);
     screen.get_screen_handle()->printf("Vy:  %.6f ", sec_target);
     screen.display();
+    */
+    
+    screen.display_emg_state(emg_1_state, emg_2_state);
+    screen.display();
 
     if (ud_button.has_just_been_pressed()) {
         control_goes_up = !control_goes_up;