Revision 46:2d4b1dafcfe3, committed 2018-07-12

Comitter:

benkatz

Date:

Thu Jul 12 02:50:34 2018 +0000

Parent:

45:26801179208e

Child:

47:e1196a851f76

Commit message:

calibration frees up memory when done;

Changed in this revision

--- a/Calibration/calibration.cpp	Wed Jun 27 03:44:44 2018 +0000
+++ b/Calibration/calibration.cpp	Thu Jul 12 02:50:34 2018 +0000
@@ -75,17 +75,30 @@
     /// 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");
+    float * error_f;
+    float * error_b;
+    int * lut;
+    int * raw_f;
+    int * raw_b;
+    float * error;
+    float * error_filt;
     
     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
+    error_f = new float[n]();                                                     // error vector rotating forwards
+    error_b = new float[n]();                                                     // error vector rotating backwards
     const int  n_lut = 128;
-    int lut[n_lut]= {0};                                                        // clear any old lookup table before starting.
+    lut = new int[n_lut]();                                                        // clear any old lookup table before starting.
+    
+    error = new float[n]();
+    const int window = 128;
+    error_filt = new float[n]();
+    float cogging_current[window] = {0};
+    
     ps->WriteLUT(lut); 
-    int raw_f[n] = {0};
-    int raw_b[n] = {0};
+    raw_f = new int[n]();
+    raw_b = new int[n]();
     float theta_ref = 0;
     float theta_actual = 0;
     float v_d = .05f;                                                             // Put volts on the D-Axis
@@ -182,10 +195,7 @@
         /// 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]);
@@ -209,14 +219,14 @@
         
         
         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);
             }
             
@@ -228,6 +238,11 @@
         if (!prefs->ready()) prefs->open();
         prefs->flush();                                                         // write offset and lookup table to flash
         prefs->close();
-
+        
+        delete[] error_f;       //gotta free up that ram
+        delete[] error_b;
+        delete[] lut;
+        delete[] raw_f;
+        delete[] raw_b;
 
     }
\ No newline at end of file

--- a/Config/current_controller_config.h	Wed Jun 27 03:44:44 2018 +0000
+++ b/Config/current_controller_config.h	Thu Jul 12 02:50:34 2018 +0000
@@ -8,7 +8,7 @@
 #define KI_D 0.0255f                // PI zero, in radians per sample
 #define KI_Q 0.0255f                // PI zero, in radians per sample
 #define V_BUS 24.0f                 // Volts
-#define OVERMODULATION 1.2f         // 1.0 = no overmodulation
+#define OVERMODULATION 1.4f         // 1.0 = no overmodulation
 
 #define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples
 #define Q_INT_LIM V_BUS/(K_Q*KI_Q)  // Amps*samples

--- a/Config/motor_config.h	Wed Jun 27 03:44:44 2018 +0000
+++ b/Config/motor_config.h	Thu Jul 12 02:50:34 2018 +0000
@@ -5,7 +5,7 @@
 #define L_D 0.00003f            //Henries
 #define L_Q 0.00003f            //Henries
 #define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
-#define NPP 21                  //Number of pole pairs
+#define NPP 20                  //Number of pole pairs
 #define GR 6.0f                 //Gear ratio
 #define KT_OUT 0.45f            //KT*GR
 #define WB 0.0024f               //Flux linkage, Webers.  

--- a/DRV8323/DRV.h	Wed Jun 27 03:44:44 2018 +0000
+++ b/DRV8323/DRV.h	Thu Jul 12 02:50:34 2018 +0000
@@ -2,24 +2,151 @@
 #define DRV_H
 
 /// Registers ///
