Test the set param mode
Diff: Calibration/calibration.cpp
- Revision:
- 25:f5741040c4bb
- Parent:
- 24:58c2d7571207
- Child:
- 26:2b865c00d7e9
--- a/Calibration/calibration.cpp Fri Apr 07 16:23:39 2017 +0000 +++ b/Calibration/calibration.cpp Sun Apr 09 03:05:52 2017 +0000 @@ -13,17 +13,17 @@ printf("\n\r Checking phase ordering\n\r"); float theta_ref = 0; float theta_actual = 0; - float v_d = .2; //Put all volts on the D-Axis + float v_d = .2; //Put all volts on the D-Axis float v_q = 0.0; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5; int sample_counter = 0; ///Set voltage angle to zero, wait for rotor position to settle - abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages - svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation for(int i = 0; i<20000; i++){ - TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles + TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles TIM1->CCR2 = 0x708*(1.0f-dtc_v); TIM1->CCR1 = 0x708*(1.0f-dtc_w); wait_us(100); @@ -31,7 +31,7 @@ //ps->ZeroPosition(); ps->Sample(); wait_us(1000); - float theta_start = ps->GetMechPosition(); //get initial rotor position + float theta_start = ps->GetMechPosition(); //get initial rotor position controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); controller->i_a = -controller->i_b - controller->i_c; @@ -40,14 +40,14 @@ printf("\n\rCurrent\n\r"); printf("%f %f %f\n\r\n\r", controller->i_d, controller->i_q, current); /// Rotate voltage angle - while(theta_ref < 4*PI){ //rotate for 2 electrical cycles - abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages - svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation + while(theta_ref < 4*PI){ //rotate for 2 electrical cycles + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation wait_us(100); - TIM1->CCR3 = 0x708*(1.0f-dtc_u); //Set duty cycles + TIM1->CCR3 = 0x708*(1.0f-dtc_u); //Set duty cycles TIM1->CCR2 = 0x708*(1.0f-dtc_v); TIM1->CCR1 = 0x708*(1.0f-dtc_w); - ps->Sample(); //sample position sensor + ps->Sample(); //sample position sensor theta_actual = ps->GetMechPosition(); if(sample_counter > 200){ sample_counter = 0 ; @@ -71,26 +71,26 @@ /// and (in the future) corrects nonlinearity due to position sensor eccentricity printf("Starting calibration procedure\n\r"); - const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) - const int n2 = 10; // increments between saved samples (for smoothing motion) - float delta = 2*PI*NPP/(n*n2); // change in angle between samples - float error_f[n] = {0}; // error vector rotating forwards - float error_b[n] = {0}; // error vector rotating backwards + const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2 = 50; // increments between saved samples (for smoothing motion) + float delta = 2*PI*NPP/(n*n2); // change in angle between samples + float error_f[n] = {0}; // error vector rotating forwards + float error_b[n] = {0}; // error vector rotating backwards int raw_f[n] = {0}; int raw_b[n] = {0}; float theta_ref = 0; float theta_actual = 0; - float v_d = .2; // Put volts on the D-Axis + float v_d = .2; // Put volts on the D-Axis float v_q = 0.0; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5; ///Set voltage angle to zero, wait for rotor position to settle - abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages - svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation for(int i = 0; i<40000; i++){ - TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles + TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles if(PHASE_ORDER){ TIM1->CCR2 = 0x708*(1.0f-dtc_v); TIM1->CCR1 = 0x708*(1.0f-dtc_w); @@ -107,14 +107,14 @@ controller->i_a = -controller->i_b - controller->i_c; dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); - for(int i = 0; i<n; i++){ // rotate forwards + for(int i = 0; i<n; i++){ // rotate forwards for(int j = 0; j<n2; j++){ theta_ref += delta; - abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages - svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation TIM1->CCR3 = 0x708*(1.0f-dtc_u); - if(PHASE_ORDER){ - TIM1->CCR2 = 0x708*(1.0f-dtc_v); + if(PHASE_ORDER){ // Check phase ordering + TIM1->CCR2 = 0x708*(1.0f-dtc_v); // Set duty cycles TIM1->CCR1 = 0x708*(1.0f-dtc_w); } else{ @@ -132,11 +132,11 @@ //theta_ref += delta; } printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); - for(int i = 0; i<n; i++){ // rotate backwards + for(int i = 0; i<n; i++){ // rotate backwards for(int j = 0; j<n2; j++){ theta_ref -= delta; - abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages - svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation TIM1->CCR3 = 0x708*(1.0f-dtc_u); if(PHASE_ORDER){ TIM1->CCR2 = 0x708*(1.0f-dtc_v); @@ -149,8 +149,8 @@ wait_us(100); ps->Sample(); } - ps->Sample(); // sample position sensor - theta_actual = ps->GetMechPosition(); // get mechanical position + ps->Sample(); // sample position sensor + theta_actual = ps->GetMechPosition(); // get mechanical position error_b[i] = theta_ref/NPP - theta_actual; raw_b[i] = ps->GetRawPosition(); printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); @@ -159,33 +159,33 @@ float offset = 0; for(int i = 0; i<n; i++){ - offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset + offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset } - offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle + offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle - ps->SetElecOffset(offset); // Set position sensor offset + ps->SetElecOffset(offset); // Set position sensor offset __float_reg[0] = offset; E_OFFSET = offset; /// Perform filtering to linearize position sensor eccentricity /// FIR n-sample average, where n = number of samples in one electrical cycle /// This filter has zero gain at electrical frequency and all integer multiples - /// So cogging should be completely filtered out. + /// So cogging effects should be completely filtered out. float error[n] = {0}; const int window = 128; float error_filt[n] = {0}; float cogging_current[window] = {0}; float mean = 0; - for (int i = 0; i<n; i++){ //Average the forward and back directions + for (int i = 0; i<n; i++){ //Average the forward and back directions error[i] = 0.5f*(error_f[i] + error_b[n-i-1]); } for (int i = 0; i<n; i++){ for(int j = 0; j<window; j++){ - int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 + int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 if(ind<0){ - ind += n;} // Moving average wraps around + ind += n;} // Moving average wraps around else if(ind > n-1) { ind -= n;} error_filt[i] += error[ind]/(float)window; @@ -196,12 +196,12 @@ //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); mean += error_filt[i]/n; } - int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty + int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty const int n_lut = 128; int lut[n_lut]; printf("\n\r Encoder non-linearity compensation table\n\r"); printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r"); - for (int i = 0; i<n_lut; i++){ // build lookup table + for (int i = 0; i<n_lut; i++){ // build lookup table int ind = (raw_offset>>7) + i; if(ind > (n_lut-1)){ ind -= n_lut; @@ -210,14 +210,14 @@ printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]); } - ps->WriteLUT(lut); // write lookup table to position sensor object + ps->WriteLUT(lut); // write lookup table to position sensor object //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... - memcpy(&ENCODER_LUT, lut, sizeof(lut)); + memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset); if (!prefs->ready()) prefs->open(); - prefs->flush(); - //prefs->close(); + prefs->flush(); // write offset and lookup table to flash + prefs->close(); } \ No newline at end of file