Diff: FOC/foc.cpp

Revision:

55:6ab83cbbbca7

Parent:

53:e85efce8c1eb

diff -r 59575833d16f -r 6ab83cbbbca7 FOC/foc.cpp
--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Fri Mar 26 05:28:00 2021 +0000
@@ -51,7 +51,7 @@
 
 void linearize_dtc(float *dtc){
     /// linearizes the output of the inverter, which is not linear for small duty cycles ///
-    float sgn = 1.0f-(2.0f*(dtc<0));
+    float sgn = 1.0f-(2.0f*(*dtc<0));
     if(abs(*dtc) >= .01f){
         *dtc = *dtc*.986f+.014f*sgn;
         }
@@ -218,11 +218,32 @@
        
     }
     
+void torque_control(ControllerStruct *controller){
+        float error = controller->p_des - controller->theta_mech;
+        float kp;
     
+        if( error <= controller->range_L )  
+            kp = controller->kp_H;
+        else if ( error > controller->range_L && error <= controller->range_M )  
+            kp = controller->kp_M2;
+        else if ( error > controller->range_M && error <= controller->range_H )  
+            kp = controller->kp_M1; 
+        else
+            kp = controller->kp_L;
+        
+    float torque_ref = kp*error + 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;
+    }
+        
+        
+/*    
 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 torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
     }
+*/
  
\ No newline at end of file