modified 0511
Dependencies: mbed-dev FastPWM3
Diff: FOC/foc.cpp
- Revision:
- 55:b97b90d06ffa
- Parent:
- 53:e85efce8c1eb
- Child:
- 60:f009e39e913e
diff -r 59575833d16f -r b97b90d06ffa FOC/foc.cpp --- 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