Modified Motor Driver Firmware to include Flash + Thermal

Dependencies:   FastPWM3 mbed-dev-STM-lean

Revision:
25:f5741040c4bb
Parent:
24:58c2d7571207
Child:
26:2b865c00d7e9
--- a/Calibration/calibration.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/Calibration/calibration.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -13,17 +13,17 @@
     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_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
+    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->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);
@@ -31,7 +31,7 @@
     //ps->ZeroPosition();
     ps->Sample(); 
     wait_us(1000);
-    float theta_start = ps->GetMechPosition();                              //get initial rotor position
+    float theta_start = ps->GetMechPosition();                                  //get initial rotor position
     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;
@@ -40,14 +40,14 @@
     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
+    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->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
+       ps->Sample();                                                            //sample position sensor
        theta_actual = ps->GetMechPosition();
        if(sample_counter > 200){
            sample_counter = 0 ;
@@ -71,26 +71,26 @@
     /// 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
+    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
     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_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
+    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
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                        // Set duty cycles
         if(PHASE_ORDER){                                   
             TIM1->CCR2 = 0x708*(1.0f-dtc_v);
             TIM1->CCR1 = 0x708*(1.0f-dtc_w);
@@ -107,14 +107,14 @@
     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));
-    for(int i = 0; i<n; i++){                                               // rotate forwards
+    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
+       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(PHASE_ORDER){
-            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+        if(PHASE_ORDER){                                                        // Check phase ordering
+            TIM1->CCR2 = 0x708*(1.0f-dtc_v);                                    // Set duty cycles
             TIM1->CCR1 = 0x708*(1.0f-dtc_w);
             }
         else{
@@ -132,11 +132,11 @@
        //theta_ref += delta;
         }
     printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
-    for(int i = 0; i<n; i++){                                               // rotate backwards
+    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
+       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(PHASE_ORDER){
             TIM1->CCR2 = 0x708*(1.0f-dtc_v);
@@ -149,8 +149,8 @@
             wait_us(100);
             ps->Sample();
         }
-       ps->Sample();                                                        // sample position sensor
-       theta_actual = ps->GetMechPosition();                                // get mechanical position
+       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]);
@@ -159,33 +159,33 @@
         
         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 += (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
+        offset = fmod(offset*NPP, 2*PI);                                        // convert mechanical angle to electrical angle
         
             
-        ps->SetElecOffset(offset);                                          // Set position sensor offset
+        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 should be completely filtered out.
+        /// 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
+        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
+                int ind = -window/2 + j + i;                                    // Indexes from -window/2 to + window/2
                 if(ind<0){
-                    ind += n;}                                              // Moving average wraps around
+                    ind += n;}                                                  // Moving average wraps around
                 else if(ind > n-1) {
                     ind -= n;}
                 error_filt[i] += error[ind]/(float)window;
@@ -196,12 +196,12 @@
             //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
+        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];
         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
+        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;
@@ -210,14 +210,14 @@
             printf("%d   %d   %d   %f\n\r", i, ind, lut[ind],  cogging_current[i]);
             }
             
-        ps->WriteLUT(lut);                                            // write lookup table to position sensor object
+        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));
+        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();
-        //prefs->close();
+        prefs->flush();                                                         // write offset and lookup table to flash
+        prefs->close();
 
 
     }
\ No newline at end of file