hh
Embed:
(wiki syntax)
Show/hide line numbers
calibration.cpp
00001 /// Calibration procedures for determining position sensor offset, 00002 /// phase ordering, and position sensor linearization 00003 /// 00004 00005 #include "calibration.h" 00006 #include "foc.h" 00007 #include "PreferenceWriter.h" 00008 #include "user_config.h" 00009 #include "motor_config.h" 00010 #include "current_controller_config.h" 00011 00012 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ 00013 00014 ///Checks phase order, to ensure that positive Q current produces 00015 ///torque in the positive direction wrt the position sensor. 00016 printf("\n\r Checking phase ordering\n\r"); 00017 float theta_ref = 0; 00018 float theta_actual = 0; 00019 float v_d = V_CAL; //Put all volts on the D-Axis 00020 float v_q = 0.0f; 00021 float v_u, v_v, v_w = 0; 00022 float dtc_u, dtc_v, dtc_w = .5f; 00023 int sample_counter = 0; 00024 00025 ///Set voltage angle to zero, wait for rotor position to settle 00026 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages 00027 svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation 00028 for(int i = 0; i<20000; i++){ 00029 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles 00030 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); 00031 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); 00032 wait_us(100); 00033 } 00034 //ps->ZeroPosition(); 00035 ps->Sample(DT); 00036 wait_us(1000); 00037 //float theta_start = ps->GetMechPositionFixed(); //get initial rotor position 00038 float theta_start; 00039 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings 00040 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); 00041 controller->i_a = -controller->i_b - controller->i_c; 00042 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents 00043 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); 00044 printf("\n\rCurrent\n\r"); 00045 printf("%f %f %f\n\r\n\r", controller->i_d, controller->i_q, current); 00046 /// Rotate voltage angle 00047 while(theta_ref < 4*PI){ //rotate for 2 electrical cycles 00048 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages 00049 svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation 00050 wait_us(100); 00051 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); //Set duty cycles 00052 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); 00053 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); 00054 ps->Sample(DT); //sample position sensor 00055 theta_actual = ps->GetMechPositionFixed(); 00056 if(theta_ref==0){theta_start = theta_actual;} 00057 if(sample_counter > 200){ 00058 sample_counter = 0 ; 00059 printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual); 00060 } 00061 sample_counter++; 00062 theta_ref += 0.001f; 00063 } 00064 float theta_end = ps->GetMechPositionFixed(); 00065 int direction = (theta_end - theta_start)>0; 00066 printf("Theta Start: %f Theta End: %f\n\r", theta_start, theta_end); 00067 printf("Direction: %d\n\r", direction); 00068 if(direction){printf("Phasing correct\n\r");} 00069 else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");} 00070 PHASE_ORDER = direction; 00071 } 00072 00073 00074 void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ 00075 /// Measures the electrical angle offset of the position sensor 00076 /// and (in the future) corrects nonlinearity due to position sensor eccentricity 00077 printf("Starting calibration procedure\n\r"); 00078 float * error_f; 00079 float * error_b; 00080 int * lut; 00081 int * raw_f; 00082 int * raw_b; 00083 float * error; 00084 float * error_filt; 00085 00086 const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) 00087 const int n2 = 40; // increments between saved samples (for smoothing motion) 00088 float delta = 2*PI*NPP/(n*n2); // change in angle between samples 00089 error_f = new float[n](); // error vector rotating forwards 00090 error_b = new float[n](); // error vector rotating backwards 00091 const int n_lut = 128; 00092 lut = new int[n_lut](); // clear any old lookup table before starting. 00093 00094 error = new float[n](); 00095 const int window = 128; 00096 error_filt = new float[n](); 00097 float cogging_current[window] = {0}; 00098 00099 ps->WriteLUT(lut); 00100 raw_f = new int[n](); 00101 raw_b = new int[n](); 00102 float theta_ref = 0; 00103 float theta_actual = 0; 00104 float v_d = V_CAL; // Put volts on the D-Axis 00105 float v_q = 0.0f; 00106 float v_u, v_v, v_w = 0; 00107 float dtc_u, dtc_v, dtc_w = .5f; 00108 00109 00110 ///Set voltage angle to zero, wait for rotor position to settle 00111 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages 00112 svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation 00113 for(int i = 0; i<40000; i++){ 00114 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles 00115 if(PHASE_ORDER){ 00116 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); 00117 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); 00118 } 00119 else{ 00120 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); 00121 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); 00122 } 00123 wait_us(100); 00124 } 00125 ps->Sample(DT); 00126 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings 00127 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); 00128 controller->i_a = -controller->i_b - controller->i_c; 00129 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents 00130 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); 00131 printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); 00132 for(int i = 0; i<n; i++){ // rotate forwards 00133 for(int j = 0; j<n2; j++){ 00134 theta_ref += delta; 00135 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages 00136 svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation 00137 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); 00138 if(PHASE_ORDER){ // Check phase ordering 00139 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles 00140 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); 00141 } 00142 else{ 00143 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); 00144 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); 00145 } 00146 wait_us(100); 00147 ps->Sample(DT); 00148 } 00149 ps->Sample(DT); 00150 theta_actual = ps->GetMechPositionFixed(); 00151 error_f[i] = theta_ref/NPP - theta_actual; 00152 raw_f[i] = ps->GetRawPosition(); 00153 printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]); 00154 //theta_ref += delta; 00155 } 00156 00157 for(int i = 0; i<n; i++){ // rotate backwards 00158 for(int j = 0; j<n2; j++){ 00159 theta_ref -= delta; 00160 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages 00161 svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation 00162 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); 00163 if(PHASE_ORDER){ 00164 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); 00165 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); 00166 } 00167 else{ 00168 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); 00169 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); 00170 } 00171 wait_us(100); 00172 ps->Sample(DT); 00173 } 00174 ps->Sample(DT); // sample position sensor 00175 theta_actual = ps->GetMechPositionFixed(); // get mechanical position 00176 error_b[i] = theta_ref/NPP - theta_actual; 00177 raw_b[i] = ps->GetRawPosition(); 00178 printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); 00179 //theta_ref -= delta; 00180 } 00181 00182 float offset = 0; 00183 for(int i = 0; i<n; i++){ 00184 offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset 00185 } 00186 offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle 00187 00188 00189 ps->SetElecOffset(offset); // Set position sensor offset 00190 __float_reg[0] = offset; 00191 E_OFFSET = offset; 00192 00193 /// Perform filtering to linearize position sensor eccentricity 00194 /// FIR n-sample average, where n = number of samples in one electrical cycle 00195 /// This filter has zero gain at electrical frequency and all integer multiples 00196 /// So cogging effects should be completely filtered out. 00197 00198 00199 float mean = 0; 00200 for (int i = 0; i<n; i++){ //Average the forward and back directions 00201 error[i] = 0.5f*(error_f[i] + error_b[n-i-1]); 00202 } 00203 for (int i = 0; i<n; i++){ 00204 for(int j = 0; j<window; j++){ 00205 int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 00206 if(ind<0){ 00207 ind += n;} // Moving average wraps around 00208 else if(ind > n-1) { 00209 ind -= n;} 00210 error_filt[i] += error[ind]/(float)window; 00211 } 00212 if(i<window){ 00213 cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP); 00214 } 00215 //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); 00216 mean += error_filt[i]/n; 00217 } 00218 int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty 00219 00220 00221 printf("\n\r Encoder non-linearity compensation table\n\r"); 00222 printf(" Sample Number : Lookup Index : Lookup Value\n\r\n\r"); 00223 for (int i = 0; i<n_lut; i++){ // build lookup table 00224 int ind = (raw_offset>>7) + i; 00225 if(ind > (n_lut-1)){ 00226 ind -= n_lut; 00227 } 00228 lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI)); 00229 printf("%d %d %d \n\r", i, ind, lut[ind]); 00230 wait(.001); 00231 } 00232 00233 ps->WriteLUT(lut); // write lookup table to position sensor object 00234 //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... 00235 memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array 00236 printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset); 00237 00238 if (!prefs->ready()) prefs->open(); 00239 prefs->flush(); // write offset and lookup table to flash 00240 prefs->close(); 00241 00242 delete[] error_f; //gotta free up that ram 00243 delete[] error_b; 00244 delete[] lut; 00245 delete[] raw_f; 00246 delete[] raw_b; 00247 00248 }
Generated on Fri Jul 22 2022 01:43:47 by 1.7.2