Test the set param mode
Calibration/calibration.cpp@38:67e4e1453a4b, 2018-05-14 (annotated)
- Committer:
- benkatz
- Date:
- Mon May 14 20:59:02 2018 +0000
- Revision:
- 38:67e4e1453a4b
- Parent:
- 28:8c7e29f719c5
- Child:
- 44:8040fa2fcb0d
Fixed bug introduced into the calibration procedure when absolute zero offset was added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 22:60276ba87ac6 | 1 | /// Calibration procedures for determining position sensor offset, |
benkatz | 22:60276ba87ac6 | 2 | /// phase ordering, and position sensor linearization |
benkatz | 22:60276ba87ac6 | 3 | /// |
benkatz | 22:60276ba87ac6 | 4 | |
benkatz | 22:60276ba87ac6 | 5 | #include "calibration.h" |
benkatz | 23:2adf23ee0305 | 6 | #include "foc.h" |
benkatz | 23:2adf23ee0305 | 7 | #include "PreferenceWriter.h" |
benkatz | 23:2adf23ee0305 | 8 | #include "user_config.h" |
benkatz | 22:60276ba87ac6 | 9 | |
benkatz | 23:2adf23ee0305 | 10 | void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ |
benkatz | 28:8c7e29f719c5 | 11 | |
benkatz | 22:60276ba87ac6 | 12 | ///Checks phase order, to ensure that positive Q current produces |
benkatz | 22:60276ba87ac6 | 13 | ///torque in the positive direction wrt the position sensor. |
benkatz | 22:60276ba87ac6 | 14 | printf("\n\r Checking phase ordering\n\r"); |
benkatz | 22:60276ba87ac6 | 15 | float theta_ref = 0; |
benkatz | 22:60276ba87ac6 | 16 | float theta_actual = 0; |
benkatz | 28:8c7e29f719c5 | 17 | float v_d = .25; //Put all volts on the D-Axis |
benkatz | 22:60276ba87ac6 | 18 | float v_q = 0.0; |
benkatz | 22:60276ba87ac6 | 19 | float v_u, v_v, v_w = 0; |
benkatz | 22:60276ba87ac6 | 20 | float dtc_u, dtc_v, dtc_w = .5; |
benkatz | 22:60276ba87ac6 | 21 | int sample_counter = 0; |
benkatz | 22:60276ba87ac6 | 22 | |
benkatz | 22:60276ba87ac6 | 23 | ///Set voltage angle to zero, wait for rotor position to settle |
benkatz | 25:f5741040c4bb | 24 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages |
benkatz | 25:f5741040c4bb | 25 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation |
benkatz | 22:60276ba87ac6 | 26 | for(int i = 0; i<20000; i++){ |
benkatz | 27:501fee691e0e | 27 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles |
benkatz | 27:501fee691e0e | 28 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 29 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 30 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 31 | } |
benkatz | 22:60276ba87ac6 | 32 | //ps->ZeroPosition(); |
benkatz | 22:60276ba87ac6 | 33 | ps->Sample(); |
benkatz | 23:2adf23ee0305 | 34 | wait_us(1000); |
benkatz | 38:67e4e1453a4b | 35 | //float theta_start = ps->GetMechPositionFixed(); //get initial rotor position |
benkatz | 26:2b865c00d7e9 | 36 | float theta_start; |
benkatz | 23:2adf23ee0305 | 37 | controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings |
benkatz | 23:2adf23ee0305 | 38 | controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); |
benkatz | 23:2adf23ee0305 | 39 | controller->i_a = -controller->i_b - controller->i_c; |
benkatz | 23:2adf23ee0305 | 40 | dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents |
benkatz | 23:2adf23ee0305 | 41 | float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); |
benkatz | 23:2adf23ee0305 | 42 | printf("\n\rCurrent\n\r"); |
benkatz | 23:2adf23ee0305 | 43 | printf("%f %f %f\n\r\n\r", controller->i_d, controller->i_q, current); |
benkatz | 22:60276ba87ac6 | 44 | /// Rotate voltage angle |
benkatz | 25:f5741040c4bb | 45 | while(theta_ref < 4*PI){ //rotate for 2 electrical cycles |
benkatz | 25:f5741040c4bb | 46 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages |
benkatz | 25:f5741040c4bb | 47 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation |
benkatz | 22:60276ba87ac6 | 48 | wait_us(100); |
benkatz | 27:501fee691e0e | 49 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); //Set duty cycles |
benkatz | 27:501fee691e0e | 50 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 51 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 25:f5741040c4bb | 52 | ps->Sample(); //sample position sensor |
benkatz | 38:67e4e1453a4b | 53 | theta_actual = ps->GetMechPositionFixed(); |
benkatz | 26:2b865c00d7e9 | 54 | if(theta_ref==0){theta_start = theta_actual;} |
benkatz | 22:60276ba87ac6 | 55 | if(sample_counter > 200){ |
benkatz | 22:60276ba87ac6 | 56 | sample_counter = 0 ; |
benkatz | 22:60276ba87ac6 | 57 | printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual); |
benkatz | 22:60276ba87ac6 | 58 | } |
benkatz | 22:60276ba87ac6 | 59 | sample_counter++; |
benkatz | 22:60276ba87ac6 | 60 | theta_ref += 0.001f; |
benkatz | 22:60276ba87ac6 | 61 | } |
benkatz | 38:67e4e1453a4b | 62 | float theta_end = ps->GetMechPositionFixed(); |
benkatz | 22:60276ba87ac6 | 63 | int direction = (theta_end - theta_start)>0; |
benkatz | 22:60276ba87ac6 | 64 | printf("Theta Start: %f Theta End: %f\n\r", theta_start, theta_end); |
benkatz | 22:60276ba87ac6 | 65 | printf("Direction: %d\n\r", direction); |
benkatz | 23:2adf23ee0305 | 66 | if(direction){printf("Phasing correct\n\r");} |
benkatz | 22:60276ba87ac6 | 67 | else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");} |
benkatz | 23:2adf23ee0305 | 68 | PHASE_ORDER = direction; |
benkatz | 22:60276ba87ac6 | 69 | } |
benkatz | 22:60276ba87ac6 | 70 | |
benkatz | 22:60276ba87ac6 | 71 | |
benkatz | 23:2adf23ee0305 | 72 | void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ |
benkatz | 22:60276ba87ac6 | 73 | /// Measures the electrical angle offset of the position sensor |
benkatz | 22:60276ba87ac6 | 74 | /// and (in the future) corrects nonlinearity due to position sensor eccentricity |
benkatz | 22:60276ba87ac6 | 75 | printf("Starting calibration procedure\n\r"); |
benkatz | 22:60276ba87ac6 | 76 | |
benkatz | 25:f5741040c4bb | 77 | const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) |
benkatz | 25:f5741040c4bb | 78 | const int n2 = 50; // increments between saved samples (for smoothing motion) |
benkatz | 25:f5741040c4bb | 79 | float delta = 2*PI*NPP/(n*n2); // change in angle between samples |
benkatz | 25:f5741040c4bb | 80 | float error_f[n] = {0}; // error vector rotating forwards |
benkatz | 25:f5741040c4bb | 81 | float error_b[n] = {0}; // error vector rotating backwards |
benkatz | 28:8c7e29f719c5 | 82 | const int n_lut = 128; |
benkatz | 28:8c7e29f719c5 | 83 | int lut[n_lut]= {0}; // clear any old lookup table before starting. |
benkatz | 28:8c7e29f719c5 | 84 | ps->WriteLUT(lut); |
benkatz | 22:60276ba87ac6 | 85 | int raw_f[n] = {0}; |
benkatz | 22:60276ba87ac6 | 86 | int raw_b[n] = {0}; |
benkatz | 22:60276ba87ac6 | 87 | float theta_ref = 0; |
benkatz | 22:60276ba87ac6 | 88 | float theta_actual = 0; |
benkatz | 28:8c7e29f719c5 | 89 | float v_d = .25; // Put volts on the D-Axis |
benkatz | 22:60276ba87ac6 | 90 | float v_q = 0.0; |
benkatz | 22:60276ba87ac6 | 91 | float v_u, v_v, v_w = 0; |
benkatz | 22:60276ba87ac6 | 92 | float dtc_u, dtc_v, dtc_w = .5; |
benkatz | 22:60276ba87ac6 | 93 | |
benkatz | 22:60276ba87ac6 | 94 | |
benkatz | 22:60276ba87ac6 | 95 | ///Set voltage angle to zero, wait for rotor position to settle |
benkatz | 25:f5741040c4bb | 96 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
benkatz | 25:f5741040c4bb | 97 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
benkatz | 22:60276ba87ac6 | 98 | for(int i = 0; i<40000; i++){ |
benkatz | 27:501fee691e0e | 99 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles |
benkatz | 24:58c2d7571207 | 100 | if(PHASE_ORDER){ |
benkatz | 27:501fee691e0e | 101 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 102 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 103 | } |
benkatz | 22:60276ba87ac6 | 104 | else{ |
benkatz | 27:501fee691e0e | 105 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 106 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 107 | } |
benkatz | 22:60276ba87ac6 | 108 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 109 | } |
benkatz | 22:60276ba87ac6 | 110 | ps->Sample(); |
benkatz | 23:2adf23ee0305 | 111 | controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings |
benkatz | 23:2adf23ee0305 | 112 | controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); |
benkatz | 23:2adf23ee0305 | 113 | controller->i_a = -controller->i_b - controller->i_c; |
benkatz | 23:2adf23ee0305 | 114 | dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents |
benkatz | 23:2adf23ee0305 | 115 | float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); |
benkatz | 26:2b865c00d7e9 | 116 | printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); |
benkatz | 25:f5741040c4bb | 117 | for(int i = 0; i<n; i++){ // rotate forwards |
benkatz | 22:60276ba87ac6 | 118 | for(int j = 0; j<n2; j++){ |
benkatz | 22:60276ba87ac6 | 119 | theta_ref += delta; |
benkatz | 25:f5741040c4bb | 120 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
benkatz | 25:f5741040c4bb | 121 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
benkatz | 27:501fee691e0e | 122 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); |
benkatz | 25:f5741040c4bb | 123 | if(PHASE_ORDER){ // Check phase ordering |
benkatz | 27:501fee691e0e | 124 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles |
benkatz | 27:501fee691e0e | 125 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 126 | } |
benkatz | 22:60276ba87ac6 | 127 | else{ |
benkatz | 27:501fee691e0e | 128 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 129 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 130 | } |
benkatz | 22:60276ba87ac6 | 131 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 132 | ps->Sample(); |
benkatz | 22:60276ba87ac6 | 133 | } |
benkatz | 22:60276ba87ac6 | 134 | ps->Sample(); |
benkatz | 38:67e4e1453a4b | 135 | theta_actual = ps->GetMechPositionFixed(); |
benkatz | 22:60276ba87ac6 | 136 | error_f[i] = theta_ref/NPP - theta_actual; |
benkatz | 22:60276ba87ac6 | 137 | raw_f[i] = ps->GetRawPosition(); |
benkatz | 22:60276ba87ac6 | 138 | printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]); |
benkatz | 22:60276ba87ac6 | 139 | //theta_ref += delta; |
benkatz | 22:60276ba87ac6 | 140 | } |
benkatz | 26:2b865c00d7e9 | 141 | |
benkatz | 25:f5741040c4bb | 142 | for(int i = 0; i<n; i++){ // rotate backwards |
benkatz | 22:60276ba87ac6 | 143 | for(int j = 0; j<n2; j++){ |
benkatz | 22:60276ba87ac6 | 144 | theta_ref -= delta; |
benkatz | 25:f5741040c4bb | 145 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
benkatz | 25:f5741040c4bb | 146 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
benkatz | 27:501fee691e0e | 147 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); |
benkatz | 24:58c2d7571207 | 148 | if(PHASE_ORDER){ |
benkatz | 27:501fee691e0e | 149 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 150 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 151 | } |
benkatz | 22:60276ba87ac6 | 152 | else{ |
benkatz | 27:501fee691e0e | 153 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); |
benkatz | 27:501fee691e0e | 154 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 155 | } |
benkatz | 22:60276ba87ac6 | 156 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 157 | ps->Sample(); |
benkatz | 22:60276ba87ac6 | 158 | } |
benkatz | 25:f5741040c4bb | 159 | ps->Sample(); // sample position sensor |
benkatz | 38:67e4e1453a4b | 160 | theta_actual = ps->GetMechPositionFixed(); // get mechanical position |
benkatz | 22:60276ba87ac6 | 161 | error_b[i] = theta_ref/NPP - theta_actual; |
benkatz | 22:60276ba87ac6 | 162 | raw_b[i] = ps->GetRawPosition(); |
benkatz | 22:60276ba87ac6 | 163 | printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); |
benkatz | 22:60276ba87ac6 | 164 | //theta_ref -= delta; |
benkatz | 22:60276ba87ac6 | 165 | } |
benkatz | 22:60276ba87ac6 | 166 | |
benkatz | 22:60276ba87ac6 | 167 | float offset = 0; |
benkatz | 22:60276ba87ac6 | 168 | for(int i = 0; i<n; i++){ |
benkatz | 25:f5741040c4bb | 169 | offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset |
benkatz | 22:60276ba87ac6 | 170 | } |
benkatz | 25:f5741040c4bb | 171 | offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle |
benkatz | 23:2adf23ee0305 | 172 | |
benkatz | 22:60276ba87ac6 | 173 | |
benkatz | 25:f5741040c4bb | 174 | ps->SetElecOffset(offset); // Set position sensor offset |
benkatz | 23:2adf23ee0305 | 175 | __float_reg[0] = offset; |
benkatz | 23:2adf23ee0305 | 176 | E_OFFSET = offset; |
benkatz | 22:60276ba87ac6 | 177 | |
benkatz | 22:60276ba87ac6 | 178 | /// Perform filtering to linearize position sensor eccentricity |
benkatz | 22:60276ba87ac6 | 179 | /// FIR n-sample average, where n = number of samples in one electrical cycle |
benkatz | 22:60276ba87ac6 | 180 | /// This filter has zero gain at electrical frequency and all integer multiples |
benkatz | 25:f5741040c4bb | 181 | /// So cogging effects should be completely filtered out. |
benkatz | 22:60276ba87ac6 | 182 | |
benkatz | 22:60276ba87ac6 | 183 | float error[n] = {0}; |
benkatz | 23:2adf23ee0305 | 184 | const int window = 128; |
benkatz | 22:60276ba87ac6 | 185 | float error_filt[n] = {0}; |
benkatz | 23:2adf23ee0305 | 186 | float cogging_current[window] = {0}; |
benkatz | 22:60276ba87ac6 | 187 | float mean = 0; |
benkatz | 25:f5741040c4bb | 188 | for (int i = 0; i<n; i++){ //Average the forward and back directions |
benkatz | 22:60276ba87ac6 | 189 | error[i] = 0.5f*(error_f[i] + error_b[n-i-1]); |
benkatz | 22:60276ba87ac6 | 190 | } |
benkatz | 22:60276ba87ac6 | 191 | for (int i = 0; i<n; i++){ |
benkatz | 22:60276ba87ac6 | 192 | for(int j = 0; j<window; j++){ |
benkatz | 25:f5741040c4bb | 193 | int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 |
benkatz | 22:60276ba87ac6 | 194 | if(ind<0){ |
benkatz | 25:f5741040c4bb | 195 | ind += n;} // Moving average wraps around |
benkatz | 22:60276ba87ac6 | 196 | else if(ind > n-1) { |
benkatz | 22:60276ba87ac6 | 197 | ind -= n;} |
benkatz | 22:60276ba87ac6 | 198 | error_filt[i] += error[ind]/(float)window; |
benkatz | 22:60276ba87ac6 | 199 | } |
benkatz | 23:2adf23ee0305 | 200 | if(i<window){ |
benkatz | 23:2adf23ee0305 | 201 | cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP); |
benkatz | 23:2adf23ee0305 | 202 | } |
benkatz | 22:60276ba87ac6 | 203 | //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); |
benkatz | 22:60276ba87ac6 | 204 | mean += error_filt[i]/n; |
benkatz | 22:60276ba87ac6 | 205 | } |
benkatz | 25:f5741040c4bb | 206 | int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty |
benkatz | 28:8c7e29f719c5 | 207 | |
benkatz | 28:8c7e29f719c5 | 208 | |
benkatz | 23:2adf23ee0305 | 209 | printf("\n\r Encoder non-linearity compensation table\n\r"); |
benkatz | 23:2adf23ee0305 | 210 | printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r"); |
benkatz | 25:f5741040c4bb | 211 | for (int i = 0; i<n_lut; i++){ // build lookup table |
benkatz | 22:60276ba87ac6 | 212 | int ind = (raw_offset>>7) + i; |
benkatz | 22:60276ba87ac6 | 213 | if(ind > (n_lut-1)){ |
benkatz | 22:60276ba87ac6 | 214 | ind -= n_lut; |
benkatz | 22:60276ba87ac6 | 215 | } |
benkatz | 22:60276ba87ac6 | 216 | lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI)); |
benkatz | 23:2adf23ee0305 | 217 | printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]); |
benkatz | 26:2b865c00d7e9 | 218 | wait(.001); |
benkatz | 22:60276ba87ac6 | 219 | } |
benkatz | 23:2adf23ee0305 | 220 | |
benkatz | 25:f5741040c4bb | 221 | ps->WriteLUT(lut); // write lookup table to position sensor object |
benkatz | 23:2adf23ee0305 | 222 | //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... |
benkatz | 25:f5741040c4bb | 223 | memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array |
benkatz | 23:2adf23ee0305 | 224 | printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset); |
benkatz | 22:60276ba87ac6 | 225 | |
benkatz | 23:2adf23ee0305 | 226 | if (!prefs->ready()) prefs->open(); |
benkatz | 25:f5741040c4bb | 227 | prefs->flush(); // write offset and lookup table to flash |
benkatz | 25:f5741040c4bb | 228 | prefs->close(); |
benkatz | 23:2adf23ee0305 | 229 | |
benkatz | 23:2adf23ee0305 | 230 | |
benkatz | 22:60276ba87ac6 | 231 | } |