Diff: FOC/foc.cpp

Revision:

69:ba1c89835a54

Parent:

58:32e8927fe39f

Child:

70:2ea6c555a2cb

--- a/FOC/foc.cpp	Tue Feb 09 16:33:00 2021 +0000
+++ b/FOC/foc.cpp	Wed Feb 10 21:29:13 2021 +0000
@@ -98,6 +98,7 @@
     controller->v_q = 0;
     controller->v_d = 0;
     controller->otw_flag = 0;
+    controller->velocity_sum = 0;
 
     }
     
@@ -262,7 +263,11 @@
     
     
 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);
+    float windup = 34.0;
+    controller->velocity_sum += controller->kd/100*(controller->v_des - controller->dtheta_mech);
+    controller->velocity_sum = fmaxf(fminf(controller->velocity_sum, windup), -windup);
+    //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 = controller->kp*(controller->v_des - controller->dtheta_mech) + controller->velocity_sum + controller->t_ff;
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;