Diff: main.cpp

Revision:

50:5d2176b93a65

Parent:

49:b4fd52d244f9

--- a/main.cpp	Mon Nov 05 05:59:14 2018 +0000
+++ b/main.cpp	Mon Nov 05 10:14:31 2018 +0000
@@ -24,36 +24,36 @@
 Motor main_motor(D6, D7, D13, D12);
 Motor sec_motor(D5, D4, D10, D11);
 
-
+// For debugging purposes (with potmeter)
 AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2
 
-EMGFilter emg_1(A0);
-EMGFilter emg_2(A1);
-
+EMGFilter emg_1(A0);    // Input for EMG signals
+EMGFilter emg_2(A1);    // Input for EMG signals
 
 States current_state;   // Defining the state we are currently in
 States last_state;      // To detect state changes.
-Ticker loop_ticker;     // Ticker for the loop function
 
 // Order of buttons: up_down, left_right, panic
 // D2, D3, D8
-Button ud_button(D2);
-Button lr_button(D3);
-Button p_button(D8);
+Button ud_button(D2);       // Up-down
+Button lr_button(D3);       // Left-right
+Button p_button(D8);        // Panic (stop)
 
-Ticker button_ticker;
+Ticker button_ticker;       // Ticker for poll button to check whether the button is still pressed
 
+// LEDs for debugging purposes
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
 DigitalOut led_blue(LED_BLUE);
 
-// The last arguent is the reset pin.
+// The last arguent (D6) is the reset pin.
 // The screen doesn't use it, but the library requires it
 // So pick a pin we don't use.
-Screen screen(D14, D15, D9);
+// D14 and D15 are from and to the screen
+Screen screen(D14, D15, D9);        // Connects OLED screen (Adafruit)
 
-// Which direction the emg will control the arm.
+// Which direction the EMG (or for demo mode) will control the arm.
 // Up or down.
 // Left or right.
 bool control_goes_up = false;
@@ -137,8 +137,9 @@
 
 void do_state_homing()
 {
+    // Position end-effector
     const double home_x = 0.6524; // Meters.
-    const double home_y = 0.3409;
+    const double home_y = 0.3409; // Meters
 
     double main_home;
     double sec_home;
@@ -164,7 +165,7 @@
         current_state = calib_bicep1;
     }
     if (lr_button.has_just_been_pressed()) {
-        current_state = demo;
+        current_state = demo;                   // Back-up for if EMG does not work
     }
 }
 
@@ -178,7 +179,7 @@
         screen.clear_display();
         screen.display_state_name("EMG 1 calibration");
 
-        calibration.start();
+        calibration.start();        // Starts calibration of bicep 1
     }
 
     if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
@@ -206,9 +207,7 @@
 
 void do_state_operation()
 {
-    static bool debug_forward_kinematics;
-    
-    static const double max_speed = 0.01;
+    static const double max_speed = 0.01;       // 1 cm per iteration (as long as EMG signals are received, setpoint is set 1 cm away)
     static double speed_x;
     static double speed_y;
     
@@ -229,8 +228,6 @@
 
         screen.display_up_down_arrow(control_goes_up);
         screen.display_left_right_arrow(control_goes_right);
-
- //       debug_forward_kinematics = true;
         
         last_state_1 = false;
         last_state_2 = false;
@@ -266,61 +263,8 @@
     
     last_state_1 = emg_1_state;
     last_state_2 = emg_2_state;
-
-    /*
-    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);
-        screen.get_screen_handle()->printf("M_a: %.6f      \n", main_angle);
-        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;
-    }*/
-    
-    
-    /*
-    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);
-    }
-    */
-    
-    if (speed_x || speed_y) {
+  
+    if (speed_x || speed_y) {               // Ensures that if the end-effector is moved externally, the motors will resist (setpoint changes)
         
         double main_cur_angle = main_motor.get_current_angle();
         double sec_cur_angle = sec_motor.get_current_angle();
@@ -333,17 +277,6 @@
         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;
@@ -452,6 +385,7 @@
 
 void main_loop()
 {
+    // Update buttons
     ud_button.update();
     lr_button.update();
     p_button.update();
@@ -516,6 +450,7 @@
     // 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.
+    // Extra reduction ratio is needed to give the "real" angles that the gears need to make (ratio of both gears is different)
     main_motor.set_extra_reduction_ratio(-main_gear_ratio);
     sec_motor.set_extra_reduction_ratio(sec_gear_ratio);
 
@@ -540,7 +475,7 @@
     button_ticker.attach(&poll_buttons, button_poll_interval);
 
     while (true) {
-        main_loop();
+        main_loop();        // In while(true) to keep from stalling the interrupts (around 0.01 times per seconds)
 
         wait(main_loop_wait_time);
     }