Diff: FOC/foc.cpp

Revision:

45:26801179208e

Parent:

44:8040fa2fcb0d

Child:

46:2d4b1dafcfe3

--- a/FOC/foc.cpp	Mon Jun 11 00:04:06 2018 +0000
+++ b/FOC/foc.cpp	Wed Jun 27 03:44:44 2018 +0000
@@ -56,6 +56,15 @@
     *offset_1 = adc1_offset/n;
     *offset_2 = adc2_offset/n;
     }
+    
+void init_controller_params(ControllerStruct *controller){
+    controller->ki_d = KI_D;
+    controller->ki_q = KI_Q;
+    controller->k_d = K_SCALE*I_BW;
+    controller->k_q = K_SCALE*I_BW;
+    controller->alpha = 1.0f - 1.0f/(1.0f - DT*I_BW*2.0f*PI);
+    
+    }
 
 void reset_foc(ControllerStruct *controller){
     TIM1->CCR3 = (PWM_ARR>>1)*(0.5f);
@@ -71,6 +80,12 @@
     controller->v_q = 0;
     controller->v_d = 0;
     }
+    
+void limit_current_ref (ControllerStruct *controller){
+    float i_q_max_limit = (0.5774f*controller->v_bus - controller->dtheta_elec*WB)/R_PHASE;
+    float i_q_min_limit = (-0.5774f*controller->v_bus - controller->dtheta_elec*WB)/R_PHASE;
+    controller->i_q_ref = fmaxf(fminf(i_q_max_limit, controller->i_q_ref), i_q_min_limit);
+    }
 
 
 void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
@@ -109,44 +124,45 @@
         observer->i_q_est += K_O*observer->e_q + .001f*observer->e_q_int;
         
         
-        float scog12 = FastSin(12.0f*theta);
-        float scog1 = s;
-        float cogging_current = 0.25f*scog1 - 0.3f*scog12;
+        //float scog12 = FastSin(12.0f*theta);
+        //float scog1 = s;
+        //float cogging_current = 0.25f*scog1 - 0.3f*scog12;
+        
+        // Filter the current references to the desired closed-loopbandwidth
+        // Allows calculation of desired di/dt for inductance, etc
+        controller->did_dt = controller->i_d_ref_filt;
+        controller->diq_dt = controller->i_q_ref_filt;
+        controller->i_d_ref_filt = (1.0f-controller->alpha)*controller->i_d_ref_filt + controller->alpha*controller->i_d_ref;
+        controller->i_q_ref_filt = (1.0f-controller->alpha)*controller->i_q_ref_filt + controller->alpha*controller->i_q_ref;
+        controller->did_dt = (controller->i_d_ref_filt - controller->did_dt)/DT;
+        controller->diq_dt = (controller->i_q_ref_filt - controller->diq_dt)/DT;
        
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
-       float i_q_error = controller->i_q_ref - controller->i_q  + cogging_current;
+       float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current;
        
-       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));
+       // Calculate feed-forward voltages //
+       float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE + L_D*controller->did_dt - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
+       float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE + L_Q*controller->diq_dt + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
        
+       // Integrate Error //
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
-       
-       //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; 
-       
-       //controller->v_q = 4.0f;
-       //controller->v_d = 0.0f;
-       
-       //controller->v_d = v_d_ff;
-       //controller->v_q = v_q_ff; 
+
+       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(controller->k_q*controller->ki_q));        
+       controller->v_d = controller->k_d*(i_d_error + controller->ki_d*controller->d_int);// + v_d_ff;  
+       controller->v_q = controller->k_q*(i_q_error + controller->ki_q*controller->q_int);// + v_q_ff; 
        
        limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
-       abc(controller->theta_elec+0.5f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
-    
-        //controller->v_u = c*controller->v_d - s*controller->v_q;                // Faster Inverse DQ0 transform
-        //controller->v_v = (0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
-        //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
+       abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
        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;
        
+       //controller->dtc_u = 0.5f;
+       //controller->dtc_v = 0.6f;
+       //controller->dtc_w = 0.5f;
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles
             TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
@@ -168,7 +184,7 @@
            //printf("%.2f  %.2f  %.2f\n\r", controller->i_a, controller->i_b, controller->i_c);
            //printf("%f\n\r", controller->dtheta_mech*GR);
            //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
-           //pc.printf("%f    %f\n\r", controller->i_d, controller->i_q);
+           //printf("%f    %f\n\r", controller->k_d, controller->k_q);
            //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
             }
     }