Diff: main.cpp

Revision:

35:6110d0b5513b

Parent:

34:7d672cd04486

Child:

36:dc0571d14e30

Child:

39:5db36ce5e620

--- a/main.cpp	Tue Oct 30 08:54:33 2018 +0000
+++ b/main.cpp	Tue Oct 30 09:11:19 2018 +0000
@@ -50,7 +50,8 @@
 States current_state;
 Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
 
-double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2; //will be set by the motor_controller function
+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;
 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;
 
@@ -62,14 +63,13 @@
 
 
 // Variables for calibration
-double calib_q1 = 0.5f*3.1415926535f;
-double calib_q2 = 3.1415926535f;
+double calib_q1 = 3.1415926535f;
+double calib_q2 = 1.5f*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;
 
 // Variables defined for the homing state
-double q1_homing = 0, q2_homing = 0;
-double q1_dot_ref_homing, q2_dot_ref_homing;
+double q1_homing = 0.5f*3.1415926535f, q2_homing = 3.1415926535f;
 double beta = 5;
 double k_hom = 2;
 
@@ -151,6 +151,8 @@
                 state_changed = true;
                 off_set_q1 = calib_q1 - q1;
                 off_set_q2 = calib_q2 - q2;
+                u1 = 0;
+                u2 = 0;
             }    
             q1old = q1;
             q2old = q2;