Diff: main.cpp

Revision:

51:29e1686e8b3e

Parent:

50:1fe5c2c53af1

Child:

52:d4d5e3414865

--- a/main.cpp	Tue May 26 09:24:13 2020 +0000
+++ b/main.cpp	Thu Aug 06 07:25:51 2020 +0000
@@ -158,7 +158,7 @@
 void calibrate(void){
     gpio.enable->write(1);                                                      // Enable gate drive
     gpio.led->write(1);                                                    // Turn on status LED
-    order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
+    //order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
     //j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
     //j_calibrate(&ma700,&gpio, &controller, &prefs);     
@@ -290,8 +290,8 @@
         
         ///hjb add joint angle
         controller.theta_joint_raw= -(ma700.GetMechPosition()+spi.GetMechPosition());
-        if(controller.dtheta_mech>0) {controller.theta_joint_raw += PI/180.0f;}  //compensate
-        else if(controller.dtheta_mech<0) {controller.theta_joint_raw -= PI/180.0f;}
+        //if(controller.dtheta_mech>0) {controller.theta_joint_raw += PI/180.0f;}  //compensate
+       // else if(controller.dtheta_mech<0) {controller.theta_joint_raw -= PI/180.0f;}
         if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;}
         if(controller.theta_joint_raw>=360){controller.theta_joint_raw -= 2.0f*PI;}
 
@@ -305,6 +305,7 @@
         controller.Ncycle = controller.theta_joint_raw/(2.0f*PI/GR);
         controller.Ncycle_mod = fmod(controller.theta_joint_raw,2.0f*PI/GR);
         controller.Mech_mod = fmod(spi.GetMechPosition(),2.0f*PI);
+      /*
         if(abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)>4)  //not in the error range
         {
             if (controller.Mech_mod>PI)   //
@@ -317,15 +318,19 @@
            } 
             
         }
+        */
         controller.theta_joint= controller.Ncycle*(2.0f*PI/GR)+controller.Mech_mod/GR;  //In the real use, should turn to the theta_mech
         if(J_M_flag == 0)
           {
-           if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<3)
-            {
-            Joint_init =  controller.theta_joint- (1.0f/GR)*spi.GetMechPosition();
-            controller.theta_mech = controller.theta_joint;////controller.theta_joint;                                 // easy way to use, whether available in the shock?
-            J_M_flag = 1;
-            }
+            if(0<abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)<2)
+           {
+               if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<2)
+                {
+                Joint_init =  controller.theta_joint- (1.0f/GR)*spi.GetMechPosition();
+                controller.theta_mech = controller.theta_joint;////controller.theta_joint;                                 // easy way to use, whether available in the shock?
+                J_M_flag = 1;
+                }
+              }
             controller.theta_mech = controller.theta_joint_raw;
            }
         else if(J_M_flag == 1)