Diff: main.cpp

Revision:

41:8640b5782bc7

Parent:

40:7e8d0632757c

Child:

42:cafa56da9ed2

--- a/main.cpp	Thu Nov 01 15:46:25 2018 +0000
+++ b/main.cpp	Thu Nov 01 16:05:41 2018 +0000
@@ -74,7 +74,7 @@
         screen.get_screen_handle()->printf("          V          ");
         screen.display();
     }
-    
+
     if (ud_button.has_just_been_pressed()) {
         current_state = calib_motor;
     }
@@ -88,7 +88,7 @@
     static int sec_iterations_not_moving;
     static bool main_is_calibrated;
     static bool sec_is_calibrated;
-      
+
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
@@ -97,7 +97,7 @@
         led_blue = 1;
         screen.clear_display();
         screen.display_state_name("Motor calibration");
-        
+
         main_last_angle = -10;
         sec_last_angle = -10;
         main_iterations_not_moving = 0;
@@ -105,7 +105,7 @@
         main_is_calibrated = false;
         sec_is_calibrated = false;
     }
-    
+
     if (!main_is_calibrated) {
         led_green = 1;
         main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving);
@@ -124,12 +124,12 @@
             led_blue = 0;
         }
     }
-    
+
     screen.get_screen_handle()->setTextCursor(0, 8);
     screen.get_screen_handle()->printf("M: %i    \n", main_iterations_not_moving);
     screen.get_screen_handle()->printf("S: %i    \n", sec_iterations_not_moving);
     screen.display();
-    
+
     if (main_is_calibrated && sec_is_calibrated) {
         current_state = homing;
     }
@@ -139,27 +139,27 @@
 {
     const double home_x = 0.6524; // Meters.
     const double home_y = 0.3409;
-    
+
     double main_home;
     double sec_home;
-    
+
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
         screen.clear_display();
         screen.display_state_name("Homing");
-        
+
         inverse_kinematics(home_x, home_y, main_home, sec_home);
-        
+
         main_motor.set_target_angle(main_home);
         sec_motor.set_target_angle(sec_home);
-        
+
         screen.get_screen_handle()->setTextCursor(0, 8);
         screen.get_screen_handle()->printf("Ma: %.6f    \n", main_home);
         screen.get_screen_handle()->printf("Sa: %.6f    \n", sec_home);
         screen.display();
     }
-    
+
     if (ud_button.has_just_been_pressed()) {
         current_state = calib_bicep1;
     }
@@ -171,16 +171,16 @@
 void do_state_calib_bicep1()
 {
     static EMG_calibration calibration = EMG_calibration(&screen, &emg_1);
-        
+
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
         screen.clear_display();
         screen.display_state_name("EMG 1 calibration");
-        
+
         calibration.start();
     }
-    
+
     if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
         current_state = calib_bicep2;
     }
@@ -189,16 +189,16 @@
 void do_state_calib_bicep2()
 {
     static EMG_calibration calibration = EMG_calibration(&screen, &emg_2);
-    
+
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
         screen.clear_display();
         screen.display_state_name("EMG 2 calibration");
-        
+
         calibration.start();
     }
-    
+
     if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
         current_state = operation;
     }
@@ -207,50 +207,80 @@
 void do_state_operation()
 {
     static bool debug_forward_kinematics;
-        
+    
+    static const double max_speed = 0.01;
+    static double speed_x;
+    static double speed_y;
+
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
         screen.clear_display();
         screen.display_state_name("Normal operation");
-        
+
         control_goes_up = true;
         control_goes_right = true;
         
+        speed_x = 0;
+        speed_y = 0;
+
         screen.display_up_down_arrow(control_goes_up);
         screen.display_left_right_arrow(control_goes_right);
-        
+
         debug_forward_kinematics = true;
     }
     
+    if (emg_1.get_is_envelope_over_threshold()) {
+        led_green = 0;
+
+        if (control_goes_up) {
+            speed_y = max_speed;
+        } else {
+            speed_y = -max_speed;
+        }
+    } else {
+        led_green = 1;
+    }
+    
+    if (emg_2.get_is_envelope_over_threshold()) {
+        led_blue = 0;
+        if (control_goes_right) {
+            speed_x = max_speed;
+        } else {
+            speed_x = -max_speed;
+        }
+    } else {
+        led_blue = 1;
+    }
+
     /*
     if (debug_forward_kinematics) {
         // Using potmeters for debugging purposes;
         double main_angle = ((potmeter1.read() * 2) - 1) * PI;
         double sec_angle = ((potmeter2.read() * 2) - 1) * PI;
-        
+
         double e_x = 0.0;
         double e_y = 0.0;
-        
+
         forward_kinematics(main_angle, sec_angle, e_x, e_y);
-        
+
         screen.get_screen_handle()->setTextCursor(0, 0);
         screen.get_screen_handle()->printf("M_a: %.6f    \n", main_angle);
         screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_angle);
         screen.get_screen_handle()->printf("X:   %.6f    \n", e_x);
         screen.get_screen_handle()->printf("Y:   %.6f    ", e_y);
         screen.display();
-    
+
     } else {
         // Using potmeters for debugging purposes;
         double e_x = potmeter1.read();
         double e_y = potmeter2.read();
-        
+
         double main_angle = 0.0;
         double sec_angle = 0.0;
-        
+
         inverse_kinematics(e_x, e_y, main_angle, sec_angle);
-        
+
         screen.get_screen_handle()->setTextCursor(0, 0);
         screen.get_screen_handle()->printf("E_x: %.6f    \n", e_x);
         screen.get_screen_handle()->printf("E_y: %.6f    \n", e_y);
@@ -258,41 +288,59 @@
         screen.get_screen_handle()->printf("S_a: %.6f      ", sec_angle);
         screen.display();
     }
-    
+
     if (lr_button.has_just_been_pressed()) {
         debug_forward_kinematics = !debug_forward_kinematics;
     }*/
     
