Diff: Calibration/calibration.cpp

Revision:

22:60276ba87ac6

Child:

23:2adf23ee0305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/calibration.cpp	Fri Mar 31 18:24:46 2017 +0000
@@ -0,0 +1,201 @@
+/// 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
+         
+        
+        
+        
+    
+    
+    }
\ No newline at end of file