Shao Rui / Joint_control_2AS5147_DRV8323RH_HKC_FOC_2020_9_17

1

Dependencies:   mbed-dev-f303 FastPWM3

Calibration/calibration.cpp@55:489f01927025, 2021-05-12 (annotated)

Committer:
shaorui
Date:
Wed May 12 12:41:39 2021 +0000
Revision:
55:489f01927025
Parent:
54:4c9415402628 
1

Who changed what in which revision?

UserRevisionLine numberNew 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 44:efcde0af8390 9 #include "motor_config.h"
benkatz 44:efcde0af8390 10 #include "current_controller_config.h"
benkatz 22:60276ba87ac6 11
benkatz 23:2adf23ee0305 12 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
benkatz 28:8c7e29f719c5 13
benkatz 22:60276ba87ac6 14 ///Checks phase order, to ensure that positive Q current produces
benkatz 22:60276ba87ac6 15 ///torque in the positive direction wrt the position sensor.
benkatz 22:60276ba87ac6 16 printf("\n\r Checking phase ordering\n\r");
benkatz 22:60276ba87ac6 17 float theta_ref = 0;
benkatz 22:60276ba87ac6 18 float theta_actual = 0;
shaorui 48:1b51771c3647 19 //float v_d = .15f;
shaorui 55:489f01927025 20 //float v_d = .20f;
shaorui 55:489f01927025 21 float v_d = .40f; //Put all volts on the D-Axis
benkatz 44:efcde0af8390 22 float v_q = 0.0f;
benkatz 22:60276ba87ac6 23 float v_u, v_v, v_w = 0;
benkatz 44:efcde0af8390 24 float dtc_u, dtc_v, dtc_w = .5f;
benkatz 22:60276ba87ac6 25 int sample_counter = 0;
benkatz 22:60276ba87ac6 26
benkatz 22:60276ba87ac6 27 ///Set voltage angle to zero, wait for rotor position to settle
benkatz 25:f5741040c4bb 28 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages
Rushu 54:4c9415402628 29 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
benkatz 22:60276ba87ac6 30 for(int i = 0; i<20000; i++){
benkatz 27:501fee691e0e 31 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
benkatz 27:501fee691e0e 32 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 33 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 34 wait_us(100);
benkatz 22:60276ba87ac6 35 }
benkatz 22:60276ba87ac6 36 //ps->ZeroPosition();
benkatz 45:aadebe074af6 37 ps->Sample(DT);
benkatz 23:2adf23ee0305 38 wait_us(1000);
benkatz 38:67e4e1453a4b 39 //float theta_start = ps->GetMechPositionFixed(); //get initial rotor position
benkatz 26:2b865c00d7e9 40 float theta_start;
benkatz 23:2adf23ee0305 41 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
benkatz 23:2adf23ee0305 42 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
benkatz 23:2adf23ee0305 43 controller->i_a = -controller->i_b - controller->i_c;
benkatz 23:2adf23ee0305 44 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 45 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
benkatz 23:2adf23ee0305 46 printf("\n\rCurrent\n\r");
benkatz 23:2adf23ee0305 47 printf("%f %f %f\n\r\n\r", controller->i_d, controller->i_q, current);
benkatz 22:60276ba87ac6 48 /// Rotate voltage angle
benkatz 25:f5741040c4bb 49 while(theta_ref < 4*PI){ //rotate for 2 electrical cycles
benkatz 25:f5741040c4bb 50 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages
Rushu 54:4c9415402628 51 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
benkatz 22:60276ba87ac6 52 wait_us(100);
benkatz 27:501fee691e0e 53 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); //Set duty cycles
benkatz 27:501fee691e0e 54 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 55 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 45:aadebe074af6 56 ps->Sample(DT); //sample position sensor
benkatz 38:67e4e1453a4b 57 theta_actual = ps->GetMechPositionFixed();
benkatz 26:2b865c00d7e9 58 if(theta_ref==0){theta_start = theta_actual;}
benkatz 22:60276ba87ac6 59 if(sample_counter > 200){
benkatz 22:60276ba87ac6 60 sample_counter = 0 ;
benkatz 22:60276ba87ac6 61 printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual);
benkatz 22:60276ba87ac6 62 }
benkatz 22:60276ba87ac6 63 sample_counter++;
benkatz 22:60276ba87ac6 64 theta_ref += 0.001f;
benkatz 22:60276ba87ac6 65 }
benkatz 38:67e4e1453a4b 66 float theta_end = ps->GetMechPositionFixed();
Rushu 49:7eac11914980 67 int direction = 1;//(theta_end - theta_start)>0;
benkatz 22:60276ba87ac6 68 printf("Theta Start: %f Theta End: %f\n\r", theta_start, theta_end);
benkatz 22:60276ba87ac6 69 printf("Direction: %d\n\r", direction);
benkatz 23:2adf23ee0305 70 if(direction){printf("Phasing correct\n\r");}
benkatz 22:60276ba87ac6 71 else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");}
benkatz 23:2adf23ee0305 72 PHASE_ORDER = direction;
benkatz 22:60276ba87ac6 73 }
shaorui 48:1b51771c3647 74
shaorui 48:1b51771c3647 75
benkatz 23:2adf23ee0305 76 void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
benkatz 22:60276ba87ac6 77 /// Measures the electrical angle offset of the position sensor
Rushu 51:29e1686e8b3e 78 /// and (in the future) corrects nonlinearity due to position sensor eccentricity
benkatz 22:60276ba87ac6 79 printf("Starting calibration procedure\n\r");
benkatz 22:60276ba87ac6 80
benkatz 25:f5741040c4bb 81 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 82 const int n2 = 50; // increments between saved samples (for smoothing motion)
benkatz 25:f5741040c4bb 83 float delta = 2*PI*NPP/(n*n2); // change in angle between samples
benkatz 25:f5741040c4bb 84 float error_f[n] = {0}; // error vector rotating forwards
benkatz 25:f5741040c4bb 85 float error_b[n] = {0}; // error vector rotating backwards
benkatz 28:8c7e29f719c5 86 const int n_lut = 128;
benkatz 28:8c7e29f719c5 87 int lut[n_lut]= {0}; // clear any old lookup table before starting.
benkatz 28:8c7e29f719c5 88 ps->WriteLUT(lut);
benkatz 22:60276ba87ac6 89 int raw_f[n] = {0};
benkatz 22:60276ba87ac6 90 int raw_b[n] = {0};
benkatz 22:60276ba87ac6 91 float theta_ref = 0;
benkatz 22:60276ba87ac6 92 float theta_actual = 0;
shaorui 48:1b51771c3647 93 //float v_d = .15f;
shaorui 55:489f01927025 94 // float v_d = .20f;
shaorui 55:489f01927025 95 float v_d = .40f; // Put volts on the D-Axis
benkatz 44:efcde0af8390 96 float v_q = 0.0f;
benkatz 22:60276ba87ac6 97 float v_u, v_v, v_w = 0;
benkatz 44:efcde0af8390 98 float dtc_u, dtc_v, dtc_w = .5f;
benkatz 22:60276ba87ac6 99
benkatz 22:60276ba87ac6 100
benkatz 22:60276ba87ac6 101 ///Set voltage angle to zero, wait for rotor position to settle
benkatz 25:f5741040c4bb 102 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 103 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
benkatz 22:60276ba87ac6 104 for(int i = 0; i<40000; i++){
benkatz 27:501fee691e0e 105 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
benkatz 24:58c2d7571207 106 if(PHASE_ORDER){
benkatz 27:501fee691e0e 107 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 108 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 109 }
benkatz 22:60276ba87ac6 110 else{
benkatz 27:501fee691e0e 111 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 112 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 113 }
benkatz 22:60276ba87ac6 114 wait_us(100);
benkatz 22:60276ba87ac6 115 }
benkatz 45:aadebe074af6 116 ps->Sample(DT);
benkatz 23:2adf23ee0305 117 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
benkatz 23:2adf23ee0305 118 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
benkatz 23:2adf23ee0305 119 controller->i_a = -controller->i_b - controller->i_c;
benkatz 23:2adf23ee0305 120 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 121 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
benkatz 26:2b865c00d7e9 122 printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
benkatz 25:f5741040c4bb 123 for(int i = 0; i<n; i++){ // rotate forwards
benkatz 22:60276ba87ac6 124 for(int j = 0; j<n2; j++){
benkatz 22:60276ba87ac6 125 theta_ref += delta;
benkatz 25:f5741040c4bb 126 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 127 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
benkatz 27:501fee691e0e 128 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
benkatz 25:f5741040c4bb 129 if(PHASE_ORDER){ // Check phase ordering
benkatz 27:501fee691e0e 130 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
benkatz 27:501fee691e0e 131 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 132 }
benkatz 22:60276ba87ac6 133 else{
benkatz 27:501fee691e0e 134 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 135 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 136 }
benkatz 22:60276ba87ac6 137 wait_us(100);
benkatz 45:aadebe074af6 138 ps->Sample(DT);
benkatz 22:60276ba87ac6 139 }
benkatz 45:aadebe074af6 140 ps->Sample(DT);
benkatz 38:67e4e1453a4b 141 theta_actual = ps->GetMechPositionFixed();
benkatz 22:60276ba87ac6 142 error_f[i] = theta_ref/NPP - theta_actual;
benkatz 22:60276ba87ac6 143 raw_f[i] = ps->GetRawPosition();
Rushu 51:29e1686e8b3e 144 printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);// theta_ref/(NPP)=2pi/(128*21)=0.0023374944940476
Rushu 49:7eac11914980 145
Rushu 49:7eac11914980 146 // printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
benkatz 22:60276ba87ac6 147 //theta_ref += delta;
benkatz 22:60276ba87ac6 148 }
benkatz 26:2b865c00d7e9 149
benkatz 25:f5741040c4bb 150 for(int i = 0; i<n; i++){ // rotate backwards
benkatz 22:60276ba87ac6 151 for(int j = 0; j<n2; j++){
benkatz 22:60276ba87ac6 152 theta_ref -= delta;
benkatz 25:f5741040c4bb 153 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 154 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
benkatz 27:501fee691e0e 155 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
benkatz 24:58c2d7571207 156 if(PHASE_ORDER){
benkatz 27:501fee691e0e 157 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 158 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 159 }
benkatz 22:60276ba87ac6 160 else{
benkatz 27:501fee691e0e 161 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
benkatz 27:501fee691e0e 162 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
benkatz 22:60276ba87ac6 163 }
benkatz 22:60276ba87ac6 164 wait_us(100);
benkatz 45:aadebe074af6 165 ps->Sample(DT);
benkatz 22:60276ba87ac6 166 }
benkatz 45:aadebe074af6 167 ps->Sample(DT); // sample position sensor
benkatz 38:67e4e1453a4b 168 theta_actual = ps->GetMechPositionFixed(); // get mechanical position
benkatz 22:60276ba87ac6 169 error_b[i] = theta_ref/NPP - theta_actual;
benkatz 22:60276ba87ac6 170 raw_b[i] = ps->GetRawPosition();
Rushu 49:7eac11914980 171 printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
Rushu 49:7eac11914980 172
Rushu 49:7eac11914980 173 //printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
benkatz 22:60276ba87ac6 174 //theta_ref -= delta;
benkatz 22:60276ba87ac6 175 }
benkatz 22:60276ba87ac6 176
benkatz 22:60276ba87ac6 177 float offset = 0;
benkatz 22:60276ba87ac6 178 for(int i = 0; i<n; i++){
benkatz 25:f5741040c4bb 179 offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset
benkatz 22:60276ba87ac6 180 }
benkatz 25:f5741040c4bb 181 offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle
benkatz 23:2adf23ee0305 182
benkatz 22:60276ba87ac6 183
benkatz 25:f5741040c4bb 184 ps->SetElecOffset(offset); // Set position sensor offset
benkatz 23:2adf23ee0305 185 __float_reg[0] = offset;
benkatz 23:2adf23ee0305 186 E_OFFSET = offset;
benkatz 22:60276ba87ac6 187
benkatz 22:60276ba87ac6 188 /// Perform filtering to linearize position sensor eccentricity
benkatz 22:60276ba87ac6 189 /// FIR n-sample average, where n = number of samples in one electrical cycle
benkatz 22:60276ba87ac6 190 /// This filter has zero gain at electrical frequency and all integer multiples
benkatz 25:f5741040c4bb 191 /// So cogging effects should be completely filtered out.
benkatz 22:60276ba87ac6 192
benkatz 22:60276ba87ac6 193 float error[n] = {0};
benkatz 23:2adf23ee0305 194 const int window = 128;
benkatz 22:60276ba87ac6 195 float error_filt[n] = {0};
benkatz 23:2adf23ee0305 196 float cogging_current[window] = {0};
benkatz 22:60276ba87ac6 197 float mean = 0;
benkatz 25:f5741040c4bb 198 for (int i = 0; i<n; i++){ //Average the forward and back directions
benkatz 22:60276ba87ac6 199 error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
benkatz 22:60276ba87ac6 200 }
benkatz 22:60276ba87ac6 201 for (int i = 0; i<n; i++){
benkatz 22:60276ba87ac6 202 for(int j = 0; j<window; j++){
benkatz 25:f5741040c4bb 203 int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
benkatz 22:60276ba87ac6 204 if(ind<0){
benkatz 25:f5741040c4bb 205 ind += n;} // Moving average wraps around
benkatz 22:60276ba87ac6 206 else if(ind > n-1) {
benkatz 22:60276ba87ac6 207 ind -= n;}
benkatz 22:60276ba87ac6 208 error_filt[i] += error[ind]/(float)window;
benkatz 22:60276ba87ac6 209 }
benkatz 23:2adf23ee0305 210 if(i<window){
benkatz 23:2adf23ee0305 211 cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
benkatz 23:2adf23ee0305 212 }
benkatz 22:60276ba87ac6 213 //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
benkatz 22:60276ba87ac6 214 mean += error_filt[i]/n;
benkatz 22:60276ba87ac6 215 }
benkatz 25:f5741040c4bb 216 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 217
benkatz 28:8c7e29f719c5 218
benkatz 23:2adf23ee0305 219 printf("\n\r Encoder non-linearity compensation table\n\r");
benkatz 23:2adf23ee0305 220 printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
benkatz 25:f5741040c4bb 221 for (int i = 0; i<n_lut; i++){ // build lookup table
benkatz 22:60276ba87ac6 222 int ind = (raw_offset>>7) + i;
benkatz 22:60276ba87ac6 223 if(ind > (n_lut-1)){
benkatz 22:60276ba87ac6 224 ind -= n_lut;
benkatz 22:60276ba87ac6 225 }
benkatz 22:60276ba87ac6 226 lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
benkatz 23:2adf23ee0305 227 printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]);
benkatz 26:2b865c00d7e9 228 wait(.001);
benkatz 22:60276ba87ac6 229 }
benkatz 23:2adf23ee0305 230
benkatz 25:f5741040c4bb 231 ps->WriteLUT(lut); // write lookup table to position sensor object
benkatz 23:2adf23ee0305 232 //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
Rushu 52:d4d5e3414865 233 memcpy(&ENCODER_LUT, lut, 128*4); // copy the lookup table to the flash array
benkatz 23:2adf23ee0305 234 printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset);
benkatz 22:60276ba87ac6 235
benkatz 23:2adf23ee0305 236 if (!prefs->ready()) prefs->open();
benkatz 25:f5741040c4bb 237 prefs->flush(); // write offset and lookup table to flash
benkatz 25:f5741040c4bb 238 prefs->close();
benkatz 23:2adf23ee0305 239
benkatz 23:2adf23ee0305 240
shaorui 48:1b51771c3647 241 }
shaorui 48:1b51771c3647 242
shaorui 48:1b51771c3647 243 /*
shaorui 48:1b51771c3647 244 void j_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
shaorui 48:1b51771c3647 245 {
shaorui 48:1b51771c3647 246 /// Measures the electrical angle offset of the position sensor
shaorui 48:1b51771c3647 247 /// and (in the future) corrects nonlinearity due to position sensor eccentricity
shaorui 48:1b51771c3647 248 //printf("Starting calibration procedure\n\r");
shaorui 48:1b51771c3647 249 printf("s\n\r");
shaorui 48:1b51771c3647 250
shaorui 48:1b51771c3647 251 const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
shaorui 48:1b51771c3647 252 const int n2 = 50; // increments between saved samples (for smoothing motion)
shaorui 48:1b51771c3647 253 float delta = 2*PI*NPP/(n*n2); // change in angle between samples
shaorui 48:1b51771c3647 254 float error_f[n] = {0}; // error vector rotating forwards
shaorui 48:1b51771c3647 255 float error_b[n] = {0}; // error vector rotating backwards
shaorui 48:1b51771c3647 256 float error_joint_f[n] = {0}; // error vector rotating forwards
shaorui 48:1b51771c3647 257 float error_joint_b[n] = {0}; // error vector rotating backwards
shaorui 48:1b51771c3647 258
shaorui 48:1b51771c3647 259 const int n_lut = 128;
shaorui 48:1b51771c3647 260 const int n_joint = 128;
shaorui 48:1b51771c3647 261 int lut[n_lut]= {0}; // clear any old lookup table before starting.
shaorui 48:1b51771c3647 262 int joint[n_joint]= {0}; // clear any old lookup table before starting.
shaorui 48:1b51771c3647 263 ps->WriteLUT(lut);
shaorui 48:1b51771c3647 264 jps->WriteLUT(joint);
shaorui 48:1b51771c3647 265 int raw_f[n] = {0};
shaorui 48:1b51771c3647 266 int raw_b[n] = {0};
shaorui 48:1b51771c3647 267 int joint_raw_f[n] = {0};
shaorui 48:1b51771c3647 268 int joint_raw_b[n] = {0};
shaorui 48:1b51771c3647 269
shaorui 48:1b51771c3647 270 float theta_ref = 0;
shaorui 48:1b51771c3647 271 float theta_actual = 0;
shaorui 48:1b51771c3647 272 float theta_joint_ref = 0;
shaorui 48:1b51771c3647 273 float theta_joint_actual = 0;
shaorui 48:1b51771c3647 274
shaorui 48:1b51771c3647 275 //float v_d = .15f;
shaorui 48:1b51771c3647 276 float v_d = .2f; // Put volts on the D-Axis
shaorui 48:1b51771c3647 277 float v_q = 0.0f;
shaorui 48:1b51771c3647 278 float v_u, v_v, v_w = 0;
shaorui 48:1b51771c3647 279 float dtc_u, dtc_v, dtc_w = .5f;
shaorui 48:1b51771c3647 280
shaorui 48:1b51771c3647 281
shaorui 48:1b51771c3647 282 ///Set voltage angle to zero, wait for rotor position to settle
shaorui 48:1b51771c3647 283 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 284 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
shaorui 48:1b51771c3647 285 for(int i = 0; i<40000; i++){
shaorui 48:1b51771c3647 286 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
shaorui 48:1b51771c3647 287 if(PHASE_ORDER){
shaorui 48:1b51771c3647 288 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 289 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 290 }
shaorui 48:1b51771c3647 291 else{
shaorui 48:1b51771c3647 292 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 293 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 294 }
shaorui 48:1b51771c3647 295 wait_us(100);
shaorui 48:1b51771c3647 296 }
shaorui 48:1b51771c3647 297 ps->Sample(DT);
shaorui 48:1b51771c3647 298 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
shaorui 48:1b51771c3647 299 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
shaorui 48:1b51771c3647 300 controller->i_a = -controller->i_b - controller->i_c;
shaorui 48:1b51771c3647 301 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
shaorui 48:1b51771c3647 302 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
shaorui 48:1b51771c3647 303 printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
shaorui 48:1b51771c3647 304 for(int i = 0; i<n; i++){ // rotate forwards
shaorui 48:1b51771c3647 305 for(int j = 0; j<n2; j++){
shaorui 48:1b51771c3647 306 theta_ref += delta;
shaorui 48:1b51771c3647 307 //theta_joint_ref=theta_joint_ref- delta-theta_ref/GR;
shaorui 48:1b51771c3647 308 theta_joint_ref-=delta;
shaorui 48:1b51771c3647 309 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 310 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
shaorui 48:1b51771c3647 311 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
shaorui 48:1b51771c3647 312 if(PHASE_ORDER){ // Check phase ordering
shaorui 48:1b51771c3647 313 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
shaorui 48:1b51771c3647 314 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 315 }
shaorui 48:1b51771c3647 316 else{
shaorui 48:1b51771c3647 317 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 318 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 319 }
shaorui 48:1b51771c3647 320 wait_us(100);
shaorui 48:1b51771c3647 321 ps->Sample(DT);
shaorui 48:1b51771c3647 322 jps->Sample(DT);
shaorui 48:1b51771c3647 323 }
shaorui 48:1b51771c3647 324 ps->Sample(DT);
shaorui 48:1b51771c3647 325 jps->Sample(DT);
shaorui 48:1b51771c3647 326 theta_actual = ps->GetMechPositionFixed();
shaorui 48:1b51771c3647 327 error_f[i] = theta_ref/NPP - theta_actual;
shaorui 48:1b51771c3647 328 raw_f[i] = ps->GetRawPosition();
shaorui 48:1b51771c3647 329 theta_joint_actual = jps->GetMechPositionFixed()+(1/GR)*theta_actual;
shaorui 48:1b51771c3647 330 error_joint_f[i] = theta_joint_ref/NPP - theta_joint_actual;
shaorui 48:1b51771c3647 331 joint_raw_f[i] = jps->GetRawPosition();
shaorui 48:1b51771c3647 332
shaorui 48:1b51771c3647 333 printf("%.4f %.4f %d-------%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i],theta_joint_ref/(NPP), theta_joint_actual, joint_raw_f[i]);
shaorui 48:1b51771c3647 334
shaorui 48:1b51771c3647 335 //theta_ref += delta;
shaorui 48:1b51771c3647 336 }
shaorui 48:1b51771c3647 337
shaorui 48:1b51771c3647 338 for(int i = 0; i<n; i++){ // rotate backwards
shaorui 48:1b51771c3647 339 for(int j = 0; j<n2; j++){
shaorui 48:1b51771c3647 340 theta_ref -= delta;
shaorui 48:1b51771c3647 341 //theta_joint_ref=theta_joint_ref+delta+theta_ref/GR;
shaorui 48:1b51771c3647 342 theta_joint_ref+=delta;
shaorui 48:1b51771c3647 343 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 344 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
shaorui 48:1b51771c3647 345 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
shaorui 48:1b51771c3647 346 if(PHASE_ORDER){
shaorui 48:1b51771c3647 347 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 348 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 349 }
shaorui 48:1b51771c3647 350 else{
shaorui 48:1b51771c3647 351 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 352 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 353 }
shaorui 48:1b51771c3647 354 wait_us(100);
shaorui 48:1b51771c3647 355 ps->Sample(DT);
shaorui 48:1b51771c3647 356 jps->Sample(DT);
shaorui 48:1b51771c3647 357 }
shaorui 48:1b51771c3647 358 ps->Sample(DT); // sample position sensor
shaorui 48:1b51771c3647 359 jps->Sample(DT);
shaorui 48:1b51771c3647 360 theta_actual = ps->GetMechPositionFixed(); // get mechanical position
shaorui 48:1b51771c3647 361 error_b[i] = theta_ref/NPP - theta_actual;
shaorui 48:1b51771c3647 362 raw_b[i] = ps->GetRawPosition();
shaorui 48:1b51771c3647 363 theta_joint_actual = jps->GetMechPositionFixed()+(1/GR)*theta_actual;
shaorui 48:1b51771c3647 364 error_joint_b[i] = theta_joint_ref/NPP - theta_joint_actual;
shaorui 48:1b51771c3647 365 joint_raw_b[i] = jps->GetRawPosition()-raw_b[i]/GR;
shaorui 48:1b51771c3647 366
shaorui 48:1b51771c3647 367 //printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
shaorui 48:1b51771c3647 368 printf("%.4f %.4f %d-------%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i],theta_joint_ref/(NPP), theta_joint_actual, joint_raw_b[i]);
shaorui 48:1b51771c3647 369 //theta_ref -= delta;
shaorui 48:1b51771c3647 370 }
shaorui 48:1b51771c3647 371
shaorui 48:1b51771c3647 372 float offset = 0;
shaorui 48:1b51771c3647 373 float joint_offset=0;
shaorui 48:1b51771c3647 374 for(int i = 0; i<n; i++){
shaorui 48:1b51771c3647 375 offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset
shaorui 48:1b51771c3647 376 joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n);
shaorui 48:1b51771c3647 377 }
shaorui 48:1b51771c3647 378 offset = fmod(offset*NPP, 2*PI);
shaorui 48:1b51771c3647 379 joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle
shaorui 48:1b51771c3647 380
shaorui 48:1b51771c3647 381
shaorui 48:1b51771c3647 382 ps->SetElecOffset(offset); // Set position sensor offset
shaorui 48:1b51771c3647 383 __float_reg[0] = offset;
shaorui 48:1b51771c3647 384 E_OFFSET = offset;
shaorui 48:1b51771c3647 385
shaorui 48:1b51771c3647 386 jps->SetElecOffset(joint_offset); // Set position sensor offset
shaorui 48:1b51771c3647 387 __float_reg[8] = joint_offset;
shaorui 48:1b51771c3647 388 JOINT_E_OFFSET = joint_offset;
shaorui 48:1b51771c3647 389
shaorui 48:1b51771c3647 390 /// Perform filtering to linearize position sensor eccentricity
shaorui 48:1b51771c3647 391 /// FIR n-sample average, where n = number of samples in one electrical cycle
shaorui 48:1b51771c3647 392 /// This filter has zero gain at electrical frequency and all integer multiples
shaorui 48:1b51771c3647 393 /// So cogging effects should be completely filtered out.
shaorui 48:1b51771c3647 394
shaorui 48:1b51771c3647 395 float error[n] = {0};
shaorui 48:1b51771c3647 396 float joint_error[n]={0};
shaorui 48:1b51771c3647 397 const int window = 128;
shaorui 48:1b51771c3647 398 float error_filt[n] = {0};
shaorui 48:1b51771c3647 399 float error_joint_filt[n] = {0};
shaorui 48:1b51771c3647 400 float cogging_current[window] = {0};
shaorui 48:1b51771c3647 401 float cogging_joint_current[window] = {0};
shaorui 48:1b51771c3647 402 float mean = 0;
shaorui 48:1b51771c3647 403 float joint_mean=0;
shaorui 48:1b51771c3647 404 for (int i = 0; i<n; i++){ //Average the forward and back directions
shaorui 48:1b51771c3647 405 error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
shaorui 48:1b51771c3647 406 joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]);
shaorui 48:1b51771c3647 407 }
shaorui 48:1b51771c3647 408 for (int i = 0; i<n; i++){
shaorui 48:1b51771c3647 409 for(int j = 0; j<window; j++){
shaorui 48:1b51771c3647 410 int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
shaorui 48:1b51771c3647 411 if(ind<0){
shaorui 48:1b51771c3647 412 ind += n;
shaorui 48:1b51771c3647 413 } // Moving average wraps around
shaorui 48:1b51771c3647 414 else if(ind > n-1) {
shaorui 48:1b51771c3647 415 ind -= n;
shaorui 48:1b51771c3647 416 }
shaorui 48:1b51771c3647 417 error_filt[i] += error[ind]/(float)window;
shaorui 48:1b51771c3647 418 error_joint_filt[i] += joint_error[ind]/(float)window;
shaorui 48:1b51771c3647 419 }
shaorui 48:1b51771c3647 420 if(i<window){
shaorui 48:1b51771c3647 421 cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
shaorui 48:1b51771c3647 422 cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP);
shaorui 48:1b51771c3647 423 }
shaorui 48:1b51771c3647 424 //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
shaorui 48:1b51771c3647 425 mean += error_filt[i]/n;
shaorui 48:1b51771c3647 426 joint_mean += error_joint_filt[i]/n;
shaorui 48:1b51771c3647 427 }
shaorui 48:1b51771c3647 428 int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
shaorui 48:1b51771c3647 429 int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2;
shaorui 48:1b51771c3647 430
shaorui 48:1b51771c3647 431
shaorui 48:1b51771c3647 432 printf("\n\r Encoder non-linearity compensation table\n\r");
shaorui 48:1b51771c3647 433 printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
shaorui 48:1b51771c3647 434 for (int i = 0; i<n_lut; i++){ // build lookup table
shaorui 48:1b51771c3647 435 int ind = (raw_offset>>7) + i;
shaorui 48:1b51771c3647 436 if(ind > (n_lut-1)){
shaorui 48:1b51771c3647 437 ind -= n_lut;
shaorui 48:1b51771c3647 438 }
shaorui 48:1b51771c3647 439 lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
shaorui 48:1b51771c3647 440 printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]);
shaorui 48:1b51771c3647 441 wait(.001);
shaorui 48:1b51771c3647 442 }
shaorui 48:1b51771c3647 443
shaorui 48:1b51771c3647 444 for (int i = 0; i<n_joint; i++){ // build lookup table
shaorui 48:1b51771c3647 445 int joint_ind = (joint_raw_offset>>7) + i;
shaorui 48:1b51771c3647 446 if(joint_ind > (n_joint-1)){
shaorui 48:1b51771c3647 447 joint_ind -= n_joint;
shaorui 48:1b51771c3647 448 }
shaorui 48:1b51771c3647 449 joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI));
shaorui 48:1b51771c3647 450 printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]);
shaorui 48:1b51771c3647 451 wait(.001);
shaorui 48:1b51771c3647 452 }
shaorui 48:1b51771c3647 453
shaorui 48:1b51771c3647 454
shaorui 48:1b51771c3647 455 ps->WriteLUT(lut);
shaorui 48:1b51771c3647 456 jps->WriteLUT(joint); // write lookup table to position sensor object
shaorui 48:1b51771c3647 457 //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
shaorui 48:1b51771c3647 458 memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array
shaorui 48:1b51771c3647 459 memcpy(&ENCODER_JOINT, joint, sizeof(joint));
shaorui 48:1b51771c3647 460
shaorui 48:1b51771c3647 461 printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset);
shaorui 48:1b51771c3647 462 printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset);
shaorui 48:1b51771c3647 463
shaorui 48:1b51771c3647 464 if (!prefs->ready()) prefs->open();
shaorui 48:1b51771c3647 465 prefs->flush(); // write offset and lookup table to flash
shaorui 48:1b51771c3647 466 prefs->close();
shaorui 48:1b51771c3647 467
shaorui 48:1b51771c3647 468 }
shaorui 48:1b51771c3647 469 */
shaorui 48:1b51771c3647 470
shaorui 48:1b51771c3647 471
shaorui 48:1b51771c3647 472 void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
shaorui 48:1b51771c3647 473 {
shaorui 48:1b51771c3647 474 /// Measures the electrical angle offset of the position sensor
shaorui 48:1b51771c3647 475 /// and (in the future) corrects nonlinearity due to position sensor eccentricity
shaorui 48:1b51771c3647 476 printf("Starting joint calibration procedure\n\r");
shaorui 48:1b51771c3647 477
shaorui 48:1b51771c3647 478 const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
shaorui 48:1b51771c3647 479 const int n2 = 50; // increments between saved samples (for smoothing motion)
shaorui 48:1b51771c3647 480 float delta = 2*PI*NPP/(n*n2); // change in angle between samples
shaorui 48:1b51771c3647 481 float error_joint_f[n] = {0}; // error vector rotating forwards
shaorui 48:1b51771c3647 482 float error_joint_b[n] = {0}; // error vector rotating backwards
shaorui 48:1b51771c3647 483
shaorui 48:1b51771c3647 484 const int n_joint = 128;
shaorui 48:1b51771c3647 485 int joint[n_joint]= {0}; // clear any old lookup table before starting.
shaorui 48:1b51771c3647 486 jps->WriteLUT(joint);
shaorui 48:1b51771c3647 487
shaorui 48:1b51771c3647 488
Rushu 49:7eac11914980 489 // jps->ReadLUT();
shaorui 48:1b51771c3647 490
shaorui 48:1b51771c3647 491 int joint_raw_f[n] = {0};
shaorui 48:1b51771c3647 492 int joint_raw_b[n] = {0};
shaorui 48:1b51771c3647 493
shaorui 48:1b51771c3647 494 float theta_ref = 0;
shaorui 48:1b51771c3647 495 float theta_joint_ref = 0;
shaorui 48:1b51771c3647 496 float theta_joint_actual = 0;
shaorui 48:1b51771c3647 497
shaorui 48:1b51771c3647 498 //float v_d = .15f;
shaorui 55:489f01927025 499 //float v_d = .18f;
shaorui 55:489f01927025 500 float v_d = .60f; // Put volts on the D-Axis
shaorui 48:1b51771c3647 501 float v_q = 0.0f;
shaorui 48:1b51771c3647 502 float v_u, v_v, v_w = 0;
shaorui 48:1b51771c3647 503 float dtc_u, dtc_v, dtc_w = .5f;
Rushu 49:7eac11914980 504 PHASE_ORDER =1; //!!!hjb added, direction always run error, so pre to 1 in hardware UVW.
shaorui 48:1b51771c3647 505
shaorui 48:1b51771c3647 506 ///Set voltage angle to zero, wait for rotor position to settle
shaorui 48:1b51771c3647 507 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 508 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
shaorui 48:1b51771c3647 509 for(int i = 0; i<40000; i++){
shaorui 48:1b51771c3647 510 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
shaorui 48:1b51771c3647 511 if(PHASE_ORDER){
shaorui 48:1b51771c3647 512 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 513 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 514 }
shaorui 48:1b51771c3647 515 else{
shaorui 48:1b51771c3647 516 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 517 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 518 }
shaorui 48:1b51771c3647 519 wait_us(100);
shaorui 48:1b51771c3647 520 }
shaorui 48:1b51771c3647 521 jps->Sample(DT);
shaorui 48:1b51771c3647 522 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
shaorui 48:1b51771c3647 523 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
shaorui 48:1b51771c3647 524 controller->i_a = -controller->i_b - controller->i_c;
shaorui 48:1b51771c3647 525 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
shaorui 48:1b51771c3647 526 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
shaorui 48:1b51771c3647 527 printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
shaorui 48:1b51771c3647 528 for(int i = 0; i<n; i++){ // rotate forwards
shaorui 48:1b51771c3647 529 for(int j = 0; j<n2; j++){
shaorui 48:1b51771c3647 530 theta_ref += delta;
shaorui 48:1b51771c3647 531 theta_joint_ref-=delta;
shaorui 48:1b51771c3647 532
shaorui 48:1b51771c3647 533 // theta_joint_ref+=delta;
shaorui 48:1b51771c3647 534 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 535 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
shaorui 48:1b51771c3647 536 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
shaorui 48:1b51771c3647 537 if(PHASE_ORDER){ // Check phase ordering
shaorui 48:1b51771c3647 538 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
shaorui 48:1b51771c3647 539 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 540 }
shaorui 48:1b51771c3647 541 else{
shaorui 48:1b51771c3647 542 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 543 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 544 }
shaorui 48:1b51771c3647 545 wait_us(100);
shaorui 48:1b51771c3647 546 jps->Sample(DT);
shaorui 48:1b51771c3647 547 }
shaorui 48:1b51771c3647 548 jps->Sample(DT);
shaorui 48:1b51771c3647 549
shaorui 48:1b51771c3647 550 theta_joint_actual = jps->GetMechPositionFixed();
shaorui 48:1b51771c3647 551 //error_joint_f[i] = theta_joint_ref*(1+1/GR)/NPP -theta_joint_actual;
shaorui 48:1b51771c3647 552 error_joint_f[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
shaorui 48:1b51771c3647 553 joint_raw_f[i] = jps->GetRawPosition();
shaorui 48:1b51771c3647 554
shaorui 48:1b51771c3647 555 //printf("%.4f %.4f %d\n\r", theta_joint_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_f[i]);
Rushu 49:7eac11914980 556 printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_f[i]);
shaorui 48:1b51771c3647 557
shaorui 48:1b51771c3647 558 //theta_ref += delta;
shaorui 48:1b51771c3647 559 }
shaorui 48:1b51771c3647 560
shaorui 48:1b51771c3647 561 for(int i = 0; i<n; i++){ // rotate backwards
shaorui 48:1b51771c3647 562 for(int j = 0; j<n2; j++){
shaorui 48:1b51771c3647 563 theta_ref -= delta;
shaorui 48:1b51771c3647 564 //theta_joint_ref-=delta;
shaorui 48:1b51771c3647 565 theta_joint_ref+=delta;
shaorui 48:1b51771c3647 566 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 567 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
shaorui 48:1b51771c3647 568 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
shaorui 48:1b51771c3647 569 if(PHASE_ORDER){
shaorui 48:1b51771c3647 570 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 571 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 572 }
shaorui 48:1b51771c3647 573 else{
shaorui 48:1b51771c3647 574 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 575 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 576 }
shaorui 48:1b51771c3647 577 wait_us(100);
shaorui 48:1b51771c3647 578 jps->Sample(DT);
shaorui 48:1b51771c3647 579 }
shaorui 48:1b51771c3647 580
shaorui 48:1b51771c3647 581 jps->Sample(DT);
shaorui 48:1b51771c3647 582
shaorui 48:1b51771c3647 583 theta_joint_actual = jps->GetMechPositionFixed();
shaorui 48:1b51771c3647 584 //error_joint_b[i] = theta_joint_ref*(1+1/GR)/NPP - theta_joint_actual;
shaorui 48:1b51771c3647 585 error_joint_b[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
shaorui 48:1b51771c3647 586 joint_raw_b[i] = jps->GetRawPosition();//-raw_b[i]/GR;
shaorui 48:1b51771c3647 587
shaorui 48:1b51771c3647 588
shaorui 48:1b51771c3647 589 //printf("%.4f %.4f %d-\n\r", theta_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_b[i]);
Rushu 49:7eac11914980 590 printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_b[i]);
shaorui 48:1b51771c3647 591 //theta_ref -= delta;
shaorui 48:1b51771c3647 592 }
shaorui 48:1b51771c3647 593
shaorui 48:1b51771c3647 594
shaorui 48:1b51771c3647 595 float joint_offset=0;
shaorui 48:1b51771c3647 596 for(int i = 0; i<n; i++){
shaorui 48:1b51771c3647 597 joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); // calclate average position sensor joint offset
shaorui 48:1b51771c3647 598 }
shaorui 48:1b51771c3647 599 joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle
shaorui 48:1b51771c3647 600
shaorui 48:1b51771c3647 601
shaorui 48:1b51771c3647 602
shaorui 48:1b51771c3647 603
shaorui 48:1b51771c3647 604 jps->SetElecOffset(joint_offset); // Set position joint sensor offset
shaorui 48:1b51771c3647 605 __float_reg[8] = joint_offset;
shaorui 48:1b51771c3647 606 JOINT_E_OFFSET = joint_offset;
shaorui 48:1b51771c3647 607
shaorui 48:1b51771c3647 608 /// Perform filtering to linearize position sensor eccentricity
shaorui 48:1b51771c3647 609 /// FIR n-sample average, where n = number of samples in one electrical cycle
shaorui 48:1b51771c3647 610 /// This filter has zero gain at electrical frequency and all integer multiples
shaorui 48:1b51771c3647 611 /// So cogging effects should be completely filtered out.
shaorui 48:1b51771c3647 612
shaorui 48:1b51771c3647 613
shaorui 48:1b51771c3647 614 float joint_error[n]={0};
shaorui 48:1b51771c3647 615 const int window = 128;
shaorui 48:1b51771c3647 616
shaorui 48:1b51771c3647 617 float error_joint_filt[n] = {0};
shaorui 48:1b51771c3647 618
shaorui 48:1b51771c3647 619 float cogging_joint_current[window] = {0};
shaorui 48:1b51771c3647 620
shaorui 48:1b51771c3647 621 float joint_mean=0;
shaorui 48:1b51771c3647 622 for (int i = 0; i<n; i++){ //Average the forward and back directions
shaorui 48:1b51771c3647 623
shaorui 48:1b51771c3647 624 joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]);
shaorui 48:1b51771c3647 625 }
shaorui 48:1b51771c3647 626 for (int i = 0; i<n; i++){
shaorui 48:1b51771c3647 627 for(int j = 0; j<window; j++){
shaorui 48:1b51771c3647 628 int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
shaorui 48:1b51771c3647 629 if(ind<0){
shaorui 48:1b51771c3647 630 ind += n;
shaorui 48:1b51771c3647 631 } // Moving average wraps around
shaorui 48:1b51771c3647 632 else if(ind > n-1) {
shaorui 48:1b51771c3647 633 ind -= n;
shaorui 48:1b51771c3647 634 }
shaorui 48:1b51771c3647 635
shaorui 48:1b51771c3647 636 error_joint_filt[i] += joint_error[ind]/(float)window;
shaorui 48:1b51771c3647 637 }
shaorui 48:1b51771c3647 638 if(i<window){
shaorui 48:1b51771c3647 639
shaorui 48:1b51771c3647 640 cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP);
shaorui 48:1b51771c3647 641 }
shaorui 48:1b51771c3647 642 //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
shaorui 48:1b51771c3647 643
shaorui 48:1b51771c3647 644 joint_mean += error_joint_filt[i]/n;
shaorui 48:1b51771c3647 645 }
shaorui 48:1b51771c3647 646
shaorui 48:1b51771c3647 647 int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
shaorui 48:1b51771c3647 648
shaorui 48:1b51771c3647 649
shaorui 48:1b51771c3647 650 printf("\n\r Encoder non-linearity compensation table\n\r");
shaorui 48:1b51771c3647 651 printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
shaorui 48:1b51771c3647 652
shaorui 48:1b51771c3647 653 for (int i = 0; i<n_joint; i++){ // build lookup table
shaorui 48:1b51771c3647 654 int joint_ind = (joint_raw_offset>>7) + i;
shaorui 48:1b51771c3647 655 if(joint_ind > (n_joint-1)){
shaorui 48:1b51771c3647 656 joint_ind -= n_joint;
shaorui 48:1b51771c3647 657 }
shaorui 48:1b51771c3647 658 joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI));
shaorui 48:1b51771c3647 659 printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]);
shaorui 48:1b51771c3647 660 wait(.001);
shaorui 48:1b51771c3647 661 }
shaorui 48:1b51771c3647 662
shaorui 48:1b51771c3647 663
shaorui 48:1b51771c3647 664
shaorui 48:1b51771c3647 665 jps->WriteLUT(joint); // write lookup table to position sensor object
shaorui 48:1b51771c3647 666 //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
shaorui 48:1b51771c3647 667
shaorui 48:1b51771c3647 668 memcpy(&ENCODER_JOINT, joint, sizeof(joint));
shaorui 48:1b51771c3647 669
shaorui 48:1b51771c3647 670 printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset);
shaorui 48:1b51771c3647 671
shaorui 48:1b51771c3647 672 if (!prefs->ready()) prefs->open();
shaorui 48:1b51771c3647 673 prefs->flush(); // write offset and lookup table to flash
shaorui 48:1b51771c3647 674 prefs->close();
shaorui 48:1b51771c3647 675
shaorui 48:1b51771c3647 676 }
shaorui 48:1b51771c3647 677
shaorui 48:1b51771c3647 678
shaorui 48:1b51771c3647 679 /*
shaorui 48:1b51771c3647 680
shaorui 48:1b51771c3647 681 void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
shaorui 48:1b51771c3647 682 {
shaorui 48:1b51771c3647 683 /// Measures the electrical angle offset of the position sensor
shaorui 48:1b51771c3647 684 /// and (in the future) corrects nonlinearity due to position sensor eccentricity
shaorui 48:1b51771c3647 685 printf("Starting joint calibration procedure\n\r");
shaorui 48:1b51771c3647 686
shaorui 48:1b51771c3647 687 const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later)
shaorui 48:1b51771c3647 688 const int n2 = 50; // increments between saved samples (for smoothing motion)
shaorui 48:1b51771c3647 689 float delta = 2*PI*NPP/(n*n2); // change in angle between samples
shaorui 48:1b51771c3647 690 float error_joint_f[n] = {0}; // error vector rotating forwards
shaorui 48:1b51771c3647 691 float error_joint_b[n] = {0}; // error vector rotating backwards
shaorui 48:1b51771c3647 692
shaorui 48:1b51771c3647 693 const int n_joint = 128;
shaorui 48:1b51771c3647 694 int joint[n_joint]= {0}; // clear any old lookup table before starting.
shaorui 48:1b51771c3647 695 jps->WriteLUT(joint);
shaorui 48:1b51771c3647 696
shaorui 48:1b51771c3647 697 int joint_raw_f[n] = {0};
shaorui 48:1b51771c3647 698 int joint_raw_b[n] = {0};
shaorui 48:1b51771c3647 699
shaorui 48:1b51771c3647 700 float theta_ref = 0;
shaorui 48:1b51771c3647 701 float theta_joint_ref = 0;
shaorui 48:1b51771c3647 702 float theta_joint_actual = 0;
shaorui 48:1b51771c3647 703
shaorui 48:1b51771c3647 704 //float v_d = .15f;
shaorui 48:1b51771c3647 705 float v_d = .2f; // Put volts on the D-Axis
shaorui 48:1b51771c3647 706 float v_q = 0.0f;
shaorui 48:1b51771c3647 707 float v_u, v_v, v_w = 0;
shaorui 48:1b51771c3647 708 float dtc_u, dtc_v, dtc_w = .5f;
shaorui 48:1b51771c3647 709
shaorui 48:1b51771c3647 710
shaorui 48:1b51771c3647 711 ///Set voltage angle to zero, wait for rotor position to settle
shaorui 48:1b51771c3647 712 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 713 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
shaorui 48:1b51771c3647 714 for(int i = 0; i<40000; i++){
shaorui 48:1b51771c3647 715 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles
shaorui 48:1b51771c3647 716 if(PHASE_ORDER){
shaorui 48:1b51771c3647 717 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 718 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 719 }
shaorui 48:1b51771c3647 720 else{
shaorui 48:1b51771c3647 721 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 722 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 723 }
shaorui 48:1b51771c3647 724 wait_us(100);
shaorui 48:1b51771c3647 725 }
shaorui 48:1b51771c3647 726 jps->Sample(DT);
shaorui 48:1b51771c3647 727 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
shaorui 48:1b51771c3647 728 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
shaorui 48:1b51771c3647 729 controller->i_a = -controller->i_b - controller->i_c;
shaorui 48:1b51771c3647 730 dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
shaorui 48:1b51771c3647 731 float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
shaorui 48:1b51771c3647 732 printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
shaorui 48:1b51771c3647 733 for(int i = 0; i<n; i++){ // rotate forwards
shaorui 48:1b51771c3647 734 for(int j = 0; j<n2; j++){
shaorui 48:1b51771c3647 735 //theta_ref += delta;
shaorui 48:1b51771c3647 736 //theta_joint_ref+=delta;
shaorui 48:1b51771c3647 737
shaorui 48:1b51771c3647 738 theta_joint_ref+=delta;
shaorui 48:1b51771c3647 739 abc(theta_joint_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 740 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
shaorui 48:1b51771c3647 741 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
shaorui 48:1b51771c3647 742 if(PHASE_ORDER){ // Check phase ordering
shaorui 48:1b51771c3647 743 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles
shaorui 48:1b51771c3647 744 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 745 }
shaorui 48:1b51771c3647 746 else{
shaorui 48:1b51771c3647 747 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 748 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 749 }
shaorui 48:1b51771c3647 750 wait_us(100);
shaorui 48:1b51771c3647 751 jps->Sample(DT);
shaorui 48:1b51771c3647 752 }
shaorui 48:1b51771c3647 753 jps->Sample(DT);
shaorui 48:1b51771c3647 754
shaorui 48:1b51771c3647 755 theta_joint_actual = jps->GetMechPositionFixed();
shaorui 48:1b51771c3647 756 error_joint_f[i] = -theta_joint_ref*(1+1/GR)/NPP -theta_joint_actual;
shaorui 48:1b51771c3647 757 // error_joint_f[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
shaorui 48:1b51771c3647 758 joint_raw_f[i] = jps->GetRawPosition();
shaorui 48:1b51771c3647 759
shaorui 48:1b51771c3647 760 printf("%.4f %.4f %d\n\r", theta_joint_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_f[i]);
shaorui 48:1b51771c3647 761 // printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_f[i]);
shaorui 48:1b51771c3647 762
shaorui 48:1b51771c3647 763 //theta_ref += delta;
shaorui 48:1b51771c3647 764 }
shaorui 48:1b51771c3647 765
shaorui 48:1b51771c3647 766 for(int i = 0; i<n; i++){ // rotate backwards
shaorui 48:1b51771c3647 767 for(int j = 0; j<n2; j++){
shaorui 48:1b51771c3647 768 //theta_ref -= delta;
shaorui 48:1b51771c3647 769 theta_joint_ref-=delta;
shaorui 48:1b51771c3647 770 //theta_joint_ref+=delta;
shaorui 48:1b51771c3647 771 abc( theta_joint_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages
Rushu 54:4c9415402628 772 svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
shaorui 48:1b51771c3647 773 TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
shaorui 48:1b51771c3647 774 if(PHASE_ORDER){
shaorui 48:1b51771c3647 775 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 776 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 777 }
shaorui 48:1b51771c3647 778 else{
shaorui 48:1b51771c3647 779 TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
shaorui 48:1b51771c3647 780 TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
shaorui 48:1b51771c3647 781 }
shaorui 48:1b51771c3647 782 wait_us(100);
shaorui 48:1b51771c3647 783 jps->Sample(DT);
shaorui 48:1b51771c3647 784 }
shaorui 48:1b51771c3647 785
shaorui 48:1b51771c3647 786 jps->Sample(DT);
shaorui 48:1b51771c3647 787
shaorui 48:1b51771c3647 788 theta_joint_actual = jps->GetMechPositionFixed();
shaorui 48:1b51771c3647 789 error_joint_b[i] = -theta_joint_ref*(1+1/GR)/NPP - theta_joint_actual;
shaorui 48:1b51771c3647 790 //error_joint_b[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual;
shaorui 48:1b51771c3647 791 joint_raw_b[i] = jps->GetRawPosition();//-raw_b[i]/GR;
shaorui 48:1b51771c3647 792
shaorui 48:1b51771c3647 793
shaorui 48:1b51771c3647 794 printf("%.4f %.4f %d-\n\r", theta_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_b[i]);
shaorui 48:1b51771c3647 795 // printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_b[i]);
shaorui 48:1b51771c3647 796 //theta_ref -= delta;
shaorui 48:1b51771c3647 797 }
shaorui 48:1b51771c3647 798
shaorui 48:1b51771c3647 799
shaorui 48:1b51771c3647 800 float joint_offset=0;
shaorui 48:1b51771c3647 801 for(int i = 0; i<n; i++){
shaorui 48:1b51771c3647 802 joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); // calclate average position sensor joint offset
shaorui 48:1b51771c3647 803 }
shaorui 48:1b51771c3647 804 joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle
shaorui 48:1b51771c3647 805
shaorui 48:1b51771c3647 806
shaorui 48:1b51771c3647 807
shaorui 48:1b51771c3647 808
shaorui 48:1b51771c3647 809 jps->SetElecOffset(joint_offset); // Set position joint sensor offset
shaorui 48:1b51771c3647 810 __float_reg[8] = joint_offset;
shaorui 48:1b51771c3647 811 JOINT_E_OFFSET = joint_offset;
shaorui 48:1b51771c3647 812
shaorui 48:1b51771c3647 813 /// Perform filtering to linearize position sensor eccentricity
shaorui 48:1b51771c3647 814 /// FIR n-sample average, where n = number of samples in one electrical cycle
shaorui 48:1b51771c3647 815 /// This filter has zero gain at electrical frequency and all integer multiples
shaorui 48:1b51771c3647 816 /// So cogging effects should be completely filtered out.
shaorui 48:1b51771c3647 817
shaorui 48:1b51771c3647 818
shaorui 48:1b51771c3647 819 float joint_error[n]={0};
shaorui 48:1b51771c3647 820 const int window = 128;
shaorui 48:1b51771c3647 821
shaorui 48:1b51771c3647 822 float error_joint_filt[n] = {0};
shaorui 48:1b51771c3647 823
shaorui 48:1b51771c3647 824 float cogging_joint_current[window] = {0};
shaorui 48:1b51771c3647 825
shaorui 48:1b51771c3647 826 float joint_mean=0;
shaorui 48:1b51771c3647 827 for (int i = 0; i<n; i++){ //Average the forward and back directions
shaorui 48:1b51771c3647 828
shaorui 48:1b51771c3647 829 joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]);
shaorui 48:1b51771c3647 830 }
shaorui 48:1b51771c3647 831 for (int i = 0; i<n; i++){
shaorui 48:1b51771c3647 832 for(int j = 0; j<window; j++){
shaorui 48:1b51771c3647 833 int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2
shaorui 48:1b51771c3647 834 if(ind<0){
shaorui 48:1b51771c3647 835 ind += n;
shaorui 48:1b51771c3647 836 } // Moving average wraps around
shaorui 48:1b51771c3647 837 else if(ind > n-1) {
shaorui 48:1b51771c3647 838 ind -= n;
shaorui 48:1b51771c3647 839 }
shaorui 48:1b51771c3647 840
shaorui 48:1b51771c3647 841 error_joint_filt[i] += joint_error[ind]/(float)window;
shaorui 48:1b51771c3647 842 }
shaorui 48:1b51771c3647 843 if(i<window){
shaorui 48:1b51771c3647 844
shaorui 48:1b51771c3647 845 cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP);
shaorui 48:1b51771c3647 846 }
shaorui 48:1b51771c3647 847 //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
shaorui 48:1b51771c3647 848
shaorui 48:1b51771c3647 849 joint_mean += error_joint_filt[i]/n;
shaorui 48:1b51771c3647 850 }
shaorui 48:1b51771c3647 851
shaorui 48:1b51771c3647 852 int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty
shaorui 48:1b51771c3647 853
shaorui 48:1b51771c3647 854
shaorui 48:1b51771c3647 855 printf("\n\r Encoder non-linearity compensation table\n\r");
shaorui 48:1b51771c3647 856 printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
shaorui 48:1b51771c3647 857
shaorui 48:1b51771c3647 858 for (int i = 0; i<n_joint; i++){ // build lookup table
shaorui 48:1b51771c3647 859 int joint_ind = (joint_raw_offset>>7) + i;
shaorui 48:1b51771c3647 860 if(joint_ind > (n_joint-1)){
shaorui 48:1b51771c3647 861 joint_ind -= n_joint;
shaorui 48:1b51771c3647 862 }
shaorui 48:1b51771c3647 863 joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI));
shaorui 48:1b51771c3647 864 printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]);
shaorui 48:1b51771c3647 865 wait(.001);
shaorui 48:1b51771c3647 866 }
shaorui 48:1b51771c3647 867
shaorui 48:1b51771c3647 868
shaorui 48:1b51771c3647 869
shaorui 48:1b51771c3647 870 jps->WriteLUT(joint); // write lookup table to position sensor object
shaorui 48:1b51771c3647 871 //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet....
shaorui 48:1b51771c3647 872
shaorui 48:1b51771c3647 873 memcpy(&ENCODER_JOINT, joint, sizeof(joint));
shaorui 48:1b51771c3647 874
shaorui 48:1b51771c3647 875 printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset);
shaorui 48:1b51771c3647 876
shaorui 48:1b51771c3647 877 if (!prefs->ready()) prefs->open();
shaorui 48:1b51771c3647 878 prefs->flush(); // write offset and lookup table to flash
shaorui 48:1b51771c3647 879 prefs->close();
shaorui 48:1b51771c3647 880
shaorui 48:1b51771c3647 881 }*/