Revision 34:47a55f96fbc4, committed 2017-10-02

Comitter:

benkatz

Date:

Mon Oct 02 00:55:39 2017 +0000

Parent:

33:3ab3cce8b44d

Commit message:

Auto-measurement

Changed in this revision

--- a/Calibration/calibration.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/Calibration/calibration.cpp	Mon Oct 02 00:55:39 2017 +0000
@@ -6,6 +6,13 @@
 #include "foc.h"
 #include "PreferenceWriter.h"
 #include "user_config.h"
+#include "math.h"
+#include "math_ops.h"
+
+void measure_rl(int n, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
+    
+    
+    }
 
 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){   
     
@@ -14,7 +21,7 @@
     printf("\n\r Checking phase ordering\n\r");
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .25;                                                             //Put all volts on the D-Axis
+    float v_d = 2.0;                                                             //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;
@@ -22,33 +29,33 @@
     
     ///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
+    svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                            //space vector modulation
+    TIM1->CCR3 = (PWM_ARR)*(0.5f);                                        // Set duty cycles
+    TIM1->CCR2 = (PWM_ARR)*(0.5f);
+    TIM1->CCR1 = (PWM_ARR)*(0.5f);
+    wait_us(100);
     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);
+        wait_us(50);
+        TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u);                                        // Set duty cycles
+        TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+        TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
         }
+        
+    
     //ps->ZeroPosition();
     ps->Sample(); 
     wait_us(1000);
     //float theta_start = ps->GetMechPosition();                                  //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
+    while(theta_ref < 4.0f*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
+        svm(V_BUS, 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);
+        TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u);                                        //Set duty cycles
+        TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+        TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
        ps->Sample();                                                            //sample position sensor
        theta_actual = ps->GetMechPosition();
        if(theta_ref==0){theta_start = theta_actual;}
@@ -61,6 +68,9 @@
         }
     float theta_end = ps->GetMechPosition();
     int direction = (theta_end - theta_start)>0;
+    float ratio = abs(4.0f*PI/(theta_end-theta_start));
+    int pole_pairs = (int) roundf(ratio);
+        
     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");}
@@ -86,7 +96,7 @@
     int raw_b[n] = {0};
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .25;                                                             // Put volts on the D-Axis
+    float v_d = 2.0f;                                                             // 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;
@@ -94,39 +104,34 @@
         
     ///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
+    svm(V_BUS, 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
+        TIM1->CCR3 = (PWM_ARR)*(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);
+            TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+            TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
             }
         else{
-            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
-            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
+            TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v);
+            TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w);
             }
         wait_us(100);
         }
     ps->Sample();   
-    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);
+       svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
+        TIM1->CCR3 = (PWM_ARR)*(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);
+            TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);                                    // Set duty cycles
+            TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
             }
         else{
-            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
-            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
+            TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v);
+            TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w);
             }
             wait_us(100);
             ps->Sample();
@@ -143,15 +148,15 @@
        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);
+       svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
+        TIM1->CCR3 = (PWM_ARR)*(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);
+            TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+            TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
             }
         else{
-            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
-            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
+            TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v);
+            TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w);
             }
             wait_us(100);
             ps->Sample();
@@ -183,7 +188,6 @@
         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]);
@@ -197,9 +201,7 @@
                     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;
             }
@@ -207,19 +209,18 @@
         
         
         printf("\n\r Encoder non-linearity compensation table\n\r");
-        printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
+        printf(" Sample Number : Lookup Index : Lookup Value\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]);
+            printf("%d   %d   %d\n\r", i, ind, lut[ind]);
             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);
         

--- a/Calibration/calibration.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/Calibration/calibration.h	Mon Oct 02 00:55:39 2017 +0000
@@ -10,4 +10,5 @@
 
 void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
 void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
+void measure_rl(int n, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs);
 #endif

