Shao Rui / Joint_control_2AS5147_DRV8323RH_GR49_HJB_2020_8_10

1

Dependencies:   mbed-dev-f303 FastPWM3

Calibration/calibration.cpp@53:32218a36df05, 2020-09-15 (annotated)

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