Diff: main.cpp

Revision:

30:7a66951a0122

Parent:

29:d1e8eb135e6c

Child:

31:393a7ec1d396

--- a/main.cpp	Mon Oct 29 15:20:50 2018 +0000
+++ b/main.cpp	Mon Oct 29 19:45:36 2018 +0000
@@ -38,12 +38,8 @@
 Timer state_timer;
 Timer emg_timer;
 
-<<<<<<< local
-enum States {failure, waiting, calib_emg, homing, calib_enc,  operational, demo}; //All possible robot states
-=======
-enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot 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
->>>>>>> other
 
 //Global variables/objects
 States current_state;
@@ -53,6 +49,12 @@
 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;
 
+// Variables defined for the homing state
+double q1_homing = 0, q2_homing = 0;
+double q1_dot_ref_homing, q2_dot_ref_homing;
+double beta = 5;
+double k = 2;
+
 // Meaning of process_emg_0 and such
 // - process_emg_0 is right biceps
 // - process_emg_1 is right triceps
@@ -64,6 +66,9 @@
 const double T = 0.001;
 
 
+// Resolution of the encoder at the output axle
+double resolution = (2.0f*3.1415926535f/double(counts_per_rotation))*(1.0/131.0); // In radians
+
 // Functions
 void measure_all() 
 {
@@ -76,9 +81,6 @@
     processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
 }
 
-void homing() {
-    PID_controller(0.5*3.1415926535f-q1,3.1415926535f-q2,u1,u2,T)
-    
 void output_all() {
     motor1_pwm = fabs(u1);
     motor1_dir = u1 > 0.0f;
@@ -152,7 +154,7 @@
                             right_bicep_max = process_emg_0;
                             }
                         }
-                    else{
+                    else if (process_emg_0 < 0.1*right_bicep_max){
                             current_emg_calibration_state = calib_right_tricep;
                             emg_timer.reset();
                             emg_timer.start();
@@ -164,23 +166,23 @@
                                 right_tricep_max = process_emg_1;
                                 }
                             }
-                        else{
-                                current_emg_calibration_state = calib_left_bicep;
-                                emg_timer.reset();
-                                emg_timer.start();
-                            }
+                    else if (process_emg_1 < 0.1*right_tricep_max){
+                            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;
-                                }
+                    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();
-                            }
+                        }
+                    else if (process_emg_2 < 0.1*left_bicep_max){
+                            current_emg_calibration_state = calib_left_tricep;
+                            emg_timer.reset();
+                            emg_timer.start();
+                        }
                     break;
                 case calib_left_tricep:
                         if(emg_timer < 5.0f){
@@ -188,20 +190,45 @@
                                 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();
-                            }
+                        else if (process_emg_3 < 0.1*left_tricep_max){
+                            current_emg_calibration_state = not_in_calib_emg;
+                            current_state = homing;
+                            state_changed = true;
+                            emg_timer.reset();
+                        }
                     break;
                 default:
                     current_state = failure;
                     state_changed = true;
                     
+        case homing: 
+            if (state_changed == true){
+                    state_timer.reset();
+                    state_timer.start();
+                    qref1 = q1; //NOT SURE IF WE NEED THIS. I do not think so, but just to be sure.
+                    qref2 = q2;
+                }
+                q1_dot_ref_homing = min(beta, k*(q1 - q1_homing));
+                q2_dot_ref_homing = min(beta, k*(q2 - q2_homing));
+                qref1 = q1_dot_ref_homing*T + q1;
+                qref2 = q2_dot_ref_homing*T + q2;
+                
+                // The value of 3.0 and 2*resolution can be changed
+                if (fabs(q1-q1_homing) <= 2*resolution && fabs(q2-q2_homing) <= 2 * resolution ){
+                        if (state_timer > 3.0f){
+                            current_state = operational;
+                            state_changed = true;
+                        }
+                    }
+                else{
+                    state_timer.reset();    
+                }
         
         case operational:       //interpreting emg-signals to move the end effector
+            if (state_changed == true){
+                    state_changed = false;
+                }
+            
                        
             // here we have to determine the desired velocity based on the processed emg signals and calibration
             if (process_emg_0 >= 0.16) { des_vx = vxmax; }
@@ -214,14 +241,23 @@
             else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
             else { des_vy = 0; }
             
-            if (button.read() == true) { current_state = demo; }
+            if (button.read() == true) {
+                current_state = demo; 
+                state_changed = true;
+                } // Isn't this going to cause problems as it will switch on and of very quickly?
             
             
             break;
             
         case demo: //moving according to a specified trajectory
-            
-            if (button.read() == true) { current_state = operational; }
+            if (state_changed == true){
+                    state_changed = false;
+                }
+                
+            if (button.read() == true) {
+                current_state = operational;
+                state_changed = true;
+            }
             
             break;