Revision 55:c4c9fec8539c, committed 2019-10-04

Comitter:

benkatz

Date:

Fri Oct 04 14:18:39 2019 +0000

Parent:

54:59575833d16f

Child:

56:fe5056ac6740

Commit message:

new mini cheetah first revision;

Changed in this revision

--- a/CAN/CAN_com.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/CAN/CAN_com.cpp	Fri Oct 04 14:18:39 2019 +0000
@@ -1,10 +1,10 @@
 #include "CAN_com.h"
 
 
- #define P_MIN -95.5f
- #define P_MAX 95.5f
- #define V_MIN -45.0f
- #define V_MAX 45.0f
+ #define P_MIN -12.5f
+ #define P_MAX 12.5f
+ #define V_MIN -65.0f
+ #define V_MAX 65.0f
  #define KP_MIN 0.0f
  #define KP_MAX 500.0f
  #define KD_MIN 0.0f

--- a/Calibration/calibration.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/Calibration/calibration.cpp	Fri Oct 04 14:18:39 2019 +0000
@@ -24,7 +24,7 @@
     
     ///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(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w);                            //space vector modulation
     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);
@@ -46,7 +46,7 @@
     /// 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
+        svm(1.0, v_u, v_v, v_w, 0, &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);
@@ -109,7 +109,7 @@
         
     ///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(1.0, v_u, v_v, v_w, 0, &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
         if(PHASE_ORDER){                                   
@@ -133,7 +133,7 @@
        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
+       svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
         if(PHASE_ORDER){                                                        // Check phase ordering
             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);                                    // Set duty cycles
@@ -158,7 +158,7 @@
        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
+       svm(1.0, v_u, v_v, v_w, 0, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
         TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
         if(PHASE_ORDER){
             TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
@@ -232,8 +232,12 @@
             
         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
+        
+        memcpy(&ENCODER_LUT, lut, 128*4);                                 // copy the lookup table to the flash array
         printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
+        //for(int i = 0; i<128; i++){printf("%d\n\r", __int_reg[i]);}
+        //printf("\n\r %d \n\r", sizeof(lut));
+
         
         if (!prefs->ready()) prefs->open();
         prefs->flush();                                                         // write offset and lookup table to flash

--- a/Config/current_controller_config.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/Config/current_controller_config.h	Fri Oct 04 14:18:39 2019 +0000
@@ -5,8 +5,8 @@
 #define K_D .05f                    // Loop gain,  Volts/Amp
 #define K_Q .05f                    // Loop gain,  Volts/Amp
 #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 KI_D 0.0455f                // PI zero, in radians per sample
+#define KI_Q 0.0455f                // PI zero, in radians per sample
 #define V_BUS 24.0f                 // Volts
 #define OVERMODULATION 1.15f        // 1.0 = no overmodulation
 

--- a/Config/hw_config.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/Config/hw_config.h	Fri Oct 04 14:18:39 2019 +0000
@@ -11,8 +11,8 @@
 #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
+#define DTC_COMP .000f          /// deadtime compensation (100 ns / 25 us)
 
-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	Thu Aug 08 17:39:43 2019 +0000
+++ b/Config/motor_config.h	Fri Oct 04 14:18:39 2019 +0000
@@ -2,15 +2,16 @@
 #define MOTOR_CONFIG_H
 
 #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 L_D 0.00008f            //Henries
+#define L_Q 0.00008f            //Henries
+#define KT .075f                 //N-m per peak phase amp, = WB*NPP*3/2
 #define NPP 21                  //Number of pole pairs
 #define GR 6.0f                 //Gear ratio
 #define KT_OUT 0.45f            //KT*GR
-#define WB 0.0025f              //Flux linkage, Webers.  
+#define WB 0.00287f              //Flux linkage, Webers.  
 #define R_TH 1.25f              //Kelvin per watt
-#define INV_M_TH 0.03125f       //Kelvin per joule
+#define INV_M_TH 0.02825f       //Kelvin per joule
+#define T_AMBIENT 25.0f        // ambient temperature during temp calibration
 
 
 

--- a/Config/user_config.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/Config/user_config.h	Fri Oct 04 14:18:39 2019 +0000
@@ -11,7 +11,9 @@
 #define THETA_MIN               __float_reg[4]                                  // Minimum position setpoint
 #define THETA_MAX               __float_reg[5]                                  // Maximum position setpoint
 #define I_FW_MAX                __float_reg[6]                                  // Maximum field weakening current
-
+#define R_NOMINAL               __float_reg[7]                                  // Nominal motor resistance, set during calibration
+#define TEMP_MAX                __float_reg[8]                                  // Temperature safety lmit
+#define I_MAX_CONT              __float_reg[9]                                  // Continuous max current
 
 #define PHASE_ORDER             __int_reg[0]                                    // Phase swapping during calibration
 #define CAN_ID                  __int_reg[1]                                    // CAN bus ID

--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Fri Oct 04 14:18:39 2019 +0000
@@ -29,15 +29,21 @@
        
     }
     
-void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
+void svm(float v_bus, float u, float v, float w, int i_sector, float *dtc_u, float *dtc_v, float *dtc_w){
     /// Space Vector Modulation ///
     /// u,v,w amplitude = v_bus for full modulation depth ///
     
     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);
-    *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 
+    // Dead-time compensation
+    float u_comp = DTC_COMP*(-(i_sector==4) + (i_sector==3));
+    float v_comp = DTC_COMP*(-(i_sector==2) + (i_sector==5));
+    float w_comp = DTC_COMP*((i_sector==6) - (i_sector==1));
+    
+    
+    *dtc_u = fminf(fmaxf((.5f*(u -v_offset)/(v_bus*(DTC_MAX-DTC_MIN)) + (DTC_MAX+DTC_MIN)*.5f + u_comp), DTC_MIN), DTC_MAX);
+    *dtc_v = fminf(fmaxf((.5f*(v -v_offset)/(v_bus*(DTC_MAX-DTC_MIN)) + (DTC_MAX+DTC_MIN)*.5f + v_comp), DTC_MIN), DTC_MAX);
+    *dtc_w = fminf(fmaxf((.5f*(w -v_offset)/(v_bus*(DTC_MAX-DTC_MIN)) + (DTC_MAX+DTC_MIN)*.5f + w_comp), DTC_MIN), DTC_MAX); 
     
     /*
     sinusoidal pwm
@@ -49,19 +55,6 @@
     
     }
 
-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;
@@ -85,7 +78,10 @@
     controller->k_d = K_SCALE*I_BW;
     controller->k_q = K_SCALE*I_BW;
     controller->alpha = 1.0f - 1.0f/(1.0f - DT*I_BW*2.0f*PI);
-    
+    for(int i = 0; i<128; i++)
+    {
+        controller->inverter_tab[i] = 1.0f + 1.2f*exp(-0.0078125f*i/.032f);
+    }
     }
 
 void reset_foc(ControllerStruct *controller){
@@ -101,12 +97,15 @@
     controller->d_int = 0;
     controller->v_q = 0;
     controller->v_d = 0;
+    controller->otw_flag = 0;
 
     }
     
 void reset_observer(ObserverStruct *observer){
+    
     observer->temperature = 25.0f;
-    observer->resistance = .1f;
+    observer->temp_measured = 25.0f;
+    //observer->resistance = .1f;
     }
     
 void limit_current_ref (ControllerStruct *controller){
@@ -115,26 +114,59 @@
     controller->i_q_ref = fmaxf(fminf(i_q_max_limit, controller->i_q_ref), i_q_min_limit);
     }
 
+void update_observer(ControllerStruct *controller, ObserverStruct *observer)
+{
+    /// Update observer estimates ///
+    // Resistance observer //
+    // Temperature Observer //
+    observer->delta_t = (float)observer->temperature - T_AMBIENT;
+    float i_sq = controller->i_d*controller->i_d + controller->i_q*controller->i_q;
+    observer->q_in = (R_NOMINAL*1.5f)*(1.0f + .00393f*observer->delta_t)*i_sq;
+    observer->q_out = observer->delta_t*R_TH;
+    observer->temperature += (INV_M_TH*DT)*(observer->q_in-observer->q_out);
+    
+    //float r_d = (controller->v_d*(DTC_MAX-DTC_MIN) + SQRT3*controller->dtheta_elec*(L_Q*controller->i_q))/(controller->i_d*SQRT3);
+    float r_q = (controller->v_q*(DTC_MAX-DTC_MIN) - SQRT3*controller->dtheta_elec*(L_D*controller->i_d + WB))/(controller->i_q*SQRT3);
+    observer->resistance = r_q;//(r_d*controller->i_d + r_q*controller->i_q)/(controller->i_d + controller->i_q); // voltages more accurate at higher duty cycles
+    
+    //observer->resistance = controller->v_q/controller->i_q;
+    if(isnan(observer->resistance) || isinf(observer->resistance)){observer->resistance = R_NOMINAL;}
+    float t_raw = ((T_AMBIENT + ((observer->resistance/R_NOMINAL) - 1.0f)*254.5f));
+    if(t_raw > 200.0f){t_raw = 200.0f;}
+    else if(t_raw < 0.0f){t_raw = 0.0f;}
+    observer->temp_measured = .999f*observer->temp_measured + .001f*t_raw;
+    float e = (float)observer->temperature - observer->temp_measured;
+    observer->trust = (1.0f - .004f*fminf(abs(controller->dtheta_elec), 250.0f)) * (.01f*(fminf(i_sq, 100.0f)));
+    observer->temperature -= observer->trust*.0001f*e;
+    //printf("%.3f\n\r", e);
+    
+    if(observer->temperature > TEMP_MAX){controller->otw_flag = 1;}
+    else{controller->otw_flag = 0;}
+}
 
-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*SQRT3 + controller->i_q*controller->i_q*R_PHASE*SQRT3);
-        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 - SQRT3*controller->dtheta_elec*(WB))/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);
-        
+float linearize_dtc(ControllerStruct *controller, float dtc)
+{
+    float duty = fmaxf(fminf(abs(dtc), .999f), 0.0f);;
+    int index = (int) (duty*127.0f);
+    float val1 = controller->inverter_tab[index];
+    float val2 = controller->inverter_tab[index+1];
+    return val1 + (val2 - val1)*(duty*128.0f - (float)index);
+}
 
+void field_weaken(ControllerStruct *controller)
+{
+       /// 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_FW_MAX);
+       controller->i_d_ref = controller->fw_int;
+       float q_max = sqrt(controller->i_max*controller->i_max - controller->i_d_ref*controller->i_d_ref);
+       controller->i_q_ref = fmaxf(fminf(controller->i_q_ref, q_max), -q_max);
+       //float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
+       
+}
+void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta)
+{
        /// Commutation Loop ///
        controller->loop_count ++;   
        if(PHASE_ORDER){                                                                          // Check current sensor ordering
@@ -146,6 +178,7 @@
            controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
            }
        controller->i_a = -controller->i_b - controller->i_c;       
+       if((abs(controller->i_b) > 41.0f)|(abs(controller->i_c) > 41.0f)|(abs(controller->i_a) > 41.0f)){controller->oc_flag = 1;}
        
        float s = FastSin(theta); 
        float c = FastCos(theta);                            
@@ -158,27 +191,31 @@
         
         
         // Filter the current references to the desired closed-loop bandwidth
-        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->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->i_max = I_MAX*(!controller->otw_flag) + I_MAX_CONT*controller->otw_flag;
+        
+        // Temperature Controller //
+        /*
+        if(observer->temperature > TEMP_MAX)
+        {
+            float qdot_des = 1.0f*(TEMP_MAX - observer->temperature);
+            float i_limit = sqrt((qdot_des + observer->q_out)/(R_NOMINAL*1.5f));
+            controller->i_max = fmaxf(fminf(i_limit, I_MAX), I_MAX_CONT);
+        }
+        else{controller->i_max = I_MAX;}
+        */
+        
+        limit_norm(&controller->i_d_ref, &controller->i_q_ref, controller->i_max);
 
-       
-       /// 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_FW_MAX);
-       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 = 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));
+       float v_d_ff = SQRT3*(0.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
+       float v_q_ff =  SQRT3*(0.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 0.0f*WB));
        
        // Integrate Error //
        controller->d_int += controller->k_d*controller->ki_d*i_d_error;   
