Revision 48:74a40481740c, committed 2019-03-03

Comitter:

benkatz

Date:

Sun Mar 03 02:51:51 2019 +0000

Parent:

47:e1196a851f76

Child:

49:83d83040ea51

Commit message:

Working on the new hardware

Changed in this revision

--- a/Config/current_controller_config.h	Wed Dec 05 04:07:46 2018 +0000
+++ b/Config/current_controller_config.h	Sun Mar 03 02:51:51 2019 +0000
@@ -7,14 +7,15 @@
 #define K_SCALE 0.0001f             // K_loop/Loop BW (Hz) 0.0042
 #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 35.0f                 // Volts
-#define OVERMODULATION 1.15f         // 1.0 = no overmodulation
+#define V_BUS 24.0f                 // Volts
+#define OVERMODULATION 1.15f        // 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
 
-#define I_MAX 40.0f
-#define I_MAX_FW 0.0f
+#define I_MAX 40.0f                 // Max Current
+#define I_MAX_FW 0.0f               // Max field weakening current
+#define I_MAX_CONT 15.0f            // Max continuous current, for thermal limiting
 
 //Observer//
 #define DT 0.000025f

--- a/Config/hw_config.h	Wed Dec 05 04:07:46 2018 +0000
+++ b/Config/hw_config.h	Sun Mar 03 02:51:51 2019 +0000
@@ -7,11 +7,12 @@
 #define ENABLE_PIN PA_11        // Enable gate drive pin
 #define LED         PC_5        // LED Pin
 #define I_SCALE 0.02014160156f  // Amps per A/D Count
-#define V_SCALE 0.00886f        // Bus volts per A/D Count
+#define V_SCALE 0.012890625f     // Bus volts per A/D Count
 #define DTC_MAX 0.94f          // Max phase duty cycle
 #define DTC_MIN 0.0f          // Min phase duty cycle
 #define PWM_ARR 0x8CA           /// timer autoreload value
 
+static float inverter_tab[16] = {2.5f, 2.4f, 2.3f, 2.2f, 2.1f, 2.0f, 1.9f, 1.8f, 1.7f, 1.6f, 1.59f, 1.58f, 1.57f, 1.56f, 1.55f, 1.5f};
 
 
 #endif

--- a/Config/motor_config.h	Wed Dec 05 04:07:46 2018 +0000
+++ b/Config/motor_config.h	Sun Mar 03 02:51:51 2019 +0000
@@ -1,14 +1,16 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.02f          //Ohms
+#define R_PHASE 0.13f           //Ohms
 #define L_D 0.00002f            //Henries
 #define L_Q 0.00002f            //Henries
 #define KT .08f                 //N-m per peak phase amp, = WB*NPP*3/2
 #define NPP 21                  //Number of pole pairs
 #define GR 1.0f                 //Gear ratio
 #define KT_OUT 0.45f            //KT*GR
-#define WB 0.0024f               //Flux linkage, Webers.  
+#define WB 0.0025f              //Flux linkage, Webers.  
+#define R_TH 1.25f              //Kelvin per watt
+#define INV_M_TH 0.03125f       //Kelvin per joule
 
 
 

--- a/FOC/foc.cpp	Wed Dec 05 04:07:46 2018 +0000
+++ b/FOC/foc.cpp	Sun Mar 03 02:51:51 2019 +0000
@@ -33,7 +33,7 @@
     /// Space Vector Modulation ///
     /// u,v,w amplitude = v_bus for full modulation depth ///
     
-    float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
+    float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))*0.5f;
     
     *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
     *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
@@ -49,7 +49,19 @@
     
     }
 
-
+void linearize_dtc(float *dtc){
+    /// linearizes the output of the inverter, which is not linear for small duty cycles ///
+    float sgn = 1.0f-(2.0f*(dtc<0));
+    if(abs(*dtc) >= .01f){
+        *dtc = *dtc*.986f+.014f*sgn;
+        }
+    else{
+        *dtc = 2.5f*(*dtc);
+        }
+    
+    }
+    
+    
 void zero_current(int *offset_1, int *offset_2){                                // Measure zero-offset of the current sensors
     int adc1_offset = 0;
     int adc2_offset = 0;
@@ -92,6 +104,11 @@
 
     }
     
+void reset_observer(ObserverStruct *observer){
+    observer->temperature = 25.0f;
+    observer->resistance = .1f;
+    }
+    
 void limit_current_ref (ControllerStruct *controller){
     float i_q_max_limit = (0.5774f*controller->v_bus - controller->dtheta_elec*WB)/R_PHASE;
     float i_q_min_limit = (-0.5774f*controller->v_bus - controller->dtheta_elec*WB)/R_PHASE;
@@ -99,7 +116,24 @@
     }
 
 
