Diff: main.cpp

Revision:

33:eb77ed8d167c

Parent:

32:2596cc9156ec

Child:

34:7d672cd04486

diff -r 2596cc9156ec -r eb77ed8d167c main.cpp
--- a/main.cpp	Mon Oct 29 20:11:31 2018 +0000
+++ b/main.cpp	Tue Oct 30 08:34:45 2018 +0000
@@ -56,6 +56,12 @@
 double raw_emg_3, process_emg_3;
 
 
+// Variables for calibration
+double calib_q1 = 0.5*3.1415926535f;
+double calib_q2 = 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;
@@ -84,9 +90,9 @@
 void measure_all() 
 {
 
-    q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
-    q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation;
-    forwardkinematics_function(q1,q2,x,y);  //motor_angle is global, this function ne
+    q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
+    q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q2;
+    forwardkinematics_function(q1,q2,x,y);  
     raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
     raw_emg_1 = emg1.read();
     processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
@@ -135,7 +141,8 @@
                 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;
             }    
             q1old = q1;
             q2old = q2;
@@ -221,16 +228,16 @@
                     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_hom*(q1 - q1_homing));
-            q2_dot_ref_homing = min(beta, k_hom*(q2 - q2_homing));
-            qref1 = q1_dot_ref_homing*T + q1;
-            qref2 = q2_dot_ref_homing*T + q2;
+            des_vx = min(beta, k_hom*(q1 - q1_homing)); // Little bit different then that Arvid told us, but now it works with the motor controller
+            des_vy = min(beta, k_hom*(q2 - q2_homing));
                 
             // 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;
+                        des_vx = 0; // Not sure if needed but added it anyways.
+                        des_vy = 0;
                     }
                 }
             else{
@@ -264,6 +271,7 @@
             break;
             
         case demo: //moving according to a specified trajectory
+            // We want to draw a square. Hence, first move to a certain point and then start moving a square.
             if (state_changed == true){
                     state_changed = false;
                 }