@@ -188,20 +225,28 @@
        controller->q_int = fmaxf(fminf(controller->q_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus); 
        
        //limit_norm(&controller->d_int, &controller->q_int, OVERMODULATION*controller->v_bus);     
-       controller->v_d = controller->k_d*i_d_error + controller->d_int ;//+ v_d_ff;  
-       controller->v_q = controller->k_q*i_q_error + controller->q_int ;//+ v_q_ff; 
-       
+       controller->v_d = controller->k_d*i_d_error + controller->d_int;// + v_d_ff;  
+       controller->v_q = controller->k_q*i_q_error + controller->q_int;// + v_q_ff; 
+       //controller->v_q = 0.0f;
+       //controller->v_d = 1.0f*controller->v_bus;
        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 = controller->v_ref/controller->v_bus;
+       float scale = linearize_dtc(controller, dtc);
+       //controller->v_d = scale*controller->v_d;
+       //controller->v_q = scale*controller->v_q;
        //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
+       abc(controller->theta_elec + 0.0f*DT*controller->dtheta_elec, scale*controller->v_d, scale*controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
+       controller->current_sector = ((controller->i_a>0)<<2)|((controller->i_b>0)<<1)|(controller->i_c>0);
+       svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, controller->current_sector, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
+       
+       
+        
 
        if(PHASE_ORDER){                                                         // Check which phase order to use, 
             TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles

--- a/FOC/foc.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.h	Fri Oct 04 14:18:39 2019 +0000
@@ -14,7 +14,7 @@
 
 void abc(float theta, float d, float q, float *a, float *b, float *c);
 void dq0(float theta, float a, float b, float c, float *d, float *q);
-void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w);
+void svm(float v_bus, float u, float v, float w, int current_sector, 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);
@@ -22,5 +22,7 @@
 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);
+void update_observer(ControllerStruct *controller, ObserverStruct *observer);
+void field_weaken(ControllerStruct *controller);
+float linearize_dtc(ControllerStruct *controller, float dtc);
 #endif

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Fri Oct 04 14:18:39 2019 +0000
@@ -4,13 +4,14 @@
 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
 /// Version for the TI DRV8323 Everything Chip
 
-#define REST_MODE 0
-#define CALIBRATION_MODE 1
-#define MOTOR_MODE 2
-#define SETUP_MODE 4
-#define ENCODER_MODE 5
+#define REST_MODE           0
+#define CALIBRATION_MODE    1
+#define MOTOR_MODE          2
+#define SETUP_MODE          4
+#define ENCODER_MODE        5
+#define INIT_TEMP_MODE      6
 
-#define VERSION_NUM "1.9"
+#define VERSION_NUM "1.10"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
@@ -19,7 +20,7 @@
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
-#include "foc.h"
+#include "foc.h" 
 #include "calibration.h"
 #include "hw_setup.h"
 #include "math_ops.h" 
@@ -87,6 +88,7 @@
 
 void enter_menu_state(void){
     drv.disable_gd();
+    reset_foc(&controller); 
     //gpio.enable->write(0);
     printf("\n\r\n\r\n\r");
     printf(" Commands:\n\r");
@@ -124,6 +126,10 @@
     wait_us(10);
     printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
     wait_us(10);
+    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "h", "Temp Cutoff (C) (0 = none)", "0", "150", TEMP_MAX);
+    wait_us(10);
+    printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "c", "Continuous Current (A)", "0", "40.0", I_MAX_CONT);
+    wait_us(10);
     printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
     wait_us(10);
     state_change = 0;
