Shao Rui / RV_brake_2021_3_19

1

Dependencies:   mbed-dev-f303 FastPWM3

Joint_Calibration/joint_calibration.cpp@54:4c9415402628, 2020-09-17 (annotated)

Committer:
Rushu
Date:
Thu Sep 17 07:49:27 2020 +0000
Revision:
54:4c9415402628
Parent:
49:7eac11914980 
use new FOC

Who changed what in which revision?

UserRevisionLine numberNew 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
Rushu 54:4c9415402628 40 svm(1.0, v_u, v_v, v_w, 0, &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
Rushu 54:4c9415402628 84 svm(1.0, v_u, v_v, v_w, 0, &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
Rushu 54:4c9415402628 112 svm(1.0, v_u, v_v, v_w, 0, &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 }