--- a/Config/current_controller_config.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/Config/current_controller_config.h	Mon Oct 02 00:55:39 2017 +0000
@@ -1,11 +1,11 @@
 #ifndef CURRENT_CONTROLLER_CONFIG_H
 #define CURRENT_CONTROLLER_CONFIG_H
 
-#define K_D .05f                     // Volts/Amp
-#define K_Q .05f                     // Volts/Amp
-#define K_SCALE 0.0001f            // K_loop/Loop BW (Hz)
-#define KI_D 0.06f                  // 1/samples
-#define KI_Q 0.06f                  // 1/samples
+#define K_D 5.0f                     // Volts/Amp
+#define K_Q 5.0f                     // Volts/Amp
+#define K_SCALE 0.00043f            // K_loop/Loop BW (Hz)
+#define KI_D 0.0255f                  // 1/samples
+#define KI_Q 0.0255f                  // 1/samples
 #define V_BUS 24.0f                 // Volts
 
 #define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples

--- a/FOC/foc.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/FOC/foc.cpp	Mon Oct 02 00:55:39 2017 +0000
@@ -109,7 +109,7 @@
        controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*KI_D*controller->d_int;// + v_d_ff;  
        controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*KI_Q*controller->q_int;// + v_q_ff; 
        
-       //controller->v_q = 4.0f;;
+       //controller->v_q = 4.0f;
        //controller->v_d = 0.0f;
        
        //controller->v_d = v_d_ff;

--- a/main.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/main.cpp	Mon Oct 02 00:55:39 2017 +0000
@@ -10,13 +10,12 @@
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
-#define VERSION_NUM "1.0.1"
+#define VERSION_NUM "TEST_MODE"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
 int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
-
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -31,7 +30,8 @@
 #include "FlashWriter.h"
 #include "user_config.h"
 #include "PreferenceWriter.h"
-
+#include "FastMath.h"
+using namespace FastMath;
  
 PreferenceWriter prefs(6);
 
@@ -59,6 +59,9 @@
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
+volatile int cal_counter = 0;
+volatile float rise = 0;
+volatile float avg_current = 0;
 
  #define P_MIN -12.5f
  #define P_MAX 12.5f
@@ -189,7 +192,7 @@
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     wait(.001);
     controller.i_d_ref = 0;
-    controller.i_q_ref = 0;                                                     // Current Setpoints
+    controller.i_q_ref = 2;                                                     // Current Setpoints
     GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
     state_change = 0;
     printf("\n\r Entering Motor Mode \n\r");