@@ -149,11 +155,13 @@
     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
     gpio.led->write(0);;                                                     // Turn off status LED
-    wait(.2);
-    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
-    drv.disable_gd();
-    //gpio.enable->write(0);
-     state_change = 0;
+    wait(.05);
+    R_NOMINAL = 0;
+    state = INIT_TEMP_MODE;
+    //printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
+    //drv.disable_gd();
+    //state_change = 0;
+     
     }
     
 void print_encoder(void){
@@ -164,14 +172,16 @@
 
 /// Current Sampling Interrupt ///
 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
+//float testing[1000];
+//float testing2[1000];
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
   if (TIM1->SR & TIM_SR_UIF ) {
-
+        //gpio.led->write(1);
         ///Sample current always ///
         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
         //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
-
+        
         spi.Sample(DT);                                                           // sample position sensor
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
@@ -181,6 +191,9 @@
         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
         controller.dtheta_elec = spi.GetElecVelocity();
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
+        
+
+        
         ///
         
         /// Check state machine state, and run the appropriate function ///
@@ -189,6 +202,7 @@
                 if(state_change){
                     enter_menu_state();
                     }
+                update_observer(&controller, &observer);
                 break;
             
             case CALIBRATION_MODE:                                              // Run encoder calibration procedure
@@ -196,7 +210,45 @@
                     calibrate();
                     }
                 break;
-             
+            case INIT_TEMP_MODE:
+                if(state_change){
+                    enter_torque_mode();
+                    count = 0;
+                    observer.resistance = 0.0f;
+                    } 
+                controller.i_d_ref = -10.0f;
+                controller.i_q_ref = 0.0f;
+                commutate(&controller, &observer, &gpio, controller.theta_elec); 
+
+                if(count > 200)
+                {
+                    float r_meas = controller.v_d*(DTC_MAX-DTC_MIN)/(controller.i_d*SQRT3);
+                    //testing2[count-100] = controller.i_d;
+                    observer.resistance += .001f*r_meas;
+                }
+                if(count > 1200)
+                {
+                    count = 0;
+                    state = REST_MODE;
+                    state_change = 1;
+                    gpio.led->write(0);
+                    observer.temperature = (double)(T_AMBIENT + ((observer.resistance/R_NOMINAL) - 1.0f)*254.5f);
+                    printf("Winding Resistance:  %f\n\r", observer.resistance);
+                    printf("Winding Temperature:  %f\n\r", observer.temperature);
+                    
+                    if(R_NOMINAL==0)
+                    {
+                        printf("Saving winding resistance\n\r");
+                        R_NOMINAL = observer.resistance;
+                        if (!prefs.ready()) prefs.open();
+                        prefs.flush();                                                         // write offset and lookup table to flash
+                        prefs.close();
+                    }
+                    //for(int i = 0; i<1000; i++){printf("%f \n\r", testing[i]);}
+                }
+                
+                count++; 
+                break;
             case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
                     enter_torque_mode();
@@ -221,15 +273,26 @@
                     controller.kd = 0;
                     controller.t_ff = 0;
                     } 
