Shao Rui / RV_brake_2021_3_19

1

Dependencies:   mbed-dev-f303 FastPWM3

Calibration/calibration.cpp@48:1b51771c3647, 2020-02-07 (annotated)

Committer:
shaorui
Date:
Fri Feb 07 11:31:37 2020 +0000
Revision:
48:1b51771c3647
Parent:
45:aadebe074af6
Child:
49:7eac11914980
test;

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