1
Dependencies: mbed-dev-f303 FastPWM3
Joint_Calibration/joint_calibration.cpp@49:7eac11914980, 2020-03-19 (annotated)
- Committer:
- Rushu
- Date:
- Thu Mar 19 03:48:24 2020 +0000
- Revision:
- 49:7eac11914980
- Parent:
- 48:1b51771c3647
- Child:
- 54:4c9415402628
1)use DRV8323RH; 2)SPI to Joint sensor; 3)motor and joint position is combined.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shaorui | 48:1b51771c3647 | 1 | /// Calibration procedures for determining position sensor offset, |
shaorui | 48:1b51771c3647 | 2 | /// phase ordering, and position sensor linearization |
shaorui | 48:1b51771c3647 | 3 | /// |
shaorui | 48:1b51771c3647 | 4 | |
shaorui | 48:1b51771c3647 | 5 | #include "joint_calibration.h" |
shaorui | 48:1b51771c3647 | 6 | #include "foc.h" |
shaorui | 48:1b51771c3647 | 7 | #include "PreferenceWriter.h" |
shaorui | 48:1b51771c3647 | 8 | #include "user_config.h" |
shaorui | 48:1b51771c3647 | 9 | #include "motor_config.h" |
shaorui | 48:1b51771c3647 | 10 | #include "current_controller_config.h" |
shaorui | 48:1b51771c3647 | 11 | #include "MA700Sensor.h" |
shaorui | 48:1b51771c3647 | 12 | |
shaorui | 48:1b51771c3647 | 13 | |
shaorui | 48:1b51771c3647 | 14 | void joint_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ |
shaorui | 48:1b51771c3647 | 15 | |
shaorui | 48:1b51771c3647 | 16 | |
shaorui | 48:1b51771c3647 | 17 | printf("Starting joint calibration procedure !\n\r"); |
shaorui | 48:1b51771c3647 | 18 | const int n = 120*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) |
shaorui | 48:1b51771c3647 | 19 | const int n2 = 50*GR; // increments between saved samples (for smoothing motion) |
shaorui | 48:1b51771c3647 | 20 | float delta = 2*PI*NPP*GR/(n*n2); // change in angle between samples |
shaorui | 48:1b51771c3647 | 21 | float error_f[n] = {0}; // error vector rotating forwards(error between motor and joint) |
shaorui | 48:1b51771c3647 | 22 | float error_b[n] = {0}; // error vector rotating backwards(error between motor and joint) |
shaorui | 48:1b51771c3647 | 23 | const int n_joint = 120; |
shaorui | 48:1b51771c3647 | 24 | int joint[n_joint]= {0}; // clear any old lookup table before starting. |
shaorui | 48:1b51771c3647 | 25 | jps->WriteLUT(joint); |
shaorui | 48:1b51771c3647 | 26 | int raw_f[n] = {0}; |
shaorui | 48:1b51771c3647 | 27 | int raw_b[n] = {0}; |
shaorui | 48:1b51771c3647 | 28 | float theta_ref = 0; |
shaorui | 48:1b51771c3647 | 29 | float theta_actual = 0; |
shaorui | 48:1b51771c3647 | 30 | float joint_theta_actual = 0; |
shaorui | 48:1b51771c3647 | 31 | //float v_d = .15f; |
Rushu | 49:7eac11914980 | 32 | float v_d = .15f; // Put volts on the D-Axis |
shaorui | 48:1b51771c3647 | 33 | float v_q = 0.0f; |
shaorui | 48:1b51771c3647 | 34 | float v_u, v_v, v_w = 0; |
shaorui | 48:1b51771c3647 | 35 | float dtc_u, dtc_v, dtc_w = .5f; |
shaorui | 48:1b51771c3647 | 36 | |
shaorui | 48:1b51771c3647 | 37 | |
shaorui | 48:1b51771c3647 | 38 | ///Set voltage angle to zero, wait for rotor position to settle |
shaorui | 48:1b51771c3647 | 39 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
shaorui | 48:1b51771c3647 | 40 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
shaorui | 48:1b51771c3647 | 41 | for(int i = 0; i<40000; i++){ |
shaorui | 48:1b51771c3647 | 42 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles |
shaorui | 48:1b51771c3647 | 43 | if(PHASE_ORDER){ |
shaorui | 48:1b51771c3647 | 44 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); |
shaorui | 48:1b51771c3647 | 45 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
shaorui | 48:1b51771c3647 | 46 | } |
shaorui | 48:1b51771c3647 | 47 | else{ |
shaorui | 48:1b51771c3647 | 48 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); |
shaorui | 48:1b51771c3647 | 49 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); |
shaorui | 48:1b51771c3647 | 50 | } |
shaorui | 48:1b51771c3647 | 51 | wait_us(100); |
shaorui | 48:1b51771c3647 | 52 | } |
shaorui | 48:1b51771c3647 | 53 | ps->Sample(DT); |
shaorui | 48:1b51771c3647 | 54 | jps->Sample(DT); |
shaorui | 48:1b51771c3647 | 55 | controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings |
shaorui | 48:1b51771c3647 | 56 | controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); |
shaorui | 48:1b51771c3647 | 57 | controller->i_a = -controller->i_b - controller->i_c; |
shaorui | 48:1b51771c3647 | 58 | 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 | 59 | float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2)); |
shaorui | 48:1b51771c3647 | 60 | printf(" Joint offset starting !\n\r\n\r"); |
shaorui | 48:1b51771c3647 | 61 | |
shaorui | 48:1b51771c3647 | 62 | /*************同时设置转子和关节零位置同步****************/ |
shaorui | 48:1b51771c3647 | 63 | ps->SetMechOffset(0); |
shaorui | 48:1b51771c3647 | 64 | jps->SetMechOffset(0); |
shaorui | 48:1b51771c3647 | 65 | ps->Sample(DT); |
shaorui | 48:1b51771c3647 | 66 | jps->Sample(DT); |
shaorui | 48:1b51771c3647 | 67 | wait_us(20); |
shaorui | 48:1b51771c3647 | 68 | M_OFFSET = ps->GetMechPosition(); |
shaorui | 48:1b51771c3647 | 69 | JOINT_M_OFFSET =jps->GetMechPosition(); |
shaorui | 48:1b51771c3647 | 70 | if (!prefs->ready()) prefs->open(); |
shaorui | 48:1b51771c3647 | 71 | prefs->flush(); // Write new prefs to flash |
shaorui | 48:1b51771c3647 | 72 | prefs->close(); |
shaorui | 48:1b51771c3647 | 73 | prefs->load(); |
shaorui | 48:1b51771c3647 | 74 | ps->SetMechOffset(M_OFFSET); |
shaorui | 48:1b51771c3647 | 75 | jps->SetMechOffset(JOINT_M_OFFSET ); |
shaorui | 48:1b51771c3647 | 76 | printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
shaorui | 48:1b51771c3647 | 77 | printf("\n\r Saved new zero position1: %.4f\n\r\n\r",JOINT_M_OFFSET ); |
shaorui | 48:1b51771c3647 | 78 | |
shaorui | 48:1b51771c3647 | 79 | /*************同时设置转子和关节零位置同步****************/ |
shaorui | 48:1b51771c3647 | 80 | for(int i = 0; i<n; i++){ // rotate forwards |
shaorui | 48:1b51771c3647 | 81 | for(int j = 0; j<n2; j++){ |
shaorui | 48:1b51771c3647 | 82 | theta_ref += delta; |
shaorui | 48:1b51771c3647 | 83 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
shaorui | 48:1b51771c3647 | 84 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
shaorui | 48:1b51771c3647 | 85 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); |
shaorui | 48:1b51771c3647 | 86 | if(PHASE_ORDER){ // Check phase ordering |
shaorui | 48:1b51771c3647 | 87 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); // Set duty cycles |
shaorui | 48:1b51771c3647 | 88 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
shaorui | 48:1b51771c3647 | 89 | } |
shaorui | 48:1b51771c3647 | 90 | else{ |
shaorui | 48:1b51771c3647 | 91 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); |
shaorui | 48:1b51771c3647 | 92 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); |
shaorui | 48:1b51771c3647 | 93 | } |
shaorui | 48:1b51771c3647 | 94 | wait_us(100); |
shaorui | 48:1b51771c3647 | 95 | ps->Sample(DT); |
shaorui | 48:1b51771c3647 | 96 | jps->Sample(DT); |
shaorui | 48:1b51771c3647 | 97 | } |
shaorui | 48:1b51771c3647 | 98 | ps->Sample(DT); |
shaorui | 48:1b51771c3647 | 99 | jps->Sample(DT); |
shaorui | 48:1b51771c3647 | 100 | theta_actual =(1.0f/GR)* ps->GetMechPosition(); |
shaorui | 48:1b51771c3647 | 101 | joint_theta_actual=jps->GetMechPosition(); |
shaorui | 48:1b51771c3647 | 102 | error_f[i] = theta_actual-joint_theta_actual; |
shaorui | 48:1b51771c3647 | 103 | raw_f[i] = jps->GetRawPosition(); |
shaorui | 48:1b51771c3647 | 104 | printf("%.4f %.4f %d\n\r", theta_actual, joint_theta_actual, raw_f[i]); |
shaorui | 48:1b51771c3647 | 105 | //theta_ref += delta; |
shaorui | 48:1b51771c3647 | 106 | } |
shaorui | 48:1b51771c3647 | 107 | |
shaorui | 48:1b51771c3647 | 108 | for(int i = 0; i<n; i++){ // rotate backwards |
shaorui | 48:1b51771c3647 | 109 | for(int j = 0; j<n2; j++){ |
shaorui | 48:1b51771c3647 | 110 | theta_ref -= delta; |
shaorui | 48:1b51771c3647 | 111 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
shaorui | 48:1b51771c3647 | 112 | svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
shaorui | 48:1b51771c3647 | 113 | TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); |
shaorui | 48:1b51771c3647 | 114 | if(PHASE_ORDER){ |
shaorui | 48:1b51771c3647 | 115 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v); |
shaorui | 48:1b51771c3647 | 116 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w); |
shaorui | 48:1b51771c3647 | 117 | } |
shaorui | 48:1b51771c3647 | 118 | else{ |
shaorui | 48:1b51771c3647 | 119 | TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v); |
shaorui | 48:1b51771c3647 | 120 | TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w); |
shaorui | 48:1b51771c3647 | 121 | } |
shaorui | 48:1b51771c3647 | 122 | wait_us(100); |
shaorui | 48:1b51771c3647 | 123 | ps->Sample(DT); |
shaorui | 48:1b51771c3647 | 124 | jps->Sample(DT); |
shaorui | 48:1b51771c3647 | 125 | } |
shaorui | 48:1b51771c3647 | 126 | ps->Sample(DT); |
shaorui | 48:1b51771c3647 | 127 | jps->Sample(DT); |
shaorui | 48:1b51771c3647 | 128 | |
shaorui | 48:1b51771c3647 | 129 | theta_actual =(1.0f/GR)* ps->GetMechPosition(); |
shaorui | 48:1b51771c3647 | 130 | joint_theta_actual=jps->GetMechPosition(); |
shaorui | 48:1b51771c3647 | 131 | error_b[i] = theta_actual-joint_theta_actual; |
shaorui | 48:1b51771c3647 | 132 | raw_b[i] = jps->GetRawPosition(); |
shaorui | 48:1b51771c3647 | 133 | printf("%.4f %.4f %d\n\r", theta_actual, joint_theta_actual, raw_b[i]); |
shaorui | 48:1b51771c3647 | 134 | //theta_ref -= delta; |
shaorui | 48:1b51771c3647 | 135 | } |
shaorui | 48:1b51771c3647 | 136 | |
shaorui | 48:1b51771c3647 | 137 | float offset = 0; |
shaorui | 48:1b51771c3647 | 138 | for(int i = 0; i<n; i++){ |
shaorui | 48:1b51771c3647 | 139 | offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset |
shaorui | 48:1b51771c3647 | 140 | } |
shaorui | 48:1b51771c3647 | 141 | //offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle |
shaorui | 48:1b51771c3647 | 142 | |
shaorui | 48:1b51771c3647 | 143 | |
shaorui | 48:1b51771c3647 | 144 | ps->SetElecOffset(offset); // Set joint position sensor offset |
shaorui | 48:1b51771c3647 | 145 | __float_reg[8]= offset; |
shaorui | 48:1b51771c3647 | 146 | //JOINT_OFFSET = offset; |
shaorui | 48:1b51771c3647 | 147 | |
shaorui | 48:1b51771c3647 | 148 | /// Perform filtering to linearize joint position sensor eccentricity |
shaorui | 48:1b51771c3647 | 149 | /// FIR n-sample average, where n = number of samples in one cycle |
shaorui | 48:1b51771c3647 | 150 | /// This filter has zero gain at electrical frequency and all integer multiples |
shaorui | 48:1b51771c3647 | 151 | /// So cogging effects should be completely filtered out. |
shaorui | 48:1b51771c3647 | 152 | |
shaorui | 48:1b51771c3647 | 153 | float error[n] = {0}; |
shaorui | 48:1b51771c3647 | 154 | const int window = 120; |
shaorui | 48:1b51771c3647 | 155 | float error_filt[n] = {0}; |
shaorui | 48:1b51771c3647 | 156 | float cogging_current[window] = {0}; |
shaorui | 48:1b51771c3647 | 157 | float mean = 0; |
shaorui | 48:1b51771c3647 | 158 | for (int i = 0; i<n; i++){ //Average the forward and back directions |
shaorui | 48:1b51771c3647 | 159 | error[i] = 0.5f*(error_f[i] + error_b[n-i-1]); |
shaorui | 48:1b51771c3647 | 160 | } |
shaorui | 48:1b51771c3647 | 161 | for (int i = 0; i<n; i++){ |
shaorui | 48:1b51771c3647 | 162 | for(int j = 0; j<window; j++){ |
shaorui | 48:1b51771c3647 | 163 | int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 |
shaorui | 48:1b51771c3647 | 164 | if(ind<0){ |
shaorui | 48:1b51771c3647 | 165 | ind += n;} // Moving average wraps around |
shaorui | 48:1b51771c3647 | 166 | else if(ind > n-1) { |
shaorui | 48:1b51771c3647 | 167 | ind -= n;} |
shaorui | 48:1b51771c3647 | 168 | error_filt[i] += error[ind]/(float)window; |
shaorui | 48:1b51771c3647 | 169 | } |
shaorui | 48:1b51771c3647 | 170 | if(i<window){ |
shaorui | 48:1b51771c3647 | 171 | cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP); |
shaorui | 48:1b51771c3647 | 172 | } |
shaorui | 48:1b51771c3647 | 173 | //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); |
shaorui | 48:1b51771c3647 | 174 | mean += error_filt[i]/n; |
shaorui | 48:1b51771c3647 | 175 | } |
shaorui | 48:1b51771c3647 | 176 | 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 | 177 | |
shaorui | 48:1b51771c3647 | 178 | |
shaorui | 48:1b51771c3647 | 179 | printf("\n\r Encoder non-linearity compensation table\n\r"); |
shaorui | 48:1b51771c3647 | 180 | printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r"); |
shaorui | 48:1b51771c3647 | 181 | for (int i = 0; i<n_joint; i++){ // build lookup table |
shaorui | 48:1b51771c3647 | 182 | int ind = (raw_offset>>7) + i; |
shaorui | 48:1b51771c3647 | 183 | if(ind > (n_joint-1)){ |
shaorui | 48:1b51771c3647 | 184 | ind -= n_joint; |
shaorui | 48:1b51771c3647 | 185 | } |
shaorui | 48:1b51771c3647 | 186 | joint[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(jps->GetCPR())/(2.0f*PI)); |
shaorui | 48:1b51771c3647 | 187 | printf("%d %d %d %f\n\r", i, ind, joint[ind], cogging_current[i]); |
shaorui | 48:1b51771c3647 | 188 | wait(.001); |
shaorui | 48:1b51771c3647 | 189 | } |
shaorui | 48:1b51771c3647 | 190 | |
shaorui | 48:1b51771c3647 | 191 | jps->WriteLUT(joint); // write lookup table to position sensor object |
shaorui | 48:1b51771c3647 | 192 | //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging)); //compensation doesn't actually work yet.... |
shaorui | 48:1b51771c3647 | 193 | memcpy(&ENCODER_JOINT, joint, sizeof(joint)); // copy the lookup table to the flash array |
shaorui | 48:1b51771c3647 | 194 | printf("\n\rEncoder Joint Offset (rad) %f\n\r", offset); |
shaorui | 48:1b51771c3647 | 195 | |
shaorui | 48:1b51771c3647 | 196 | if (!prefs->ready()) prefs->open(); |
shaorui | 48:1b51771c3647 | 197 | prefs->flush(); // write offset and lookup table to flash |
shaorui | 48:1b51771c3647 | 198 | prefs->close(); |
shaorui | 48:1b51771c3647 | 199 | |
shaorui | 48:1b51771c3647 | 200 | |
shaorui | 48:1b51771c3647 | 201 | } |