-
+    
                 torque_control(&controller);
+                update_observer(&controller, &observer);
+                field_weaken(&controller);
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
-
                 controller.timeout++;
+
+                if(controller.otw_flag)
+                { 
+                    state = REST_MODE;
+                    state_change = 1;
+                    gpio.led->write(0);
+                }
+                
                 count++; 
+                }   
+                
+                  
+                break;
             
-                }     
-                break;
             case SETUP_MODE:
                 if(state_change){
                     enter_setup_state();
@@ -240,6 +303,7 @@
                 break;
                 }                 
       }
+      //gpio.led->write(0);
   TIM1->SR = 0x0;                                                               // reset the status register
 }
 
@@ -258,7 +322,7 @@
                 state_change = 1;
                 char_count = 0;
                 cmd_id = 0;
-                gpio.led->write(0);; 
+                gpio.led->write(0);;
                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                 }
         if(state == REST_MODE){
@@ -316,6 +380,12 @@
                     case 't':
                         CAN_TIMEOUT = atoi(cmd_val);
                         break;
+                    case 'h':
+                        TEMP_MAX = fmaxf(fminf(atof(cmd_val), 150.0f), 0.0f);
+                        break;
+                    case 'c':
+                        I_MAX_CONT = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f);
+                        break;
                     default:
                         printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
                         break;
