yezhong yezhong
/
Motor_DRV8323RH_for_2019-
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed-dev-f303 FastPWM3
Embed:
(wiki syntax)
Show/hide line numbers
foc.cpp
00001 00002 #include "foc.h" 00003 using namespace FastMath; 00004 00005 00006 void abc( float theta, float d, float q, float *a, float *b, float *c){ 00007 /// Inverse DQ0 Transform /// 00008 ///Phase current amplitude = lengh of dq vector/// 00009 ///i.e. iq = 1, id = 0, peak phase current of 1/// 00010 float cf = FastCos(theta); 00011 float sf = FastSin(theta); 00012 00013 *a = cf*d - sf*q; // Faster Inverse DQ0 transform 00014 *b = (0.86602540378f*sf-.5f*cf)*d - (-0.86602540378f*cf-.5f*sf)*q; 00015 *c = (-0.86602540378f*sf-.5f*cf)*d - (0.86602540378f*cf-.5f*sf)*q; 00016 } 00017 00018 00019 void dq0(float theta, float a, float b, float c, float *d, float *q){ 00020 /// DQ0 Transform /// 00021 ///Phase current amplitude = lengh of dq vector/// 00022 ///i.e. iq = 1, id = 0, peak phase current of 1/// 00023 00024 float cf = FastCos(theta); 00025 float sf = FastSin(theta); 00026 00027 *d = 0.6666667f*(cf*a + (0.86602540378f*sf-.5f*cf)*b + (-0.86602540378f*sf-.5f*cf)*c); ///Faster DQ0 Transform 00028 *q = 0.6666667f*(-sf*a - (-0.86602540378f*cf-.5f*sf)*b - (0.86602540378f*cf-.5f*sf)*c); 00029 00030 } 00031 00032 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){ 00033 /// Space Vector Modulation /// 00034 /// u,v,w amplitude = v_bus for full modulation depth /// 00035 00036 float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f; 00037 *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 00038 *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 00039 *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 00040 00041 } 00042 00043 void zero_current(int *offset_1, int *offset_2){ // Measure zero-offset of the current sensors 00044 int adc1_offset = 0; 00045 int adc2_offset = 0; 00046 int n = 1024; 00047 for (int i = 0; i<n; i++){ // Average n samples of the ADC 00048 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f); // Write duty cycles 00049 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f); 00050 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f); 00051 ADC1->CR2 |= 0x40000000; // Begin sample and conversion 00052 wait(.001); 00053 adc2_offset += ADC2->DR; 00054 adc1_offset += ADC1->DR; 00055 } 00056 *offset_1 = adc1_offset/n; 00057 *offset_2 = adc2_offset/n; 00058 } 00059 00060 void reset_foc(ControllerStruct *controller){ 00061 TIM1->CCR3 = (PWM_ARR>>1)*(0.5f); 00062 TIM1->CCR1 = (PWM_ARR>>1)*(0.5f); 00063 TIM1->CCR2 = (PWM_ARR>>1)*(0.5f); 00064 controller->i_d_ref = 0; 00065 controller->i_q_ref = 0; 00066 controller->i_d = 0; 00067 controller->i_q = 0; 00068 controller->i_q_filt = 0; 00069 controller->q_int = 0; 00070 controller->d_int = 0; 00071 controller->v_q = 0; 00072 controller->v_d = 0; 00073 } 00074 00075 00076 void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){ 00077 /// Observer Prediction /// 00078 observer->i_d_est += DT*(observer->i_d_dot); 00079 observer->i_q_est += DT*(observer->i_q_dot); 00080 00081 /// Commutation Loop /// 00082 controller->loop_count ++; 00083 if(PHASE_ORDER){ // Check current sensor ordering 00084 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); // Calculate phase currents from ADC readings 00085 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); 00086 } 00087 else{ 00088 controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); 00089 controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); 00090 } 00091 controller->i_a = -controller->i_b - controller->i_c; 00092 00093 float s = FastSin(theta); 00094 float c = FastCos(theta); 00095 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents 00096 //controller->i_d = 0.6666667f*(c*controller->i_a + (0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c); ///Faster DQ0 Transform 00097 //controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - (0.86602540378f*c-.5f*s)*controller->i_c); 00098 00099 controller->i_q_filt = 0.95f*controller->i_q_filt + 0.05f*controller->i_q; 00100 observer->i_d_m = controller->i_d; 00101 observer->i_q_m = controller->i_q; 00102 00103 observer->e_d = observer->i_d_m - observer->i_d_est; 00104 observer->e_q = observer->i_q_m - observer->i_q_est; 00105 observer->e_d_int += observer->e_d; 00106 observer->e_q_int += observer->e_q; 00107 00108 observer->i_d_est += K_O*observer->e_d + .001f*observer->e_d_int; 00109 observer->i_q_est += K_O*observer->e_q + .001f*observer->e_q_int; 00110 00111 00112 float scog12 = FastSin(12.0f*theta); 00113 float scog1 = s; 00114 float cogging_current = 0.25f*scog1 - 0.3f*scog12; 00115 00116 /// PI Controller /// 00117 float i_d_error = controller->i_d_ref - controller->i_d; 00118 float i_q_error = controller->i_q_ref - controller->i_q + cogging_current; 00119 00120 float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE - controller->dtheta_elec*L_Q*controller->i_q_ref); //feed-forward voltages 00121 float v_q_ff = 2.0f*(controller->i_q_ref*R_PHASE + controller->dtheta_elec*(L_D*controller->i_d_ref + WB)); 00122 00123 controller->d_int += i_d_error; 00124 controller->q_int += i_q_error; 00125 00126 //v_d_ff = 0; 00127 //v_q_ff = 0; 00128 00129 limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_SCALE*I_BW*KI_Q)); // Limit integrators to prevent windup 00130 controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int;// + v_d_ff; 00131 controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int;// + v_q_ff; 00132 00133 //controller->v_q = 4.0f; 00134 //controller->v_d = 0.0f; 00135 00136 //controller->v_d = v_d_ff; 00137 //controller->v_q = v_q_ff; 00138 00139 limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus); // Normalize voltage vector to lie within curcle of radius v_bus 00140 abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages 00141 00142 //controller->v_u = c*controller->v_d - s*controller->v_q; // Faster Inverse DQ0 transform 00143 //controller->v_v = (0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q; 00144 //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q; 00145 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 00146 00147 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 00148 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; 00149 00150 if(PHASE_ORDER){ // Check which phase order to use, 00151 TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u); // Write duty cycles 00152 TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v); 00153 TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_w); 00154 } 00155 else{ 00156 TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u); 00157 TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v); 00158 TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_w); 00159 } 00160 00161 controller->theta_elec = theta; //For some reason putting this at the front breaks thins 00162 00163 00164 if(controller->loop_count >400){ 00165 //controller->i_q_ref = -controller->i_q_ref; 00166 controller->loop_count = 0; 00167 00168 //printf("%.2f %.2f %.2f\n\r", controller->i_a, controller->i_b, controller->i_c); 00169 //printf("%f\n\r", controller->dtheta_mech*GR); 00170 //pc.printf("%f %f %f\n\r", controller->i_a, controller->i_b, controller->i_c); 00171 //pc.printf("%f %f\n\r", controller->i_d, controller->i_q); 00172 //pc.printf("%d %d\n\r", controller->adc1_raw, controller->adc2_raw); 00173 } 00174 } 00175 00176 00177 void torque_control(ControllerStruct *controller){ 00178 float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech); 00179 //float torque_ref = -.1*(controller->p_des - controller->theta_mech); 00180 controller->i_q_ref = torque_ref/KT_OUT; 00181 controller->i_d_ref = 0.0f; 00182 } 00183 00184 00185 /* 00186 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){ 00187 00188 } 00189 */ 00190 /**********************************WYC 2021.10.22*****************************************/ 00191 float PID_operator (ControllerStruct *controller){ 00192 //printf("piding"); 00193 // calculate the time from the last call 00194 unsigned long timestamp_now = controller->loop_count; //check the loop_count 00195 float Ts = (timestamp_now - controller->timestamp_prev) * DT; 00196 //printf("ts:%f\n",Ts); 00197 00198 // quick fix for strange cases (micros overflow) 00199 if(Ts <= 0 || Ts > 0.5f) Ts = 0.001; 00200 00201 // u(s) = (P + I/s + Ds)e(s) 00202 // Discrete implementations 00203 // proportional part 00204 // u_p = P *e(k) 00205 float error = controller->error; 00206 float proportional = controller->kp * error; 00207 // Tustin transform of the integral part 00208 // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) 00209 float integral = controller->integral_prev + controller->ki*Ts*0.5f*(error + controller->error_prev); 00210 // antiwindup - limit the output 00211 limit(&integral, -(controller->LIMIT), controller->LIMIT); 00212 // Discrete derivation 00213 // u_dk = D(ek - ek_1)/Ts 00214 float derivative = controller->kd*(error - controller->error_prev)/Ts; 00215 00216 // sum all the components 00217 float output = proportional + integral + derivative; 00218 // antiwindup - limit the output variable 00219 00220 //limit(&output, -(controller->LIMIT), controller->LIMIT); 00221 00222 /* 00223 // if output ramp defined 00224 if(output_ramp > 0){ 00225 // limit the acceleration by ramping the output 00226 float output_rate = (output - output_prev)/Ts; 00227 if (output_rate > output_ramp) 00228 output = output_prev + output_ramp*Ts; 00229 else if (output_rate < -output_ramp) 00230 output = output_prev - output_ramp*Ts; 00231 } 00232 */ 00233 // saving for the next pass 00234 controller->integral_prev = integral; 00235 controller->output_prev = output; 00236 controller->error_prev = error; 00237 controller->timestamp_prev = timestamp_now; 00238 return output; 00239 } 00240 /*********WYC ADD 2021.10.22**********/ 00241 00242 void velocity_control(ControllerStruct *controller){ 00243 00244 controller->ki = controller->kd; 00245 controller->kd = 0; 00246 controller->error = (controller->v_des - controller->dtheta_mech); 00247 00248 float current_sp = PID_operator (controller); 00249 controller->i_q_ref = current_sp; //wyc 2021.07.26 compared with the program in mbed,old motor with no "-"in the front of "torque_des" 00250 //printf("iq:%f\n",controller->i_q_des); 00251 controller->i_d_ref = 0.0f; 00252 00253 } 00254 /*********WYC ADD 2021.10.22**********/ 00255 00256 00257 00258 /*********YZ ADD 2021.10.22**********/ 00259 00260 void Position_control(ControllerStruct *controller){ 00261 00262 controller->ki = controller->kd; 00263 controller->kd = 0; 00264 controller->error = (controller->p_des - controller->theta_mech); 00265 00266 float current_Position = PID_operator (controller); 00267 controller->i_q_ref=current_Position; //YZ 2021.07.26 compared with the program in mbed,old motor with no "-"in the front of "torque_des" 00268 //printf("iq:%f\n",controller->i_q_des); 00269 controller->i_d_ref = 0.0f; 00270 00271 } 00272 /*********YZ ADD 2021.10.22**********/
Generated on Sun Aug 14 2022 10:16:20 by
1.7.2