-#define FSR1            0x0     ///Fault Status Register 1///
-#define FSR2            0x1     ///Fault Status Register 2///
-#define DCR             0x2     ///Drive Control Register///
-#define HSR             0x3     ///Gate Drive HS Register///
-#define LSR             0x4     ///Gate Drive LS Register///
-#define OCPCR           0x5     ///OCP Control Register///
-#define CSACR           0x6     ///CSA Control Register///
+#define FSR1            0x0     /// Fault Status Register 1
+#define FSR2            0x1     /// Fault Status Register 2
+#define DCR             0x2     /// Drive Control Register
+#define HSR             0x3     /// Gate Drive HS Register 
+#define LSR             0x4     /// Gate Drive LS Register  
+#define OCPCR           0x5     /// OCP Control Register    
+#define CSACR           0x6     /// CSA Control Register    
+
+/// Drive Control Fields ///
+#define DIS_CPUV_EN         0x0     /// Charge pump UVLO fault
+#define DIS_CPUV_DIS        0x1
+#define DIS_GDF_EN          0x0     /// Gate drive fauilt
+#define DIS_GDF_DIS         0x1
+#define OTW_REP_EN          0x1     /// Over temp warning reported on nFAULT/FAULT bit
+#define OTW_REP_DIS         0x0
+#define PWM_MODE_6X         0x0     /// PWM Input Modes
+#define PWM_MODE_3X         0x1
+#define PWM_MODE_1X         0x2
+#define PWM_MODE_IND        0x3
+#define PWM_1X_COM_SYNC     0x0     /// 1x PWM Mode synchronou rectification
+#define PWM_1X_COM_ASYNC    0x1
+#define PWM_1X_DIR_0        0x0     /// In 1x PWM mode this bit is ORed with the INHC (DIR) input
+#define PWM_1X_DIR_1        0x1
+
+/// Gate Drive HS Fields ///
+#define LOCK_ON             0x6
+#define LOCK_OFF            0x3
+#define IDRIVEP_HS_10MA     0x0     /// Gate drive high side turn on current
+#define IDRIVEP_HS_30MA     0x1
+#define IDRIVEP_HS_60MA     0x2
+#define IDRIVEP_HS_80MA     0x3
+#define IDRIVEP_HS_120MA    0x4
+#define IDRIVEP_HS_140MA    0x5
+#define IDRIVEP_HS_170MA    0x6
+#define IDRIVEP_HS_190MA    0x7
+#define IDRIVEP_HS_260MA    0x8
+#define IDRIVEP_HS_330MA    0x9
+#define IDRIVEP_HS_370MA    0xA
+#define IDRIVEP_HS_440MA    0xB
+#define IDRIVEP_HS_570MA    0xC
+#define IDRIVEP_HS_680MA    0xD
+#define IDRIVEP_HS_820MA    0xE
+#define IDRIVEP_HS_1000MA   0xF
+#define IDRIVEN_HS_20MA     0x0     /// High side turn off current
+#define IDRIVEN_HS_60MA     0x1     
+#define IDRIVEN_HS_120MA    0x2
+#define IDRIVEN_HS_160MA    0x3
+#define IDRIVEN_HS_240MA    0x4
+#define IDRIVEN_HS_280MA    0x5
+#define IDRIVEN_HS_340MA    0x6
+#define IDRIVEN_HS_380MA    0x7
+#define IDRIVEN_HS_520MA    0x8
+#define IDRIVEN_HS_660MA    0x9
+#define IDRIVEN_HS_740MA    0xA
+#define IDRIVEN_HS_880MA    0xB
+#define IDRIVEN_HS_1140MA   0xC
+#define IDRIVEN_HS_1360MA   0xD
+#define IDRIVEN_HS_1640MA   0xE
+#define IDRIVEN_HS_2000MA   0xF
 