@@ -363,46 +433,41 @@
     controller.v_bus = V_BUS;
     controller.mode = 0;
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
-    wait(.1);
+    wait_us(100);
     
     gpio.enable->write(1);
     wait_us(100);
     drv.calibrate();
     wait_us(100);
-    drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
+    drv.write_DCR(0x0, DIS_GDF_DIS, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
+    wait_us(100);
+    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x1, 0x1, 0x1, SEN_LVL_1_0);   // calibrate shunt amplifiers
     wait_us(100);
-    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
+    zero_current(&controller.adc1_offset, &controller.adc2_offset); 
     wait_us(100);
-    drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);
+    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x1, 0x0, 0x0, 0x0, SEN_LVL_1_0);
+    wait_us(100);
+    drv.write_OCPCR(TRETRY_50US, DEADTIME_50NS, OCP_NONE, OCP_DEG_8US, VDS_LVL_1_88);
     
     //drv.enable_gd();
-    zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
     drv.disable_gd();
+    //zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
+    //drv.enable_gd();
 
-    wait(.1);
-    /*
-    gpio.enable->write(1);
-    TIM1->CCR3 = 0x708*(1.0f);                        // Write duty cycles
-    TIM1->CCR2 = 0x708*(1.0f);
-    TIM1->CCR1 = 0x708*(1.0f);
-    gpio.enable->write(0);
-    */
+    wait_us(100);
+    
     reset_foc(&controller);                                                     // Reset current controller
     reset_observer(&observer);                                                 // Reset observer
