File content as of revision 55:d614e29c60c5:

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

#include "joint_calibration.h"
#include "foc.h"
#include "PreferenceWriter.h"
#include "user_config.h"
#include "motor_config.h"
#include "current_controller_config.h"
#include "MA700Sensor.h"

    
void joint_calibrate(PositionSensorMA700 *jps,PositionSensorAM5147 *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
    

    printf("Starting joint calibration procedure !\n\r");
    const int n = 120*NPP;                                                      // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
    const int n2 = 50*GR;                                                          // increments between saved samples (for smoothing motion)
    float delta = 2*PI*NPP*GR/(n*n2);                                              // change in angle between samples
    float error_f[n] = {0};                                                     // error vector rotating forwards(error between motor and joint)
    float error_b[n] = {0};                                                     // error vector rotating backwards(error between motor and joint)
    const int  n_joint = 120;
    int joint[n_joint]= {0};                                                        // clear any old lookup table before starting.
    jps->WriteLUT(joint); 
    int raw_f[n] = {0};
    int raw_b[n] = {0};
    float theta_ref = 0;
    float theta_actual = 0;
     float joint_theta_actual = 0;
    //float v_d = .15f;
    float v_d = .15f;                                                              // 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, 0, &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); 
    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(" Joint offset starting !\n\r\n\r");
    
    /*************同时设置转子和关节零位置同步****************/
    ps->SetMechOffset(0);
    jps->SetMechOffset(0);
    ps->Sample(DT);
    jps->Sample(DT);
    wait_us(20);
    M_OFFSET = ps->GetMechPosition();
    JOINT_M_OFFSET   =jps->GetMechPosition();
    if (!prefs->ready()) prefs->open();
    prefs->flush();                                                  // Write new prefs to flash
    prefs->close();    
    prefs->load(); 
    ps->SetMechOffset(M_OFFSET);
    jps->SetMechOffset(JOINT_M_OFFSET  );
    printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
    printf("\n\r  Saved new zero position1:  %.4f\n\r\n\r",JOINT_M_OFFSET );

    /*************同时设置转子和关节零位置同步****************/
    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, 0, &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 =(1.0f/GR)* ps->GetMechPosition();
       joint_theta_actual=jps->GetMechPosition();
       error_f[i] = theta_actual-joint_theta_actual;
       raw_f[i] = jps->GetRawPosition();
        printf("%.4f   %.4f    %d\n\r", theta_actual, joint_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, 0, &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); 
       jps->Sample(DT); 
       
       theta_actual =(1.0f/GR)* ps->GetMechPosition();
       joint_theta_actual=jps->GetMechPosition();
       error_b[i] = theta_actual-joint_theta_actual;
       raw_b[i] = jps->GetRawPosition();
        printf("%.4f   %.4f    %d\n\r", theta_actual, joint_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 joint position sensor offset
        __float_reg[8]= offset;
        //JOINT_OFFSET  = offset;
        
        /// Perform filtering to linearize joint position sensor eccentricity
        /// FIR n-sample average, where n = number of samples in one 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 = 120;
        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_joint; i++){                                 // build lookup table
            int ind = (raw_offset>>7) + i;
            if(ind > (n_joint-1)){ 
                ind -= n_joint;
                }
           joint[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(jps->GetCPR())/(2.0f*PI));
            printf("%d   %d   %d   %f\n\r", i, ind, joint[ind], cogging_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));                                 // copy the lookup table to the flash array
        printf("\n\rEncoder Joint Offset (rad) %f\n\r",  offset);
        
        if (!prefs->ready()) prefs->open();
        prefs->flush();                                                         // write offset and lookup table to flash
        prefs->close();


    }