@@ -240,7 +243,63 @@
             
             case CALIBRATION_MODE:                                              // Run encoder calibration procedure
                 if(state_change){
-                    calibrate();
+                    gpio.enable->write(1);
+
+                    //calibrate();
+                    state_change = 0;
+                    GPIOC->ODR |= (1 << 5);  
+                    TIM1->CCR3 = (PWM_ARR)*(0.5f);                                        // Set duty cycles
+                    TIM1->CCR2 = (PWM_ARR)*(0.5f);
+                    TIM1->CCR1 = (PWM_ARR)*(0.5f);
+                    wait_us(200);
+                    cal_counter = 1;
+                    rise = 0;
+                    avg_current = 0;
+                    }
+                if(cal_counter>0){
+                    if(cal_counter<1000){
+                        TIM1->CCR3 = (PWM_ARR)*(0.5f);                                        // Set duty cycles
+                        TIM1->CCR2 = (PWM_ARR)*(0.5f);
+                        TIM1->CCR1 = (PWM_ARR)*(0.5f);
+                        }
+                    if(cal_counter>=1000){
+                        float s = FastSin(0); 
+                        float c = FastCos(0);
+                        float dtc_u, dtc_v, dtc_w;
+                        float i_a, i_b, i_c, i_d, i_q = 0.0f;
+                        controller.v_d = 3.0f;
+                        controller.v_q = 0.0f;
+                        float v_u = c*controller.v_d - s*controller.v_q;                // Faster Inverse DQ0 transform
+                        float v_v = (0.86602540378f*s-.5f*c)*controller.v_d - (-0.86602540378f*c-.5f*s)*controller.v_q;
+                        float v_w = (-0.86602540378f*s-.5f*c)*controller.v_d - (0.86602540378f*c-.5f*s)*controller.v_q;
+                        svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
+                        TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u);                                        // Set duty cycles
+                        TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+                        TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
+                        i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset);    // Calculate phase currents from ADC readings
+                        i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset);
+                        i_a = -i_b - i_c;              
+                        i_d = 0.6666667f*(c*i_a + (0.86602540378f*s-.5f*c)*i_b + (-0.86602540378f*s-.5f*c)*i_c);   ///Faster DQ0 Transform
+                        i_q = 0.6666667f*(-s*i_a - (-0.86602540378f*c-.5f*s)*i_b - (0.86602540378f*c-.5f*s)*i_c);
+                        float current = sqrt(i_d*i_d + i_q*i_q);
+                        avg_current += .000025f*current;
+                        if(cal_counter<1020){rise += .05f*current;}
+                        }
+                    cal_counter++;
+                    if(cal_counter>41000){ 
+                        cal_counter = 0;
+                        GPIOC->ODR &= !(1 << 5);
+                        wait_us(200);
+                        gpio.enable->write(0);
+                        float L = controller.v_d*.0005f/(2.0f*rise);
+                        float R = controller.v_d/avg_current;
+                        printf("%f  \n\r", rise);
+                        calibrate();
+                        printf("Inductance:   %f\n\r", L);
+                        printf("Resistance:   %f\n\r", R);
+                        
+                        }
+                    
                     }
                 break;
              
@@ -260,8 +319,16 @@
                 //TIM1->CCR2 = 0x708*(1.0f);     
                 
                 //controller.i_q_ref = controller.t_ff/KT_OUT;   
-                
-                torque_control(&controller);     
+                if(count >40){
+                     controller.i_q_ref = 20;
+                     //wait(.001);
+                    //printf(" Started commutating\n\r");
+                     }
+                if(count>80){
+                    controller.i_q_ref = -20;
+                    count = 0;
+                    }
+                //torque_control(&controller);     
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
@@ -272,14 +339,10 @@
                 //toggle.write(0);
                 controller.timeout += 1;
                 
-                if(count == 1){
-                     count = 0;
-                     //wait(.001);
-                    //printf(" Started commutating\n\r");
-                     }
-                     }
+                }
                      
                 break;
+                
             case SETUP_MODE:
                 if(state_change){
                     enter_setup_state();
@@ -289,6 +352,7 @@
                 print_encoder();
                 break;
                 }
+            
                  
       }
   TIM1->SR = 0x0;                                                               // reset the status register

--- a/math_ops.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/math_ops.cpp	Mon Oct 02 00:55:39 2017 +0000
@@ -22,6 +22,12 @@
     return (x < y ? (x < z ? x : z) : (y < z ? y : z));
     }
     
+float roundf(float x){
+    /// Returns nearest integer ///
+    
+    return x < 0.0f ? ceilf(x - 0.5f) : floorf(x + 0.5f);
+    }
+    
 void limit_norm(float *x, float *y, float limit){
     /// Scales the lenght of vector (x, y) to be <= limit ///
     float norm = sqrt(*x * *x + *y * *y);

--- a/math_ops.h	Wed Aug 30 18:10:27 2017 +0000
+++ b/math_ops.h	Mon Oct 02 00:55:39 2017 +0000
@@ -9,6 +9,7 @@
 float fminf(float x, float y);
 float fmaxf3(float x, float y, float z);
 float fminf3(float x, float y, float z);
+float roundf(float x);
 void limit_norm(float *x, float *y, float limit);
 int float_to_uint(float x, float x_min, float x_max, int bits);
 float uint_to_float(int x_int, float x_min, float x_max, int bits);