Diff: FOC/foc.cpp

Revision:

48:a74e401a6d84

Parent:

47:f4ecf3e0576a

diff -r f4ecf3e0576a -r a74e401a6d84 FOC/foc.cpp
--- a/FOC/foc.cpp	Wed May 13 09:53:27 2020 +0000
+++ b/FOC/foc.cpp	Wed Apr 07 10:12:43 2021 +0000
@@ -1,4 +1,5 @@
 
+// He version
 #include "foc.h"
 using namespace FastMath;
 
@@ -81,12 +82,16 @@
        /// Commutation Loop ///
        controller->loop_count ++;   
        if(PHASE_ORDER){                                                                          // Check current sensor ordering
-           controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
-           controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+           //controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
+           //controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+           controller->i_b = alpha * controller->i_b + (1.0f - alpha) * I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);  // low pass filter, freq=1-alpha / dt
+           controller->i_c = alpha * controller->i_c + (1.0f - alpha) * I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
            }
-        else{
-            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
-           controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+       else{
+           // controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
+           // controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+           controller->i_b = alpha * controller->i_b + (1.0f - alpha) * I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+           controller->i_c = alpha * controller->i_c + (1.0f - alpha) * I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
            }
        controller->i_a = -controller->i_b - controller->i_c;       
        
@@ -114,13 +119,14 @@
        
        /// PI Controller ///
 //       float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_d_error = controller->i_d_ref - observer->i_d_est;
-//       float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current*0.6;
-       float i_q_error = controller->i_q_ref - observer->i_q_est;// + cogging_current;
-       //float i_q_error = controller->i_q_ref - observer->i_q_est;
+      float i_d_error = controller->i_d_ref - observer->i_d_est;
+//       float i_q_error = controller->i_q_ref - controller->i_q + cogging_current*0.6;
+//       float i_q_error = controller->i_q_ref - controller->i_q;
+      float i_q_error = controller->i_q_ref - observer->i_q_est;// + cogging_current;
+//       float i_q_error = controller->i_q_ref - observer->i_q_est;
        
-       float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
-       float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
+       float v_d_ff = (controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
+       float v_q_ff =  (controller->i_q_ref*R_PHASE  + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
        
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
@@ -128,9 +134,12 @@
        //v_d_ff = 0;
        //v_q_ff = 0;
        
-       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));        // Limit integrators to prevent windup
-       controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int+ v_d_ff;  
-       controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int+ v_q_ff; 
+       // limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));        // Limit integrators to prevent windup
+       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q));
+       controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int + v_d_ff;  
+       controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int + v_q_ff; 
+         //controller->v_d = kp_design * i_d_error + ki_design * DT * controller->d_int;
+         //controller->v_q = kp_design * i_q_error + ki_design * DT * controller->q_int;
        
 //       controller->v_q = 0.0f;  // always zero
 //       controller->v_d = 0.0f;
@@ -146,8 +155,10 @@
         //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
 
-       observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
-       observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
+       // observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
+       // observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
+       observer->i_d_dot = (controller->v_d - (observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
+       observer->i_q_dot = (controller->v_q - (observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
        
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
@@ -177,9 +188,20 @@
     
     
 void torque_control(ControllerStruct *controller){
+    // controller->t_ff= 0.5f;
+    // controller->kd = 0.1f;
     float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+    if(torque_ref>18.0f){
+        torque_ref = 18.0f;
+    }
+    else{
+        if(torque_ref <-18.0f){
+            torque_ref = -18.0f;    
+        }
+    }    
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
-    controller->i_q_ref = torque_ref/KT_OUT;    
+    controller->i_q_ref = torque_ref/KT_OUT;  
+    // controller->i_q_ref = 1.0f;
     controller->i_d_ref = 0.0f;
     }
 
@@ -188,4 +210,4 @@
 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
     
     }
-*/    
\ No newline at end of file
+*/