-    TIM1->CR1 ^= TIM_CR1_UDIS;
     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
     
-    wait(.1);
+    wait_us(100);
     NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
-    
     NVIC_SetPriority(CAN1_RX0_IRQn, 3);
                                    // attach 'CAN receive-complete' interrupt handler    
     
     // If preferences haven't been user configured yet, set defaults 
     prefs.load();                                                               // Read flash
-    
-    can.filter(CAN_ID , 0xFFF, CANStandard, 0);
-                                                                    
+    can.filter(CAN_ID , 0xFFF, CANStandard, 0);                                                         
     txMsg.id = CAN_MASTER;
     txMsg.len = 6;
     rxMsg.len = 8;
@@ -412,16 +477,20 @@
     if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
     if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
     if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;}
-    if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;}
+    if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=12;}
     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;}
+    if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 1000;}
+    if(isnan(R_NOMINAL) || R_NOMINAL==-1){R_NOMINAL = 0.0f;}
+    if(isnan(TEMP_MAX) || TEMP_MAX==-1){TEMP_MAX = 125.0f;}
+    if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;}
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
     int lut[128] = {0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     init_controller_params(&controller);
+    
 
     pc.baud(921600);                                                            // set serial baud rate
     wait(.01);
@@ -435,33 +504,29 @@
     printf(" CAN ID:  %d\n\r", CAN_ID);
     
 
-
+    TIM1->CR1 ^= TIM_CR1_UDIS;
 
-    //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
-    
-    state_change = 1;
 
 
     int counter = 0;
     while(1) {
-        drv.print_faults();
+        //drv.print_faults();
         wait(.1);
-       //printf("%.4f\n\r", controller.v_bus);
-       /*
+        //printf("%.3f  %.3f\n\r" , observer.temperature, observer.q_in);
+        if(controller.otw_flag){gpio.led->write(!gpio.led->read());}
+             /*
         if(state == MOTOR_MODE)
         {
+            if(controller.otw_flag){gpio.led->write(!gpio.led->read());}
+            //printf("%f  %f\n\r", controller.dtheta_mech, controller.i_d_ref);
             //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
             //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
-            //printf("%.3f\n\r", controller.dtheta_mech);
-            wait(.002);
+            //printf("%.3f  %.3f  %.3f %.3f\n\r", controller.dtheta_elec, observer.resistance, observer.temperature, observer.temp_measured);
+            //printf("%.3f  %.3f\n\r" , observer.temperature, observer.temp_measured);
         }
+        
         */
 
     }

--- a/structs.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/structs.h	Fri Oct 04 14:18:39 2019 +0000
@@ -36,11 +36,20 @@
     float p_des, v_des, kp, kd, t_ff;                       // Desired position, velocity, gians, torque
     float v_ref, fw_int;                                     // output voltage magnitude, field-weakening integral
     float cogging[128];
+    int current_sector;
+    int otw_flag;                                           // Over-temp warning
+    float i_max;
+    float inverter_tab[128];
+    int oc_flag;
     } ControllerStruct;
 
 typedef struct{
     double temperature;                                              // Estimated temperature
-    double temperature2;
+    float temp_measured;
+    float q_in, q_out;
     float resistance;
+    float k;
+    float trust;
+    float delta_t;
     }   ObserverStruct;
 #endif