calibration.cpp

00001 /// Calibration procedures for determining position sensor offset, 
00002 /// phase ordering, and position sensor linearization
00003 /// 
00004 
00005 #include "calibration.h"
00006 #include "foc.h"
00007 #include "PreferenceWriter.h"
00008 #include "user_config.h"
00009 #include "motor_config.h"
00010 #include "current_controller_config.h"
00011 
00012 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){   
00013     
00014     ///Checks phase order, to ensure that positive Q current produces
00015     ///torque in the positive direction wrt the position sensor.
00016     printf("\n\r Checking phase ordering\n\r");
00017     float theta_ref = 0;
00018     float theta_actual = 0;
00019     float v_d = V_CAL;                                                             //Put all volts on the D-Axis
00020     float v_q = 0.0f;
00021     float v_u, v_v, v_w = 0;
00022     float dtc_u, dtc_v, dtc_w = .5f;
00023     int sample_counter = 0;
00024     
00025     ///Set voltage angle to zero, wait for rotor position to settle
00026     abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                                 //inverse dq0 transform on voltages
00027     svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                            //space vector modulation
00028     for(int i = 0; i<20000; i++){
00029         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        // Set duty cycles
00030         TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
00031         TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
00032         wait_us(100);
00033         }
00034     //ps->ZeroPosition();
00035     ps->Sample(DT); 
00036     wait_us(1000);
00037     //float theta_start = ps->GetMechPositionFixed();                                  //get initial rotor position
00038     float theta_start;
00039     controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
00040     controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
00041     controller->i_a = -controller->i_b - controller->i_c;
00042     dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
00043     float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
00044     printf("\n\rCurrent\n\r");
00045     printf("%f    %f   %f\n\r\n\r", controller->i_d, controller->i_q, current);
00046     /// Rotate voltage angle
00047     while(theta_ref < 4*PI){                                                    //rotate for 2 electrical cycles
00048         abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                             //inverse dq0 transform on voltages
00049         svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                        //space vector modulation
00050         wait_us(100);
00051         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        //Set duty cycles
00052         TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
00053         TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
00054        ps->Sample(DT);                                                            //sample position sensor
00055        theta_actual = ps->GetMechPositionFixed();
00056        if(theta_ref==0){theta_start = theta_actual;}
00057        if(sample_counter > 200){
00058            sample_counter = 0 ;
00059         printf("%.4f   %.4f\n\r", theta_ref/(NPP), theta_actual);
00060         }
00061         sample_counter++;
00062        theta_ref += 0.001f;
00063         }
00064     float theta_end = ps->GetMechPositionFixed();
00065     int direction = (theta_end - theta_start)>0;
00066     printf("Theta Start:   %f    Theta End:  %f\n\r", theta_start, theta_end);
00067     printf("Direction:  %d\n\r", direction);
00068     if(direction){printf("Phasing correct\n\r");}
00069     else if(!direction){printf("Phasing incorrect.  Swapping phases V and W\n\r");}
00070     PHASE_ORDER = direction;
00071     }
00072     
00073     
00074 void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
00075     /// Measures the electrical angle offset of the position sensor
00076     /// and (in the future) corrects nonlinearity due to position sensor eccentricity
00077     printf("Starting calibration procedure\n\r");
00078     float * error_f;
00079     float * error_b;
00080     int * lut;
00081     int * raw_f;
00082     int * raw_b;
00083     float * error;
00084     float * error_filt;
00085     
00086     const int n = 128*NPP;                                                      // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
00087     const int n2 = 40;                                                          // increments between saved samples (for smoothing motion)
00088     float delta = 2*PI*NPP/(n*n2);                                              // change in angle between samples
00089     error_f = new float[n]();                                                     // error vector rotating forwards
00090     error_b = new float[n]();                                                     // error vector rotating backwards
00091     const int  n_lut = 128;
00092     constexpr size_t lut_size = sizeof(int) * n_lut;
00093     lut = new int[n_lut]();                                                        // clear any old lookup table before starting.
00094     
00095     error = new float[n]();
00096     const int window = 128;
00097     error_filt = new float[n]();
00098     float cogging_current[window] = {0};
00099     
00100     ps->WriteLUT(lut); 
00101     raw_f = new int[n]();
00102     raw_b = new int[n]();
00103     float theta_ref = 0;
00104     float theta_actual = 0;
00105     float v_d = V_CAL;                                                             // Put volts on the D-Axis
00106     float v_q = 0.0f;
00107     float v_u, v_v, v_w = 0;
00108     float dtc_u, dtc_v, dtc_w = .5f;
00109     
00110         
00111     ///Set voltage angle to zero, wait for rotor position to settle
00112     abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                                 // inverse dq0 transform on voltages
00113     svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                            // space vector modulation
00114     for(int i = 0; i<40000; i++){
00115         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        // Set duty cycles
00116         if(PHASE_ORDER){                                   
00117             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
00118             TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
00119             }
00120         else{
00121             TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
00122             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
00123             }
00124         wait_us(100);
00125         }
00126     ps->Sample(DT);   
00127     controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
00128     controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
00129     controller->i_a = -controller->i_b - controller->i_c;
00130     dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
00131     float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
00132     printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
00133     for(int i = 0; i<n; i++){                                                   // rotate forwards
00134        for(int j = 0; j<n2; j++){   
00135         theta_ref += delta;
00136        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                              // inverse dq0 transform on voltages
00137        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
00138         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
00139         if(PHASE_ORDER){                                                        // Check phase ordering
00140             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);                                    // Set duty cycles
00141             TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
00142             }
00143         else{
00144             TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
00145             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
00146             }
00147             wait_us(100);
00148             ps->Sample(DT);
00149         }
00150        ps->Sample(DT);
00151        theta_actual = ps->GetMechPositionFixed();
00152        error_f[i] = theta_ref/NPP - theta_actual;
00153        raw_f[i] = ps->GetRawPosition();
00154         printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
00155        //theta_ref += delta;
00156         }
00157     
00158     for(int i = 0; i<n; i++){                                                   // rotate backwards
00159        for(int j = 0; j<n2; j++){
00160        theta_ref -= delta;
00161        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                              // inverse dq0 transform on voltages
00162        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
00163         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
00164         if(PHASE_ORDER){
00165             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
00166             TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
00167             }
00168         else{
00169             TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
00170             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
00171             }
00172             wait_us(100);
00173             ps->Sample(DT);
00174         }
00175        ps->Sample(DT);                                                            // sample position sensor
00176        theta_actual = ps->GetMechPositionFixed();                                    // get mechanical position
00177        error_b[i] = theta_ref/NPP - theta_actual;
00178        raw_b[i] = ps->GetRawPosition();
00179        printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
00180        //theta_ref -= delta;
00181         }    
00182         
00183         float offset = 0;                                  
00184         for(int i = 0; i<n; i++){
00185             offset += (error_f[i] + error_b[n-1-i])/(2.0f*n);                   // calclate average position sensor offset
00186             }
00187         offset = fmod(offset*NPP, 2*PI);                                        // convert mechanical angle to electrical angle
00188         
00189             
00190         ps->SetElecOffset(offset);                                              // Set position sensor offset
00191         __float_reg[0] = offset;
00192         E_OFFSET = offset;
00193         
00194         /// Perform filtering to linearize position sensor eccentricity
00195         /// FIR n-sample average, where n = number of samples in one electrical cycle
00196         /// This filter has zero gain at electrical frequency and all integer multiples
00197         /// So cogging effects should be completely filtered out.
00198         
00199         
00200         float mean = 0;
00201         for (int i = 0; i<n; i++){                                              //Average the forward and back directions
00202             error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
00203             }
00204         for (int i = 0; i<n; i++){
00205             for(int j = 0; j<window; j++){
00206                 int ind = -window/2 + j + i;                                    // Indexes from -window/2 to + window/2
00207                 if(ind<0){
00208                     ind += n;}                                                  // Moving average wraps around
00209                 else if(ind > n-1) {
00210                     ind -= n;}
00211                 error_filt[i] += error[ind]/(float)window;
00212                 }
00213             if(i<window){
00214                 cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
00215                 }
00216             //printf("%.4f   %4f    %.4f   %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
00217             mean += error_filt[i]/n;
00218             }
00219         int raw_offset = (raw_f[0] + raw_b[n-1])/2;                             //Insensitive to errors in this direction, so 2 points is plenty
00220         
00221         
00222         printf("\n\r Encoder non-linearity compensation table\n\r");
00223         printf(" Sample Number : Lookup Index : Lookup Value\n\r\n\r");
00224         for (int i = 0; i<n_lut; i++){                                          // build lookup table
00225             int ind = (raw_offset>>7) + i;
00226             if(ind > (n_lut-1)){ 
00227                 ind -= n_lut;
00228                 }
00229             lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
00230             printf("%d   %d   %d \n\r", i, ind, lut[ind]);
00231             wait_us(1000);
00232             }
00233             
00234         ps->WriteLUT(lut);                                                      // write lookup table to position sensor object
00235         //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging));  //compensation doesn't actually work yet....
00236         memcpy(&ENCODER_LUT, lut, lut_size);                                 // copy the lookup table to the flash array
00237         printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
00238         
00239         if (!prefs->ready()) prefs->open();
00240         prefs->flush();                                                         // write offset and lookup table to flash
00241         prefs->close();
00242         
00243         delete[] error_f;       //gotta free up that ram
00244         delete[] error_b;
00245         delete[] lut;
00246         delete[] raw_f;
00247         delete[] raw_b;
00248 
00249     }