Diff: FOC/foc.cpp

Revision:

55:71a6e5fe5805

Parent:

53:e85efce8c1eb

diff -r 59575833d16f -r 71a6e5fe5805 FOC/foc.cpp
--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Tue Dec 15 07:28:27 2020 +0000
@@ -1,5 +1,14 @@
+/* -----
+移除速度控制，把位置控制改成PID
+
+log file:
+11/05: 把現在的控制架構改成PID(torque_control)
+----- */
+
 
 #include "foc.h"
+
+//#define POS_MAX 6.283185f //359.9999 deg
 using namespace FastMath;
 
 
@@ -220,7 +229,39 @@
     
     
 void torque_control(ControllerStruct *controller){
-    float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+    /*----- 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);
+    /*--------------------------------*/        
+    
+    //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;