-void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
+void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
+        
+        /// Update observer estimates ///
+        // Resistance observer //
+        // Temperature Observer //
+        float t_rise = (float)observer->temperature - 25.0f;
+        float q_th_in = (1.0f + .00393f*t_rise)*(controller->i_d*controller->i_d*R_PHASE*1.5f + controller->i_q*controller->i_q*R_PHASE*1.5f);
+        float q_th_out = t_rise*R_TH;
+        observer->temperature += INV_M_TH*DT*(q_th_in-q_th_out);
+        
+        //observer->resistance = (controller->v_q - 2.0f*controller->dtheta_elec*(WB + L_D*controller->i_d))/controller->i_q;
+        observer->resistance = controller->v_q/controller->i_q;
+        if(isnan(observer->resistance)){observer->resistance = R_PHASE;}
+        observer->temperature2 = (double)(25.0f + ((observer->resistance*6.0606f)-1.0f)*275.5f);
+        double e = observer->temperature - observer->temperature2;
+        observer->temperature -= .001*e;
+        //printf("%.3f\n\r", e);
+        
 
        /// Commutation Loop ///
        controller->loop_count ++;   
@@ -124,32 +158,27 @@
         
         
         // Filter the current references to the desired closed-loopbandwidth
-        // Allows calculation of desired di/dt for inductance, etc
-        controller->did_dt = controller->i_d_ref_filt;
-        controller->diq_dt = controller->i_q_ref_filt;
         controller->i_d_ref_filt = (1.0f-controller->alpha)*controller->i_d_ref_filt + controller->alpha*controller->i_d_ref;
         controller->i_q_ref_filt = (1.0f-controller->alpha)*controller->i_q_ref_filt + controller->alpha*controller->i_q_ref;
-        controller->did_dt = (controller->i_d_ref_filt - controller->did_dt)/DT;
-        controller->diq_dt = (controller->i_q_ref_filt - controller->diq_dt)/DT;
+
+       
+       /// Field Weakening ///
+       
+       controller->fw_int += .001f*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
+       controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_MAX_FW);
+       controller->i_d_ref = controller->fw_int;
+       //float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
+       limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);
        
        
-       /// Field Weakening ///
-       /*
-       controller->fw_int += .001*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
-       controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_MAX_FW);
-       controller->i_d_ref = controller->fw_int;
-       float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
-       limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);
-       
-       */
        
        /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
        float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current;
        
        // Calculate feed-forward voltages //
-       float v_d_ff = 2.0f*(1.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
-       float v_q_ff =  2.0f*(1.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB));
+       float v_d_ff = SQRT3*(1.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
+       float v_q_ff =  SQRT3*(1.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB));
        
        // Integrate Error //
        controller->d_int += controller->k_d*controller->ki_d*i_d_error;   
@@ -165,6 +194,12 @@
        controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q);
        
        limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
+       float dtc_d = controller->v_d/controller->v_bus;
+       float dtc_q = controller->v_q/controller->v_bus;
+       linearize_dtc(&dtc_d);
+       linearize_dtc(&dtc_q);
+       controller->v_d = dtc_d*controller->v_bus;
+       controller->v_q = dtc_q*controller->v_bus;
        abc(controller->theta_elec + 0.0f*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
 
@@ -179,7 +214,7 @@
             TIM1->CCR2 =  (PWM_ARR)*(1.0f-controller->dtc_w);
         }
 
-       controller->theta_elec = theta;                                          //For some reason putting this at the front breaks thins
+       controller->theta_elec = theta;                                          
        
     }
     

--- a/FOC/foc.h	Wed Dec 05 04:07:46 2018 +0000
+++ b/FOC/foc.h	Sun Mar 03 02:51:51 2019 +0000
@@ -17,8 +17,10 @@
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w);
 void zero_current(int *offset_1, int *offset_2);
 void reset_foc(ControllerStruct *controller);
+void reset_observer(ObserverStruct *observer);
 void init_controller_params(ControllerStruct *controller);
-void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta);
+void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta);
 void torque_control(ControllerStruct *controller);
 void limit_current_ref (ControllerStruct *controller);
+void linearize_dtc(float *dtc);
 #endif

--- a/PositionSensor/PositionSensor.cpp	Wed Dec 05 04:07:46 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sun Mar 03 02:51:51 2019 +0000
@@ -30,7 +30,7 @@
     //raw = spi->write(readAngleCmd);
     //raw &= 0x3FFF;   
     raw = spi->write(0);
-    raw = raw>>1;                                                             //Extract last 14 bits
+    raw = raw>>2;                                                             //Extract last 14 bits
     GPIOA->ODR |= (1 << 15);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];

--- a/main.cpp	Wed Dec 05 04:07:46 2018 +0000
+++ b/main.cpp	Sun Mar 03 02:51:51 2019 +0000
@@ -37,6 +37,7 @@
 
 GPIOStruct gpio;
 ControllerStruct controller;
+ObserverStruct observer;
 COMStruct com;
 Serial pc(PA_2, PA_3);
 
@@ -154,8 +155,8 @@
     }
     
 void print_encoder(void){
-    //printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
-    printf("%d\n\r", spi.GetRawPosition());
+    printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+    //printf("%d\n\r", spi.GetRawPosition());
     wait(.001);
     }
 
@@ -177,7 +178,7 @@
         controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
         controller.dtheta_elec = spi.GetElecVelocity();