-    // Debug emg calibration.
-    led_blue = emg_1.get_is_envelope_over_threshold();
-    led_green = emg_2.get_is_envelope_over_threshold();
     
     /*
     double main_target = ((potmeter1.read() * 2) - 1) * PI;
     main_motor.set_target_angle(main_target);
     double sec_target = ((potmeter2.read() * 2) - 1) * PI;
     sec_motor.set_target_angle(sec_target);
-    
+
     if (lr_button.has_just_been_pressed()) {
     control_goes_right = !control_goes_right;
     screen.display_left_right_arrow(control_goes_right);
     }
     */
     
+    double main_cur_angle = main_motor.get_current_angle();
+    double sec_cur_angle = sec_motor.get_current_angle();
+
+    double main_target, sec_target;
+
+    end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
+
+    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();
+
     if (ud_button.has_just_been_pressed()) {
         control_goes_up = !control_goes_up;
+        screen.display_up_down_arrow(control_goes_up);
+    }
+
+    if (lr_button.has_just_been_pressed()) {
         control_goes_right = !control_goes_right;
-        screen.display_up_down_arrow(control_goes_up);
         screen.display_left_right_arrow(control_goes_right);
     }
 }
 
-void do_state_demo() {
+void do_state_demo()
+{
     static DebugControlDirection control_direction;
     static const double max_speed = 0.01;
     static double speed_x;
     static double speed_y;
-    
+
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed.
@@ -302,16 +350,16 @@
         led_blue = 1;
         screen.clear_display();
         screen.display_state_name("Demo mode!");
-        
+
         control_direction = debug_up;
-        
+
         speed_x = 0;
         speed_y = 0;
-        
+
         screen.display_up_down_arrow(control_goes_up);
         screen.display_left_right_arrow(control_goes_right);
     }
-    
+
     if (lr_button.has_just_been_pressed()) {
         switch (control_direction) {
             case debug_up:
@@ -336,22 +384,22 @@
                 break;
         }
     }
-    
+
     if (ud_button.is_pressed()) {
-        
+
         led_blue = 0;
-               
-        
+
+
         double main_cur_angle = main_motor.get_current_angle();
         double sec_cur_angle = sec_motor.get_current_angle();
-        
+
         double main_target, sec_target;
-        
+
         end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
-        
+
         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);
@@ -420,7 +468,8 @@
     }
 }
 
-void poll_buttons() {
+void poll_buttons()
+{
     // We need to poll the pins periodically.
     // Normally one would use rise and fall interrupts, so this wouldn't be
     // needed. But the buttons we use generate so much chatter that
@@ -436,22 +485,22 @@
     led_red = 1;
     led_green = 1;
     led_blue = 1;
-    
+
     screen.clear_display();
-    
+
     main_motor.set_pid_k_values(Kp, Ki, Kd);
     sec_motor.set_pid_k_values(Kp, Ki, Kd);
-    
+
     // One of the motors is reversed in the electronics.
     // This is fixed in the motor controll board, so we have to account
     // for it in software.
     main_motor.set_extra_reduction_ratio(-main_gear_ratio);
     sec_motor.set_extra_reduction_ratio(sec_gear_ratio);
-    
+
     // Set the maximum speed for both motors.
     main_motor.set_max_speed(0.5);
     sec_motor.set_max_speed(0.5);
-    
+
     // Start the motor controller at the desired frequency.
     main_motor.start(pid_period);
     sec_motor.start(pid_period);
@@ -461,16 +510,16 @@
     // Pretend we come from the operation state.
     // So that the waiting state knows it just got started.
     last_state = operation;
-    
+
     emg_1.start(emg_period);
     emg_2.start(emg_period);
-    
+
     // Start the button polling ticker.
     button_ticker.attach(&poll_buttons, button_poll_interval);
 
     while (true) {
         main_loop();
-        
+
         wait(main_loop_wait_time);
     }
 }
\ No newline at end of file