Test the set param mode
Diff: Calibration/calibration.cpp
- Revision:
- 22:60276ba87ac6
- Child:
- 23:2adf23ee0305
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Calibration/calibration.cpp Fri Mar 31 18:24:46 2017 +0000 @@ -0,0 +1,201 @@ +/// Calibration procedures for determining position sensor offset, +/// phase ordering, and position sensor linearization +/// + +#include "calibration.h" + + +void order_phases(PositionSensor *ps, GPIOStruct *gpio){ + ///Checks phase order, to ensure that positive Q current produces + ///torque in the positive direction wrt the position sensor. + + 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_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 + for(int i = 0; i<20000; i++){ + 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); + } + //ps->ZeroPosition(); + ps->Sample(); + float theta_start = ps->GetMechPosition(); //get initial rotor position + + /// 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 + wait_us(100); + 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 + theta_actual = ps->GetMechPosition(); + if(sample_counter > 200){ + sample_counter = 0 ; + printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual); + } + sample_counter++; + theta_ref += 0.001f; + } + float theta_end = ps->GetMechPosition(); + int direction = (theta_end - theta_start)>0; + printf("Theta Start: %f Theta End: %f\n\r", theta_start, theta_end); + printf("Direction: %d\n\r", direction); + if(direction){printf("Phaseing correct\n\r");} + else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");} + gpio->phasing = direction; + + } + + + +void calibrate(PositionSensor *ps, GPIOStruct *gpio){ + /// Measures the electrical angle offset of the position sensor + /// 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 + 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_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 + for(int i = 0; i<40000; i++){ + TIM1->CCR3 = 0x708*(1.0f-dtc_u); // Set duty cycles + if(gpio->phasing){ + TIM1->CCR2 = 0x708*(1.0f-dtc_v); + TIM1->CCR1 = 0x708*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = 0x708*(1.0f-dtc_v); + TIM1->CCR2 = 0x708*(1.0f-dtc_w); + } + wait_us(100); + } + ps->Sample(); + + 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 + TIM1->CCR3 = 0x708*(1.0f-dtc_u); + if(gpio->phasing){ + TIM1->CCR2 = 0x708*(1.0f-dtc_v); + TIM1->CCR1 = 0x708*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = 0x708*(1.0f-dtc_v); + TIM1->CCR2 = 0x708*(1.0f-dtc_w); + } + wait_us(100); + ps->Sample(); + } + ps->Sample(); + theta_actual = ps->GetMechPosition(); + error_f[i] = theta_ref/NPP - theta_actual; + raw_f[i] = ps->GetRawPosition(); + printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]); + //theta_ref += delta; + } + 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 + TIM1->CCR3 = 0x708*(1.0f-dtc_u); + if(gpio->phasing){ + TIM1->CCR2 = 0x708*(1.0f-dtc_v); + TIM1->CCR1 = 0x708*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = 0x708*(1.0f-dtc_v); + TIM1->CCR2 = 0x708*(1.0f-dtc_w); + } + wait_us(100); + ps->Sample(); + } + 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]); + //theta_ref -= delta; + } + + 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 = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle + printf("Encoder Electrical Offset (rad) %f\n\r", offset); + + ps->SetElecOffset(offset); // Set position sensor 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 also be completely filtered out. + + float error[n] = {0}; + int window = 128; + float error_filt[n] = {0}; + float mean = 0; + 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 + if(ind<0){ + ind += n;} // Moving average wraps around + else if(ind > n-1) { + ind -= n;} + error_filt[i] += error[ind]/(float)window; + } + //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 + const int n_lut = 128; + int lut[n_lut]; + 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; + } + lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI)); + printf("%d %d %d %d\n\r", raw_offset>>7, i, ind, lut[ind]); + } + ps->WriteLUT(lut); // write lookup table to position sensor object + + + + + + + } \ No newline at end of file