DRV8323RS Version
Dependencies: mbed-dev-f303 FastPWM3
Diff: FOC/foc.cpp
- Revision:
- 48:74a40481740c
- Parent:
- 47:e1196a851f76
- Child:
- 49:83d83040ea51
diff -r e1196a851f76 -r 74a40481740c FOC/foc.cpp --- a/FOC/foc.cpp Wed Dec 05 04:07:46 2018 +0000 +++ b/FOC/foc.cpp Sun Mar 03 02:51:51 2019 +0000 @@ -33,7 +33,7 @@ /// Space Vector Modulation /// /// u,v,w amplitude = v_bus for full modulation depth /// - float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f; + float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))*0.5f; *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); @@ -49,7 +49,19 @@ } - +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)); + if(abs(*dtc) >= .01f){ + *dtc = *dtc*.986f+.014f*sgn; + } + else{ + *dtc = 2.5f*(*dtc); + } + + } + + void zero_current(int *offset_1, int *offset_2){ // Measure zero-offset of the current sensors int adc1_offset = 0; int adc2_offset = 0; @@ -92,6 +104,11 @@ } +void reset_observer(ObserverStruct *observer){ + observer->temperature = 25.0f; + observer->resistance = .1f; + } + 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; @@ -99,7 +116,24 @@ } -void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){ +void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){ + + /// Update observer estimates /// + // Resistance observer // + // Temperature Observer // + float t_rise = (float)observer->temperature - 25.0f; + float q_th_in = (1.0f + .00393f*t_rise)*(controller->i_d*controller->i_d*R_PHASE*1.5f + controller->i_q*controller->i_q*R_PHASE*1.5f); + float q_th_out = t_rise*R_TH; + observer->temperature += INV_M_TH*DT*(q_th_in-q_th_out); + + //observer->resistance = (controller->v_q - 2.0f*controller->dtheta_elec*(WB + L_D*controller->i_d))/controller->i_q; + observer->resistance = controller->v_q/controller->i_q; + if(isnan(observer->resistance)){observer->resistance = R_PHASE;} + observer->temperature2 = (double)(25.0f + ((observer->resistance*6.0606f)-1.0f)*275.5f); + double e = observer->temperature - observer->temperature2; + observer->temperature -= .001*e; + //printf("%.3f\n\r", e); + /// Commutation Loop /// controller->loop_count ++; @@ -124,32 +158,27 @@ // 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; + + + /// Field Weakening /// + + controller->fw_int += .001f*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref); + controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_MAX_FW); + controller->i_d_ref = controller->fw_int; + //float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref; + limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX); - /// Field Weakening /// - /* - controller->fw_int += .001*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref); - controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_MAX_FW); - controller->i_d_ref = controller->fw_int; - float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref; - limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX); - - */ /// 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; // Calculate feed-forward voltages // - float v_d_ff = 2.0f*(1.0f*controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q); //feed-forward voltages - float v_q_ff = 2.0f*(1.0f*controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB)); + float v_d_ff = SQRT3*(1.0f*controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q); //feed-forward voltages + float v_q_ff = SQRT3*(1.0f*controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB)); // Integrate Error // controller->d_int += controller->k_d*controller->ki_d*i_d_error; @@ -165,6 +194,12 @@ controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q); limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus); // Normalize voltage vector to lie within curcle of radius v_bus + float dtc_d = controller->v_d/controller->v_bus; + float dtc_q = controller->v_q/controller->v_bus; + linearize_dtc(&dtc_d); + linearize_dtc(&dtc_q); + controller->v_d = dtc_d*controller->v_bus; + controller->v_q = dtc_q*controller->v_bus; abc(controller->theta_elec + 0.0f*DT*controller->dtheta_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 @@ -179,7 +214,7 @@ TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_w); } - controller->theta_elec = theta; //For some reason putting this at the front breaks thins + controller->theta_elec = theta; }