-#define PWM_MODE_6X     0x0     ///PWM Input Modes///
-#define PWM_MODE_3X     0x1
-#define PWM_MODE_1X     0x2
-#define PWM_MODE_IND    0x3
-#define CSA_GAIN_5      0x0     ///Current Sensor Gain///
-#define CSA_GAIN_10     0x1
-#define CSA_GAIN_20     0x2
-#define CSA_GAIN_40     0x3
-#define DIS_SEN_EN      0x0     ///Overcurrent Fault
-#define DIS_SEN_DIS     0x1
+/// Gate Drive LS Fields ///
+#define TDRIVE_500NS        0x0     /// Peak gate-current drive time
+#define TDRIVE_1000NS       0x1
+#define TDRIVE_2000NS       0x2
+#define TDRIVE_4000NS       0x3
+#define IDRIVEP_LS_10MA     0x0     /// Gate drive high side turn on current
+#define IDRIVEP_LS_30MA     0x1
+#define IDRIVEP_LS_60MA     0x2
+#define IDRIVEP_LS_80MA     0x3
+#define IDRIVEP_LS_120MA    0x4
+#define IDRIVEP_LS_140MA    0x5
+#define IDRIVEP_LS_170MA    0x6
+#define IDRIVEP_LS_190MA    0x7
+#define IDRIVEP_LS_260MA    0x8
+#define IDRIVEP_LS_330MA    0x9
+#define IDRIVEP_LS_370MA    0xA
+#define IDRIVEP_LS_440MA    0xB
+#define IDRIVEP_LS_570MA    0xC
+#define IDRIVEP_LS_680MA    0xD
+#define IDRIVEP_LS_820MA    0xE
+#define IDRIVEP_LS_1000MA   0xF
+#define IDRIVEN_LS_20MA     0x0     /// High side turn off current
+#define IDRIVEN_LS_60MA     0x1     
+#define IDRIVEN_LS_120MA    0x2
+#define IDRIVEN_LS_160MA    0x3
+#define IDRIVEN_LS_240MA    0x4
+#define IDRIVEN_LS_280MA    0x5
+#define IDRIVEN_LS_340MA    0x6
+#define IDRIVEN_LS_380MA    0x7
+#define IDRIVEN_LS_520MA    0x8
+#define IDRIVEN_LS_660MA    0x9
+#define IDRIVEN_LS_740MA    0xA
+#define IDRIVEN_LS_880MA    0xB
+#define IDRIVEN_LS_1140MA   0xC
+#define IDRIVEN_LS_1360MA   0xD
+#define IDRIVEN_LS_1640MA   0xE
+#define IDRIVEN_LS_2000MA   0xF
+
+/// OCP Control Fields ///
+#define TRETRY_4MS          0x0     /// VDS OCP and SEN OCP retry time
+#define TRETRY_50US         0x1
+#define DEADTIME_50NS       0x0     /// Deadtime
+#define DEADTIME_100NS      0x1
+#define DEADTIME_200NS      0x2
+#define DEADTIME_400NS      0x3
+#define OCP_LATCH           0x0     /// OCP Mode
+#define OCP_RETRY           0x1
+#define OCP_REPORT          0x2
+#define OCP_NONE            0x3
+#define OCP_DEG_2US         0x0     /// OCP Deglitch Time
+#define OCP_DEG_4US         0x1
+#define OCP_DEG_6US         0x2
+#define OCP_DEG_8US         0x3
+#define VDS_LVL_0_06        0x0
+#define VDS_LVL_0_13        0x1
+#define VDS_LVL_0_2         0x2
+#define VDS_LVL_0_26        0x3
+#define VDS_LVL_0_31        0x4
+#define VDS_LVL_0_45        0x5
+#define VDS_LVL_0_53        0x6
+#define VDS_LVL_0_6         0x7
+#define VDS_LVL_0_68        0x8
+#define VDS_LVL_0_75        0x9
+#define VDS_LVL_0_94        0xA
+#define VDS_LVL_1_13        0xB
+#define VDS_LVL_1_3         0xC
+#define VDS_LVL_1_5         0xD
+#define VDS_LVL_1_7         0xE
+#define VDS_LVL_1_88        0xF
+
+/// CSA Control Fields ///
+#define CSA_FET_SP          0x0     /// Current sense amplifier positive input
+#define CSA_FET_SH          0x1
+#define VREF_DIV_1          0x0     /// Amplifier reference voltage is VREV/1
+#define VREF_DIV_2          0x1     /// Amplifier reference voltage is VREV/2
+#define CSA_GAIN_5          0x0     /// Current sensor gain
+#define CSA_GAIN_10         0x1
+#define CSA_GAIN_20         0x2
+#define CSA_GAIN_40         0x3
+#define DIS_SEN_EN          0x0     /// Overcurrent Fault
+#define DIS_SEN_DIS         0x1
+#define SEN_LVL_0_25        0x0     /// Sense OCP voltage level
+#define SEN_LVL_0_5         0x1
+#define SEN_LVL_0_75        0x2
+#define SEN_LVL_1_0         0x3
 
 class DRV832x {
     public:

--- a/FOC/foc.cpp	Wed Jun 27 03:44:44 2018 +0000
+++ b/FOC/foc.cpp	Thu Jul 12 02:50:34 2018 +0000
@@ -142,19 +142,19 @@
        float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current;
        
        // Calculate feed-forward voltages //
-       float v_d_ff = 2.0f*(controller->i_d_ref*R_PHASE + L_D*controller->did_dt - controller->dtheta_elec*L_Q*controller->i_q_ref);   //feed-forward voltages
-       float v_q_ff =  2.0f*(controller->i_q_ref*R_PHASE + L_Q*controller->diq_dt + controller->dtheta_elec*(L_D*controller->i_d_ref + WB));
+       float v_d_ff = 2.0f*(0.0f*controller->i_d_ref*R_PHASE + 0.0f*L_D*controller->did_dt - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
+       float v_q_ff =  2.0f*(0.0f*controller->i_q_ref*R_PHASE + 0.0f*L_Q*controller->diq_dt + controller->dtheta_elec*(L_D*controller->i_d + 0.0f*WB));
        
        // Integrate Error //
        controller->d_int += i_d_error;   
        controller->q_int += i_q_error;
 
        limit_norm(&controller->d_int, &controller->q_int, V_BUS/(controller->k_q*controller->ki_q));        
-       controller->v_d = controller->k_d*(i_d_error + controller->ki_d*controller->d_int);// + v_d_ff;  
-       controller->v_q = controller->k_q*(i_q_error + controller->ki_q*controller->q_int);// + v_q_ff; 
+       controller->v_d = controller->k_d*(i_d_error + controller->ki_d*controller->d_int) + v_d_ff;  
+       controller->v_q = controller->k_q*(i_q_error + controller->ki_q*controller->q_int) + v_q_ff; 
        
        limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
-       abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
+       abc(controller->theta_elec + 0.5f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
 
        observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
@@ -184,7 +184,7 @@
            //printf("%.2f  %.2f  %.2f\n\r", controller->i_a, controller->i_b, controller->i_c);
            //printf("%f\n\r", controller->dtheta_mech*GR);
            //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
-           //printf("%f    %f\n\r", controller->k_d, controller->k_q);
+           printf("%f %f\n\r", v_q_ff, v_d_ff);
            //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
             }
     }

--- a/PositionSensor/PositionSensor.cpp	Wed Jun 27 03:44:44 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Thu Jul 12 02:50:34 2018 +0000
@@ -1,6 +1,7 @@
 
 #include "mbed.h"
 #include "PositionSensor.h"
+#include "math_ops.h"
 //#include "offset_lut.h"
 //#include <math.h>
 
@@ -25,8 +26,10 @@
     }
     
 void PositionSensorAM5147::Sample(){
+    GPIOA->ODR &= ~(1 << 15);
     raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;                                                              //Extract last 14 bits
+    GPIOA->ODR |= (1 << 15);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];
     int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries
