Diff: FOC/foc.cpp

Revision:

55:b97b90d06ffa

Parent:

53:e85efce8c1eb

Child:

60:f009e39e913e

--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Tue May 25 11:43:53 2021 +0000
@@ -220,9 +220,49 @@
     
     
 void torque_control(ControllerStruct *controller){
+    /*
+    // ----- origin -----
     float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
+    // ------------------
+    */
+    
+    /*----- convert theta_mech to 0~359.9999deg -----*/
+    static float pos, round;
+    pos = controller->theta_mech;
+    modf(pos/(2*PI),&round);
+    pos = pos - round*2*PI;
+    if(abs(pos) <= 0.001)
+        pos = abs(pos);
+    else if(pos < 0)
+        pos = pos + 2*PI;
+    /*-----------------------------------------------*/
+    
+    /*----- position PID control -----*/
+    static float in_err = 0, err = 0; //integral of position error
+    if(controller->p_des < pos)
+        if((controller->p_des + 2*PI - pos) < (pos - controller->p_des))
+            err = 2*PI - pos + controller->p_des;
+        else
+            err = controller->p_des - pos;
+    else
+        if((pos + 2*PI - controller->p_des) < (controller->p_des - pos))
+            err = controller->p_des - 2*PI - pos;
+        else
+            err = controller->p_des - pos;
+    
+    in_err = in_err + err;
+    /*
+    err = controller->p_des - pos;
+    in_err = in_err + err;
+    */
+    float torque_ref = controller->kp*(err) + controller->t_ff + controller->ki*(in_err) + controller->kd*(-controller->dtheta_mech);
+    /*--------------------------------*/        
+    
+    controller->i_q_ref = torque_ref/KT_OUT;    
+    controller->i_d_ref = 0.0f;
+    
     }
  
\ No newline at end of file