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