State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: main.cpp
- Revision:
- 8:9090ab7c19a8
- Parent:
- 7:e7f808875bc4
- Child:
- 9:27d00b64076e
diff -r e7f808875bc4 -r 9090ab7c19a8 main.cpp --- 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;