WANG YUCHAO
/
Joint_control_2AS5147_DRV8323RH_HKC_FOC__RV
this motor config,npp=21,GR= 151
Diff: Calibration/calibration.cpp
- Revision:
- 48:1b51771c3647
- Parent:
- 45:aadebe074af6
- Child:
- 49:7eac11914980
--- a/Calibration/calibration.cpp Sat Dec 07 08:01:06 2019 +0000 +++ b/Calibration/calibration.cpp Fri Feb 07 11:31:37 2020 +0000 @@ -16,7 +16,8 @@ printf("\n\r Checking phase ordering\n\r"); float theta_ref = 0; float theta_actual = 0; - float v_d = .15f; //Put all volts on the D-Axis + //float v_d = .15f; + float v_d = .23f; //Put all volts on the D-Axis float v_q = 0.0f; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5f; @@ -69,8 +70,8 @@ else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");} PHASE_ORDER = direction; } - - + + void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ /// Measures the electrical angle offset of the position sensor /// and (in the future) corrects nonlinearity due to position sensor eccentricity @@ -88,7 +89,8 @@ int raw_b[n] = {0}; float theta_ref = 0; float theta_actual = 0; - float v_d = .15f; // Put volts on the D-Axis + //float v_d = .15f; + float v_d = .2f; // Put volts on the D-Axis float v_q = 0.0f; float v_u, v_v, v_w = 0; float dtc_u, dtc_v, dtc_w = .5f; @@ -137,7 +139,7 @@ theta_actual = ps->GetMechPositionFixed(); error_f[i] = theta_ref/NPP - theta_actual; raw_f[i] = ps->GetRawPosition(); - printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]); + printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]); //theta_ref += delta; } @@ -162,7 +164,7 @@ theta_actual = ps->GetMechPositionFixed(); // get mechanical position error_b[i] = theta_ref/NPP - theta_actual; raw_b[i] = ps->GetRawPosition(); - printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); + printf("ref %.4f\n\r actual %.4f\n\r raw %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); //theta_ref -= delta; } @@ -230,4 +232,643 @@ prefs->close(); - } \ No newline at end of file + } + + /* +void j_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs) +{ + /// Measures the electrical angle offset of the position sensor + /// and (in the future) corrects nonlinearity due to position sensor eccentricity + //printf("Starting calibration procedure\n\r"); + printf("s\n\r"); + + const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2 = 50; // increments between saved samples (for smoothing motion) + float delta = 2*PI*NPP/(n*n2); // change in angle between samples + float error_f[n] = {0}; // error vector rotating forwards + float error_b[n] = {0}; // error vector rotating backwards + float error_joint_f[n] = {0}; // error vector rotating forwards + float error_joint_b[n] = {0}; // error vector rotating backwards + + const int n_lut = 128; + const int n_joint = 128; + int lut[n_lut]= {0}; // clear any old lookup table before starting. + int joint[n_joint]= {0}; // clear any old lookup table before starting. + ps->WriteLUT(lut); + jps->WriteLUT(joint); + int raw_f[n] = {0}; + int raw_b[n] = {0}; + int joint_raw_f[n] = {0}; + int joint_raw_b[n] = {0}; + + float theta_ref = 0; + float theta_actual = 0; + float theta_joint_ref = 0; + float theta_joint_actual = 0; + + //float v_d = .15f; + float v_d = .2f; // Put volts on the D-Axis + float v_q = 0.0f; + float v_u, v_v, v_w = 0; + float dtc_u, dtc_v, dtc_w = .5f; + + + ///Set voltage angle to zero, wait for rotor position to settle + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + for(int i = 0; i<40000; i++){ + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles + if(PHASE_ORDER){ + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + } + ps->Sample(DT); + controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings + controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); + controller->i_a = -controller->i_b - controller->i_c; + dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents + float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); + printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); + for(int i = 0; i<n; i++){ // rotate forwards + for(int j = 0; j<n2; j++){ + theta_ref += delta; + //theta_joint_ref=theta_joint_ref- delta-theta_ref/GR; + theta_joint_ref-=delta; + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); + if(PHASE_ORDER){ // Check phase ordering + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + ps->Sample(DT); + jps->Sample(DT); + } + ps->Sample(DT); + jps->Sample(DT); + theta_actual = ps->GetMechPositionFixed(); + error_f[i] = theta_ref/NPP - theta_actual; + raw_f[i] = ps->GetRawPosition(); + theta_joint_actual = jps->GetMechPositionFixed()+(1/GR)*theta_actual; + error_joint_f[i] = theta_joint_ref/NPP - theta_joint_actual; + joint_raw_f[i] = jps->GetRawPosition(); + + 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]); + + //theta_ref += delta; + } + + for(int i = 0; i<n; i++){ // rotate backwards + for(int j = 0; j<n2; j++){ + theta_ref -= delta; + //theta_joint_ref=theta_joint_ref+delta+theta_ref/GR; + theta_joint_ref+=delta; + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); + if(PHASE_ORDER){ + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + ps->Sample(DT); + jps->Sample(DT); + } + ps->Sample(DT); // sample position sensor + jps->Sample(DT); + theta_actual = ps->GetMechPositionFixed(); // get mechanical position + error_b[i] = theta_ref/NPP - theta_actual; + raw_b[i] = ps->GetRawPosition(); + theta_joint_actual = jps->GetMechPositionFixed()+(1/GR)*theta_actual; + error_joint_b[i] = theta_joint_ref/NPP - theta_joint_actual; + joint_raw_b[i] = jps->GetRawPosition()-raw_b[i]/GR; + + //printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); + 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]); + //theta_ref -= delta; + } + + float offset = 0; + float joint_offset=0; + for(int i = 0; i<n; i++){ + offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset + joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); + } + offset = fmod(offset*NPP, 2*PI); + joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle + + + ps->SetElecOffset(offset); // Set position sensor offset + __float_reg[0] = offset; + E_OFFSET = offset; + + jps->SetElecOffset(joint_offset); // Set position sensor offset + __float_reg[8] = joint_offset; + JOINT_E_OFFSET = joint_offset; + + /// Perform filtering to linearize position sensor eccentricity + /// FIR n-sample average, where n = number of samples in one electrical cycle + /// This filter has zero gain at electrical frequency and all integer multiples + /// So cogging effects should be completely filtered out. + + float error[n] = {0}; + float joint_error[n]={0}; + const int window = 128; + float error_filt[n] = {0}; + float error_joint_filt[n] = {0}; + float cogging_current[window] = {0}; + float cogging_joint_current[window] = {0}; + float mean = 0; + float joint_mean=0; + for (int i = 0; i<n; i++){ //Average the forward and back directions + error[i] = 0.5f*(error_f[i] + error_b[n-i-1]); + joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]); + } + for (int i = 0; i<n; i++){ + for(int j = 0; j<window; j++){ + int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 + if(ind<0){ + ind += n; + } // Moving average wraps around + else if(ind > n-1) { + ind -= n; + } + error_filt[i] += error[ind]/(float)window; + error_joint_filt[i] += joint_error[ind]/(float)window; + } + if(i<window){ + cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP); + cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP); + } + //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); + mean += error_filt[i]/n; + joint_mean += error_joint_filt[i]/n; + } + int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty + int joint_raw_offset =(joint_raw_f[0] + joint_raw_b[n-1])/2; + + + printf("\n\r Encoder non-linearity compensation table\n\r"); + printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r"); + for (int i = 0; i<n_lut; i++){ // build lookup table + int ind = (raw_offset>>7) + i; + if(ind > (n_lut-1)){ + ind -= n_lut; + } + lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI)); + printf("%d %d %d %f\n\r", i, ind, lut[ind], cogging_current[i]); + wait(.001); + } + + for (int i = 0; i<n_joint; i++){ // build lookup table + int joint_ind = (joint_raw_offset>>7) + i; + if(joint_ind > (n_joint-1)){ + joint_ind -= n_joint; + } + joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI)); + printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]); + wait(.001); + } + + + ps->WriteLUT(lut); + jps->WriteLUT(joint); // write lookup table to position sensor object + //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... + memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array + memcpy(&ENCODER_JOINT, joint, sizeof(joint)); + + printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset); + printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset); + + if (!prefs->ready()) prefs->open(); + prefs->flush(); // write offset and lookup table to flash + prefs->close(); + +} +*/ + + +void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs) +{ + /// Measures the electrical angle offset of the position sensor + /// and (in the future) corrects nonlinearity due to position sensor eccentricity + printf("Starting joint calibration procedure\n\r"); + + const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2 = 50; // increments between saved samples (for smoothing motion) + float delta = 2*PI*NPP/(n*n2); // change in angle between samples + float error_joint_f[n] = {0}; // error vector rotating forwards + float error_joint_b[n] = {0}; // error vector rotating backwards + + const int n_joint = 128; + int joint[n_joint]= {0}; // clear any old lookup table before starting. + jps->WriteLUT(joint); + + + jps->ReadLUT(); + + int joint_raw_f[n] = {0}; + int joint_raw_b[n] = {0}; + + float theta_ref = 0; + float theta_joint_ref = 0; + float theta_joint_actual = 0; + + //float v_d = .15f; + float v_d = .2f; // Put volts on the D-Axis + float v_q = 0.0f; + float v_u, v_v, v_w = 0; + float dtc_u, dtc_v, dtc_w = .5f; + + + ///Set voltage angle to zero, wait for rotor position to settle + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + for(int i = 0; i<40000; i++){ + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles + if(PHASE_ORDER){ + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + } + jps->Sample(DT); + controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings + controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); + controller->i_a = -controller->i_b - controller->i_c; + dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents + float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); + printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); + for(int i = 0; i<n; i++){ // rotate forwards + for(int j = 0; j<n2; j++){ + theta_ref += delta; + theta_joint_ref-=delta; + + // theta_joint_ref+=delta; + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); + if(PHASE_ORDER){ // Check phase ordering + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + jps->Sample(DT); + } + jps->Sample(DT); + + theta_joint_actual = jps->GetMechPositionFixed(); + //error_joint_f[i] = theta_joint_ref*(1+1/GR)/NPP -theta_joint_actual; + error_joint_f[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual; + joint_raw_f[i] = jps->GetRawPosition(); + + //printf("%.4f %.4f %d\n\r", theta_joint_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_f[i]); + 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]); + + //theta_ref += delta; + } + + for(int i = 0; i<n; i++){ // rotate backwards + for(int j = 0; j<n2; j++){ + theta_ref -= delta; + //theta_joint_ref-=delta; + theta_joint_ref+=delta; + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); + if(PHASE_ORDER){ + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + jps->Sample(DT); + } + + jps->Sample(DT); + + theta_joint_actual = jps->GetMechPositionFixed(); + //error_joint_b[i] = theta_joint_ref*(1+1/GR)/NPP - theta_joint_actual; + error_joint_b[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual; + joint_raw_b[i] = jps->GetRawPosition();//-raw_b[i]/GR; + + + //printf("%.4f %.4f %d-\n\r", theta_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_b[i]); + 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]); + //theta_ref -= delta; + } + + + float joint_offset=0; + for(int i = 0; i<n; i++){ + joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); // calclate average position sensor joint offset + } + joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle + + + + + jps->SetElecOffset(joint_offset); // Set position joint sensor offset + __float_reg[8] = joint_offset; + JOINT_E_OFFSET = joint_offset; + + /// Perform filtering to linearize position sensor eccentricity + /// FIR n-sample average, where n = number of samples in one electrical cycle + /// This filter has zero gain at electrical frequency and all integer multiples + /// So cogging effects should be completely filtered out. + + + float joint_error[n]={0}; + const int window = 128; + + float error_joint_filt[n] = {0}; + + float cogging_joint_current[window] = {0}; + + float joint_mean=0; + for (int i = 0; i<n; i++){ //Average the forward and back directions + + joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]); + } + for (int i = 0; i<n; i++){ + for(int j = 0; j<window; j++){ + int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 + if(ind<0){ + ind += n; + } // Moving average wraps around + else if(ind > n-1) { + ind -= n; + } + + error_joint_filt[i] += joint_error[ind]/(float)window; + } + if(i<window){ + + cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP); + } + //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); + + joint_mean += error_joint_filt[i]/n; + } + + 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 + + + printf("\n\r Encoder non-linearity compensation table\n\r"); + printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r"); + + for (int i = 0; i<n_joint; i++){ // build lookup table + int joint_ind = (joint_raw_offset>>7) + i; + if(joint_ind > (n_joint-1)){ + joint_ind -= n_joint; + } + joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI)); + printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]); + wait(.001); + } + + + + jps->WriteLUT(joint); // write lookup table to position sensor object + //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... + + memcpy(&ENCODER_JOINT, joint, sizeof(joint)); + + printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset); + + if (!prefs->ready()) prefs->open(); + prefs->flush(); // write offset and lookup table to flash + prefs->close(); + +} + + +/* + +void j_calibrate(PositionSensorMA700 *jps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs) +{ + /// Measures the electrical angle offset of the position sensor + /// and (in the future) corrects nonlinearity due to position sensor eccentricity + printf("Starting joint calibration procedure\n\r"); + + const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2 = 50; // increments between saved samples (for smoothing motion) + float delta = 2*PI*NPP/(n*n2); // change in angle between samples + float error_joint_f[n] = {0}; // error vector rotating forwards + float error_joint_b[n] = {0}; // error vector rotating backwards + + const int n_joint = 128; + int joint[n_joint]= {0}; // clear any old lookup table before starting. + jps->WriteLUT(joint); + + int joint_raw_f[n] = {0}; + int joint_raw_b[n] = {0}; + + float theta_ref = 0; + float theta_joint_ref = 0; + float theta_joint_actual = 0; + + //float v_d = .15f; + float v_d = .2f; // Put volts on the D-Axis + float v_q = 0.0f; + float v_u, v_v, v_w = 0; + float dtc_u, dtc_v, dtc_w = .5f; + + + ///Set voltage angle to zero, wait for rotor position to settle + abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + for(int i = 0; i<40000; i++){ + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles + if(PHASE_ORDER){ + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + } + jps->Sample(DT); + controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings + controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); + controller->i_a = -controller->i_b - controller->i_c; + dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents + float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); + printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); + for(int i = 0; i<n; i++){ // rotate forwards + for(int j = 0; j<n2; j++){ + //theta_ref += delta; + //theta_joint_ref+=delta; + + theta_joint_ref+=delta; + abc(theta_joint_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); + if(PHASE_ORDER){ // Check phase ordering + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + jps->Sample(DT); + } + jps->Sample(DT); + + theta_joint_actual = jps->GetMechPositionFixed(); + error_joint_f[i] = -theta_joint_ref*(1+1/GR)/NPP -theta_joint_actual; + // error_joint_f[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual; + joint_raw_f[i] = jps->GetRawPosition(); + + printf("%.4f %.4f %d\n\r", theta_joint_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_f[i]); + // printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_f[i]); + + //theta_ref += delta; + } + + for(int i = 0; i<n; i++){ // rotate backwards + for(int j = 0; j<n2; j++){ + //theta_ref -= delta; + theta_joint_ref-=delta; + //theta_joint_ref+=delta; + abc( theta_joint_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages + svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation + TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); + if(PHASE_ORDER){ + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + else{ + TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); + TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); + } + wait_us(100); + jps->Sample(DT); + } + + jps->Sample(DT); + + theta_joint_actual = jps->GetMechPositionFixed(); + error_joint_b[i] = -theta_joint_ref*(1+1/GR)/NPP - theta_joint_actual; + //error_joint_b[i] = (theta_joint_ref-(1/GR)*theta_ref)/NPP -theta_joint_actual; + joint_raw_b[i] = jps->GetRawPosition();//-raw_b[i]/GR; + + + printf("%.4f %.4f %d-\n\r", theta_ref*(1+1/GR)/(NPP), theta_joint_actual, joint_raw_b[i]); + // printf("%.4f %.4f %d\n\r", (theta_joint_ref-(1/GR)*theta_ref)/NPP, theta_joint_actual, joint_raw_b[i]); + //theta_ref -= delta; + } + + + float joint_offset=0; + for(int i = 0; i<n; i++){ + joint_offset += (error_joint_f[i] + error_joint_b[n-1-i])/(2.0f*n); // calclate average position sensor joint offset + } + joint_offset = fmod( joint_offset*NPP, 2*PI); // convert mechanical angle to electrical angle + + + + + jps->SetElecOffset(joint_offset); // Set position joint sensor offset + __float_reg[8] = joint_offset; + JOINT_E_OFFSET = joint_offset; + + /// Perform filtering to linearize position sensor eccentricity + /// FIR n-sample average, where n = number of samples in one electrical cycle + /// This filter has zero gain at electrical frequency and all integer multiples + /// So cogging effects should be completely filtered out. + + + float joint_error[n]={0}; + const int window = 128; + + float error_joint_filt[n] = {0}; + + float cogging_joint_current[window] = {0}; + + float joint_mean=0; + for (int i = 0; i<n; i++){ //Average the forward and back directions + + joint_error[i]= 0.5f*(error_joint_f[i] + error_joint_b[n-i-1]); + } + for (int i = 0; i<n; i++){ + for(int j = 0; j<window; j++){ + int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 + if(ind<0){ + ind += n; + } // Moving average wraps around + else if(ind > n-1) { + ind -= n; + } + + error_joint_filt[i] += joint_error[ind]/(float)window; + } + if(i<window){ + + cogging_joint_current[i] = current*sinf((joint_error[i] - error_joint_filt[i])*NPP); + } + //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); + + joint_mean += error_joint_filt[i]/n; + } + + 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 + + + printf("\n\r Encoder non-linearity compensation table\n\r"); + printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r"); + + for (int i = 0; i<n_joint; i++){ // build lookup table + int joint_ind = (joint_raw_offset>>7) + i; + if(joint_ind > (n_joint-1)){ + joint_ind -= n_joint; + } + joint[joint_ind] = (int) ((error_joint_filt[i*NPP] - joint_mean)*(float)(jps->GetCPR())/(2.0f*PI)); + printf("%d %d %d %f\n\r", i, joint_ind, joint[joint_ind], cogging_joint_current[i]); + wait(.001); + } + + + + jps->WriteLUT(joint); // write lookup table to position sensor object + //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... + + memcpy(&ENCODER_JOINT, joint, sizeof(joint)); + + printf("\n\rJoint Encoder Electrical Offset (rad) %f\n\r", joint_offset); + + if (!prefs->ready()) prefs->open(); + prefs->flush(); // write offset and lookup table to flash + prefs->close(); + +}*/ \ No newline at end of file