1

Dependencies:   mbed-dev-f303 FastPWM3

Calibration/calibration.cpp

Committer:
shaorui
Date:
2020-02-07
Revision:
48:1b51771c3647
Parent:
45:aadebe074af6
Child:
49:7eac11914980

File content as of revision 48:1b51771c3647:

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

#include "calibration.h"
#include "foc.h"
#include "PreferenceWriter.h"
#include "user_config.h"
#include "motor_config.h"
#include "current_controller_config.h"

void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){   
    
    ///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 = .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;
    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 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        // Set duty cycles
        TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
        TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
        wait_us(100);
        }
    //ps->ZeroPosition();
    ps->Sample(DT); 
    wait_us(1000);
    //float theta_start = ps->GetMechPositionFixed();                                  //get initial rotor position
    float theta_start;
    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("\n\rCurrent\n\r");
    printf("%f    %f   %f\n\r\n\r", controller->i_d, controller->i_q, current);
    /// 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 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        //Set duty cycles
        TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
        TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
       ps->Sample(DT);                                                            //sample position sensor
       theta_actual = ps->GetMechPositionFixed();
       if(theta_ref==0){theta_start = theta_actual;}
       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->GetMechPositionFixed();
    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("Phasing correct\n\r");}
    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
    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 = 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
    const int  n_lut = 128;
    int lut[n_lut]= {0};                                                        // clear any old lookup table before starting.
    ps->WriteLUT(lut); 
    int raw_f[n] = {0};
    int raw_b[n] = {0};
    float theta_ref = 0;
    float theta_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;
       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);
        }
       ps->Sample(DT);
       theta_actual = ps->GetMechPositionFixed();
       error_f[i] = theta_ref/NPP - theta_actual;
       raw_f[i] = ps->GetRawPosition();
        printf("ref %.4f\n\r  actual %.4f\n\r   raw %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 = (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);
        }
       ps->Sample(DT);                                                            // sample position sensor
       theta_actual = ps->GetMechPositionFixed();                                    // get mechanical position
       error_b[i] = theta_ref/NPP - theta_actual;
       raw_b[i] = ps->GetRawPosition();
       printf("ref %.4f\n\r  actual %.4f\n\r   raw %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
        
            
        ps->SetElecOffset(offset);                                              // Set position sensor offset
        __float_reg[0] = offset;
        E_OFFSET = 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};
        const int window = 128;
        float error_filt[n] = {0};
        float cogging_current[window] = {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;
                }
            if(i<window){
                cogging_current[i] = current*sinf((error[i] - error_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;
            }
        int raw_offset = (raw_f[0] + 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_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);
            }
            
        ps->WriteLUT(lut);                                                      // 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
        printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
        
        if (!prefs->ready()) prefs->open();
        prefs->flush();                                                         // write offset and lookup table to flash
        prefs->close();


    }
    
 /* 
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();

}*/