-        controller.v_bus = V_BUS;//b0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
         ///
         
         /// Check state machine state, and run the appropriate function ///
@@ -224,16 +225,24 @@
                 
                 //controller.i_q_ref = 2.0f;
                 //controller.i_d_ref = -30.0f;
-                //controller.kd = .1;
+                //controller.kp = 1;
+                //controller.kd = .1f;
                 //controller.v_des = 25;
-                torque_control(&controller);
-                //controller.i_q_ref = 2.0f;
-                //controller.i_d_ref = -20.0f;
-                commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
+                //torque_control(&controller);
+                controller.i_q_ref = 40.0f;
+                //
+                //controller.i_q_ref += .00025f;
+                //if(count>80000)
+                //{
+                //    controller.i_q_ref = 0.0f;
+                //    count = 0;
+                //    }    
+                //controller.i_d_ref = -10.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;
+                controller.timeout++;
 
                 //pc.putc(iq);
                 //pc.putc(id);
@@ -244,15 +253,21 @@
                 count++;
                 
                 /*
-                if(count == 400){
+                if(count == 10000){
                      //printf("%d  %d  %.4f  %.4f  %.4f\n\r", controller.adc1_raw, controller.adc2_raw, controller.i_a, controller.i_b, controller.i_c);
-                     printf("%.2f\n\r", 1000.0f*controller.h1_iq_afc_int1);
+                     //printf("%.2f\n\r", 1000.0f*controller.h1_iq_afc_int1);
                      //pc.putc(id);
                      //pc.putc(iq);
                      //pc.putc(ang);
                      //pc.putc('\n');
                      //pc.putc('\r');
                      //printf("%.1f\n", controller.k_q*controller.ki_q*controller.q_int);
+                     if(controller.i_d_ref< 10.0f)
+                     {
+                        controller.i_d_ref += .05f;
+                    }
+                    else{controller.i_d_ref = 0;}
+                    
                      count = 0;
                      }
                      */
@@ -415,6 +430,7 @@
     gpio.enable->write(0);
     */
     reset_foc(&controller);                                                     // Reset current controller
+    reset_observer(&observer);                                                 // Reset observer
     TIM1->CR1 ^= TIM_CR1_UDIS;
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
@@ -429,9 +445,15 @@
     rxMsg.len = 8;
     can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler    
     
+    // If preferences haven't been user configured yet, set defaults 
     prefs.load();                                                               // Read flash
     if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
     if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
+    if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
+    if(isnan(TORQUE_LIMIT) || TORQUE_LIMIT ==-1){TORQUE_LIMIT=18;}
+    if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
+    if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
+    if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;}
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
     int lut[128] = {0};
@@ -463,16 +485,27 @@
     pc.attach(&serial_interrupt);                                               // attach serial interrupt
     
     state_change = 1;
-    
+    /*
+    for(int i = 0; i< 1000; i++){
+        float dtc_in = .001f*(float)i;
+        printf("%f  ", dtc_in);
+        linearize_dtc(&dtc_in);
+        printf("%f\n\r", dtc_in);
+        wait(.001);
+        }
+    */
 
     int counter = 0;
     while(1) {
         drv.print_faults();
-       wait(.1);
+       wait(.001);
+       //printf("%.4f\n\r", controller.v_bus);
         if(state == MOTOR_MODE)
         {
-            //printf("%.3f\n\r", controller.dtheta_mech);
-            //wait(.001);
+            //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
+            //printf("%.3f  %.3f  %.3f\n\r", controller.v_q/controller.v_bus, controller.i_q, observer.resistance);
+            printf("%.3f\n\r", controller.dtheta_mech);
+            wait(.001);
         }
 
     }

--- a/math_ops.h	Wed Dec 05 04:07:46 2018 +0000
+++ b/math_ops.h	Sun Mar 03 02:51:51 2019 +0000
@@ -2,6 +2,7 @@
 #define MATH_OPS_H
 
 #define PI 3.14159265359f
+#define SQRT3 1.73205080757f
 
 #include "math.h"
 

--- a/structs.h	Wed Dec 05 04:07:46 2018 +0000
+++ b/structs.h	Sun Mar 03 02:51:51 2019 +0000
@@ -29,7 +29,6 @@
     float d_int, q_int;                                     // Current error integrals
     int adc1_offset, adc2_offset;                           // ADC offsets
     float i_d_ref, i_q_ref, i_d_ref_filt, i_q_ref_filt;     // Current references
-    float did_dt, diq_dt;                                   // Current reference derivatives
     int loop_count;                                         // Degubbing counter
     int timeout;                                            // Watchdog counter
     int mode;
@@ -40,13 +39,8 @@
     } ControllerStruct;
 
 typedef struct{
-    float h;
-    float k;
-    float phi;
-    float cos_integral;
-    float sin_integral;
-    float afc_out;
-    } AFCStruct;
-
-    
+    double temperature;                                              // Estimated temperature
+    double temperature2;
+    float resistance;
+    }   ObserverStruct;
 #endif