Diff: main.cpp

Revision:

8:9090ab7c19a8

Parent:

7:e7f808875bc4

Child:

9:27d00b64076e

--- a/main.cpp	Wed Oct 31 05:58:51 2018 +0000
+++ b/main.cpp	Wed Oct 31 06:13:38 2018 +0000
@@ -2,6 +2,7 @@
 
 #include "Button.h"
 #include "Screen.h"
+#include "motor.h"
 
 
 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
@@ -14,6 +15,16 @@
 // Time between two button polls. Used to debounce the button presses.
 const double button_poll_interval = 0.1;
 
+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;
+
+
+Motor motor1(D6, D7, D13, D12);
+Motor motor2(D5, D4, D10, D11);
+
 
 States current_state;   // Defining the state we are currently in
 States last_state;      // To detect state changes.
@@ -121,6 +132,8 @@
     }
 
     // Stop the motors!
+    motor1.stop();
+    motor2.stop();
 }
 
 
@@ -175,6 +188,12 @@
 int main()
 {
     led_red = 1;
+    
+    motor1.set_pid_k_values(Kp, Ki, Kd);
+    motor2.set_pid_k_values(Kp, Ki, Kd);
+    // Start the motor controller at the desired frequency.
+    motor1.start(pid_period);
+    motor2.start(pid_period);
 
     // Start in the waiting state.
     current_state = waiting;