Diff: main.cpp

Revision:

12:0c10396d0615

Parent:

11:d980e0e581db

Child:

13:88967c004446

--- a/main.cpp	Wed Oct 31 08:23:02 2018 +0000
+++ b/main.cpp	Wed Oct 31 09:36:52 2018 +0000
@@ -10,20 +10,25 @@
 // Global variables
 const double PI = 3.14159265359;
 // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
-const double main_loop_wait_time = 0.01;
+const float main_loop_wait_time = 0.01;
 
 // Time between two button polls. Used to debounce the button presses.
-const double button_poll_interval = 0.05;
+const float button_poll_interval = 0.05;
 
 const float pid_period = 0.001; // PID sample period in seconds.
 
-const double Kp = 10.0;
-const double Ki = 0.1;
-const double Kd = 0.5;
+const float Kp = 10.0;
+const float Ki = 0.1;
+const float Kd = 0.5;
 
 
-Motor motor1(D6, D7, D13, D12);
-Motor motor2(D5, D4, D10, D11);
+Motor main_motor(D6, D7, D13, D12);
+Motor sec_motor(D5, D4, D10, D11);
+
+// The motor -> main gear ratio is 25 / 60.
+// The motor -> secondary gear ratio is 25/50
+const float main_gear_ratio = 25.0/60.0;
+const float sec_gear_ratio = 25.0/50.0;
 
 
 States current_state;   // Defining the state we are currently in
@@ -47,6 +52,12 @@
 // So pick a pin we don't use.
 Screen screen(D14, D15, D9);
 
+// Which direction the emg will control the arm.
+// Up or down.
+// Left or right.
+bool control_goes_up = false;
+bool control_goes_right = false;
+
 
 void do_state_waiting()
 {
@@ -136,7 +147,10 @@
     }
     
     if (ud_button.has_just_been_pressed()) {
-        current_state = homing;
+        control_goes_up = !control_goes_up;
+    }
+    if (lr_button.has_just_been_pressed()) {
+        control_goes_right = !control_goes_right;
     }
 }
 
@@ -153,8 +167,8 @@
     }
 
     // Stop the motors!
-    motor1.stop();
-    motor2.stop();
+    main_motor.stop();
+    sec_motor.stop();
 }
 
 
@@ -214,11 +228,18 @@
     
     screen.clear_display();
     
-    motor1.set_pid_k_values(Kp, Ki, Kd);
-    motor2.set_pid_k_values(Kp, Ki, Kd);
+    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);
+    
     // Start the motor controller at the desired frequency.
-    motor1.start(pid_period);
-    motor2.start(pid_period);
+    main_motor.start(pid_period);
+    sec_motor.start(pid_period);
 
     // Start in the waiting state.
     current_state = waiting;