hobbyking_cheetah source code modified 2020/12/15
Dependencies: mbed-dev-f303 FastPWM3
Diff: FOC/foc.cpp
- Revision:
- 55:71a6e5fe5805
- Parent:
- 53:e85efce8c1eb
--- 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;