Diff: main.cpp

Revision:

39:f119ca6fc821

Parent:

34:ae62ebf4d494

Child:

40:7e8d0632757c

--- a/main.cpp	Thu Nov 01 15:09:10 2018 +0000
+++ b/main.cpp	Thu Nov 01 15:41:34 2018 +0000
@@ -5,10 +5,12 @@
 #include "Button.h"
 #include "Screen.h"
 #include "motor.h"
+#include "EMGFilter.h"
 #include "motor_calibration.h"
 #include "forward_kinematics.h"
 #include "inverse_kinematics.h"
 #include "end_effector_control.h"
+#include "EMG_calibration.h"
 
 
 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine
@@ -26,6 +28,9 @@
 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);
+
 
 States current_state;   // Defining the state we are currently in
 States last_state;      // To detect state changes.
@@ -165,29 +170,37 @@
 
 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()) {
+    if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
         current_state = calib_bicep2;
     }
 }
 
 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()) {
-        current_state = homing;
+    if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
+        current_state = operation;
     }
 }
 
@@ -444,6 +457,9 @@
     // 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);