Diff: main.cpp

Revision:

37:5ddfd9e6cdb2

Parent:

36:dc0571d14e30

Child:

38:0f824c45f717

--- a/main.cpp	Wed Oct 31 11:50:28 2018 +0000
+++ b/main.cpp	Wed Oct 31 13:00:49 2018 +0000
@@ -41,10 +41,12 @@
 // States
 enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //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
+enum calib_enc_states {not_in_calib_enc, calib_enc_q1, calib_enc_q2};
 
 //Global variables/objects
 States current_state;
 Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
+calib_enc_states calib_enc_state = not_in_calib_enc;
 
 double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2; //will be set by the motor_controller function
 double u1 = 0, u2= 0;
@@ -60,7 +62,7 @@
 
 // Variables for calibration
 double calib_q1 = 3.1415926535f;
-double calib_q2 = 1.5f*3.1415926535f;
+double calib_q2 = double(7)/double(6) * 3.1415926535f;
 double off_set_q1 = 0; // This variable is used to determine the off set from our definition from q1 and q2.
 double off_set_q2 = 0;
 
@@ -114,9 +116,10 @@
 void state_machine() {
     switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
         case waiting:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
-            if (button.read()==true) 
+            if (button.read()==false) 
             {
                 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
+                calib_enc_state = calib_enc_q2;
                 state_changed = true;
             }
             
@@ -129,35 +132,45 @@
                 state_timer.reset();
                 state_timer.start();
                 state_changed = false;
-                n = 0;
                 led_G = 0;
                 led_B = 1;
                 led_R = 1;
-                u1 = 0.55f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
-                u2 = 0.55f;
+                u1 = 0;
+                u2 = -0.55f;
                 q1old = 0; 
                 q2old = 0;
             }
             
-            if (q1-q1old == 0.0 && q2 - q2old < 0.0 && state_timer.read() > 5.0f)
-            {
-                current_state = calib_emg; //the NEXT loop we will be in calib_emg state
-                current_emg_calibration_state = calib_right_bicep;
-                state_changed = true;
-                off_set_q1 = calib_q1 - q1;
-                off_set_q2 = calib_q2 - q2;
-                u1 = 0;
-                u2 = 0;
-            }    
-            q1old = q1;
-            q2old = q2;
-            
-            n++;
-            if (n%1000 == 0)
-            {
-                led_G = !led_G;
+            switch(calib_enc_state){
+                case calib_enc_q2:
+                    if (q2 - q2old == 0.0 && state_timer.read() > 5.0f)
+                    {
+                        calib_enc_state = calib_enc_q1;
+                        off_set_q2 = calib_q2 - q2;
+                        u2 = 0;
+                        u1 = -0.55f;
+                        state_timer.reset();
+                        state_timer.start();
+                    }    
+                    q2old = q2;
+                    break;
+                case calib_enc_q1:
+                    if (q1 - q1old == 0.0 && state_timer.read() > 5.0f)
+                    {
+                        calib_enc_state = not_in_calib_enc;
+                        current_emg_calibration_state = calib_right_bicep;
+                        current_state = failure;
+                        state_changed = true;
+                        off_set_q1 = calib_q1 - q1;
+                        u1 = 0;
+                        state_timer.reset();
+                        state_timer.start();
+                    }    
+                    q2old = q2;
+                    break;
+                default:
+                    current_state = failure;
             }
-            
             break;
             
         case calib_emg:     //calibrate emg-signals