l yy
it works!
Dependencies: mbed-dev-f303 FastPWM3
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);
}
}