File content as of revision 22:60276ba87ac6:

/// Calibration procedures for determining position sensor offset, 
/// phase ordering, and position sensor linearization
/// 

#include "calibration.h"


void order_phases(PositionSensor *ps, GPIOStruct *gpio){   
    ///Checks phase order, to ensure that positive Q current produces
    ///torque in the positive direction wrt the position sensor.
    
    printf("\n\r Checking phase ordering\n\r");
    float theta_ref = 0;
    float theta_actual = 0;
    float v_d = .2;                                         //Put all volts on the D-Axis
    float v_q = 0.0;
    float v_u, v_v, v_w = 0;
    float dtc_u, dtc_v, dtc_w = .5;
    int sample_counter = 0;
    
    ///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<20000; i++){
        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                    // Set duty cycles
        TIM1->CCR2 = 0x708*(1.0f-dtc_v);
        TIM1->CCR1 = 0x708*(1.0f-dtc_w);
        wait_us(100);
        }
    //ps->ZeroPosition();
    ps->Sample(); 
    float theta_start = ps->GetMechPosition();              //get initial rotor position
    
    /// Rotate voltage angle
    while(theta_ref < 4*PI){                                //rotate for 2 electrical cycles
        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
        wait_us(100);
        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                    //Set duty cycles
        TIM1->CCR2 = 0x708*(1.0f-dtc_v);
        TIM1->CCR1 = 0x708*(1.0f-dtc_w);
       ps->Sample();                                        //sample position sensor
       theta_actual = ps->GetMechPosition();
       if(sample_counter > 200){
           sample_counter = 0 ;
        printf("%.4f   %.4f\n\r", theta_ref/(NPP), theta_actual);
        }
        sample_counter++;
       theta_ref += 0.001f;
        }
    float theta_end = ps->GetMechPosition();
    int direction = (theta_end - theta_start)>0;
    printf("Theta Start:   %f    Theta End:  %f\n\r", theta_start, theta_end);
    printf("Direction:  %d\n\r", direction);
    if(direction){printf("Phaseing correct\n\r");}
    else if(!direction){printf("Phasing incorrect.  Swapping phases V and W\n\r");}
    gpio->phasing = direction;
    
    }
    
    
    
void calibrate(PositionSensor *ps, GPIOStruct *gpio){
    /// 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");
    
    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 = 10;                                     // 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
    int raw_f[n] = {0};
    int raw_b[n] = {0};
    float theta_ref = 0;
    float theta_actual = 0;
    float v_d = .2;                                         // Put volts on the D-Axis
    float v_q = 0.0;
    float v_u, v_v, v_w = 0;
    float dtc_u, dtc_v, dtc_w = .5;
    
        
    ///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 = 0x708*(1.0f-dtc_u);                    // Set duty cycles
        if(gpio->phasing){
            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
            TIM1->CCR1 = 0x708*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = 0x708*(1.0f-dtc_v);
            TIM1->CCR2 = 0x708*(1.0f-dtc_w);
            }
        wait_us(100);
        }
    ps->Sample();   
    
    for(int i = 0; i<n; i++){                               // rotate forwards
       for(int j = 0; j<n2; j++){   
        theta_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 = 0x708*(1.0f-dtc_u);
        if(gpio->phasing){
            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
            TIM1->CCR1 = 0x708*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = 0x708*(1.0f-dtc_v);
            TIM1->CCR2 = 0x708*(1.0f-dtc_w);
            }
            wait_us(100);
            ps->Sample();
        }
       ps->Sample();
       theta_actual = ps->GetMechPosition();
       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]);
       //theta_ref += delta;
        }
    for(int i = 0; i<n; i++){                               // rotate backwards
       for(int j = 0; j<n2; j++){
       theta_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 = 0x708*(1.0f-dtc_u);
        if(gpio->phasing){
            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
            TIM1->CCR1 = 0x708*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = 0x708*(1.0f-dtc_v);
            TIM1->CCR2 = 0x708*(1.0f-dtc_w);
            }
            wait_us(100);
            ps->Sample();
        }
       ps->Sample();                                        // sample position sensor
       theta_actual = ps->GetMechPosition();                // 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]);
       //theta_ref -= delta;
        }    
        
        float 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
            }
        offset = fmod(offset*NPP, 2*PI);                    // convert mechanical angle to electrical angle
        printf("Encoder Electrical Offset (rad) %f\n\r",  offset);
            
        ps->SetElecOffset(offset);                          // Set position sensor 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 should also be completely filtered out.
        
        float error[n] = {0};
        int window = 128;
        float error_filt[n] = {0};
        float 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]);
            }
        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;
                }
            //printf("%.4f   %4f    %.4f   %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
            mean += error_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
        const int  n_lut = 128;
        int lut[n_lut];
        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   %d\n\r", raw_offset>>7, i, ind, lut[ind]);
            }
            ps->WriteLUT(lut);                                            // write lookup table to position sensor object
         
        
        
        
    
    
    }