@@ -40,20 +43,22 @@
     
     old_counts = angle;
     oldModPosition = modPosition;
-    modPosition = ((6.28318530718f * ((float) angle))/ (float)_CPR);
-    position = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+    modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
+    position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
     MechPosition = position - MechOffset;
-    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
-    if(elec < 0) elec += 6.28318530718f;
-    else if(elec > 6.28318530718f) elec -= 6.28318530718f ; 
+    float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
+    if(elec < 0) elec += 2.0f*PI;
+    else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 
     ElecPosition = elec;
     
     float vel;
-    if(modPosition<.1f && oldModPosition>6.1f){
-        vel = (modPosition - oldModPosition + 6.28318530718f)*40000.0f;
+    //if(modPosition<.1f && oldModPosition>6.1f){
+    if((modPosition-oldModPosition) < -3.0f){
+        vel = (modPosition - oldModPosition + 2.0f*PI)*40000.0f;
         }
-    else if(modPosition>6.1f && oldModPosition<0.1f){
-        vel = (modPosition - oldModPosition - 6.28318530718f)*40000.0f;
+    //else if(modPosition>6.1f && oldModPosition<0.1f){
+    else if((modPosition - oldModPosition) > 3.0f){
+        vel = (modPosition - oldModPosition - 2.0f*PI)*40000.0f;
         }
     else{
         vel = (modPosition-oldModPosition)*40000.0f;
@@ -66,7 +71,7 @@
         sum += velVec[n-i];
         }
     velVec[0] = vel;
-    MechVelocity =  sum/(float)n;
+    MechVelocity =  sum/((float)n);
     ElecVelocity = MechVelocity*_ppairs;
     ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
     }

--- a/main.cpp	Wed Jun 27 03:44:44 2018 +0000
+++ b/main.cpp	Thu Jul 12 02:50:34 2018 +0000
@@ -214,21 +214,24 @@
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
-                controller.i_q_ref = 2.0f;
+                //controller.i_q_ref = 2.0f;
+                //controller.i_d_ref = 30.0f;
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
                 //TIM1->CCR3 = (PWM_ARR)*(0.5f);                        // Write duty cycles
                 //TIM1->CCR2 = (PWM_ARR)*(0.5f);
                 //TIM1->CCR1 = (PWM_ARR)*(0.5f);
                 controller.timeout += 1;
                 
+                
+                count++;
                 /*
-                count++;
                 if(count == 4000){
-                     printf("%d  %d  %.4f  %.4f  %.4f\n\r", controller.adc1_raw, controller.adc2_raw, controller.i_a, controller.i_b, controller.i_c);
-                     //printf("%.3f\n\r", controller.dtheta_mech);
+                     //printf("%d  %d  %.4f  %.4f  %.4f\n\r", controller.adc1_raw, controller.adc2_raw, controller.i_a, controller.i_b, controller.i_c);
+                     printf("%.3f\n\r", controller.dtheta_mech);
                      count = 0;
                      }
                      */
+                     
                    
                      
             
@@ -365,9 +368,9 @@
     wait_us(100);
     drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
     wait_us(100);
-    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, 0x3);
+    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
     wait_us(100);
-    
+    drv.write_OCPCR(TRETRY_4MS, DEADTIME_100NS, OCP_RETRY, OCP_DEG_4US, VDS_LVL_0_75);
     
     //drv.enable_gd();
     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
@@ -426,7 +429,8 @@
     printf(" %d\n\r", drv.read_register(DCR));
     wait_us(100);
     printf(" %d\n\r", drv.read_register(CSACR));
-    
+    wait_us(100);
+    printf(" %d\n\r", drv.read_register(OCPCR));
     drv.disable_gd();
     
     pc.attach(&serial_interrupt);                                               // attach serial interrupt