Diff: main.cpp

Revision:

27:eee900e0a47d

Parent:

23:7d83af123c43

Child:

28:d952367fc0fc

--- a/main.cpp	Mon Oct 29 12:38:04 2018 +0000
+++ b/main.cpp	Mon Oct 29 15:03:14 2018 +0000
@@ -33,14 +33,24 @@
 // tickers and timers
 Ticker loop_ticker;
 Timer state_timer;
+Timer emg_timer;
 
 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
+enum Emg_measures_states {not_in_calib_emg, calib_right_bicep, calib_right_tricep, calib_left_bicep, calib_left_tricep}; // States inside
 
 //Global variables/objects
 States current_state;
+Emg_measures_states curent_emg_calibration_state = not_in_calib_enc;
 
 double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2, raw_emg_0, process_emg_0, raw_emg_1, process_emg_1; //will be set by the motor_controller function
 double vxmax = 1.0, vymax = 1.0;
+double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
+
+// Meaning of process_emg_0 and such
+// - process_emg_0 is right biceps
+// - process_emg_1 is right triceps
+// - process_emg_2 is left biceps
+// - process_emg_3 is left triceps
 
 int counts_per_rotation = 32;
 bool state_changed = false;
@@ -90,13 +100,76 @@
             // fabs(motor1.velocity()) < 0.1f && 
             if (state_timer.read() > 5.0f) {
                 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
+                curent_emg_calibration_state = calib_right_bicep;
                 state_changed = true;
             }
             break;
             
         case calib_emg:     //calibrate emg-signals
-            current_state = operational;
-            break;
+            if (state_changed == true){
+                state_timer.reset();
+                state_timer.start();
+                emg_timer.reset();
+                emg_timer.start();
+                state_changed = false;
+                }
+            
+            // calibrating right bicep
+            switch(curent_emg_calibration_state){
+                case calib_right_bicep:
+                    if(emg_timer < 5.0f){
+                        if (process_emg_0 > right_bicep_max){
+                            right_bicep_max = process_emg_0;
+                            }
+                        }
+                    else{
+                            current_emg_calibration_state = calib_right_tricep;
+                            emg_timer.reset();
+                            emg_timer.start();
+                        }
+                    break;
+                case calib_right_tricep:
+                    if(emg_timer < 5.0f){
+                            if (process_emg_1 > right_tricep_max){
+                                right_tricep_max = process_emg_1;
+                                }
+                            }
+                        else{
+                                current_emg_calibration_state = calib_left_bicep;
+                                emg_timer.reset();
+                                emg_timer.start();
+                            }
+                    break;
+                case calib_left_bicep:
+                        if(emg_timer < 5.0f){
+                            if (process_emg_2 > left_bicep_max){
+                                left_bicep_max = process_emg_2;
+                                }
+                            }
+                        else{
+                                current_emg_calibration_state = calib_left_tricep;
+                                emg_timer.reset();
+                                emg_timer.start();
+                            }
+                    break;
+                case calib_left_tricep:
+                        if(emg_timer < 5.0f){
+                            if (process_emg_3 > left_tricep_max){
+                                left_tricep_max = process_emg_3;
+                                }
+                            }
+                        else{
+                                current_emg_calibration_state = not_in_calib_emg;
+                                current_state = operational;
+                                state_changed = true;
+                                emg_timer.reset();
+                                emg_timer.start();
+                            }
+                    break;
+                default:
+                    current_state = failure;
+                    state_changed = true;
+                    
         
         case operational:       //interpreting emg-signals to move the end effector