MIT Motor FOC Control

Dependencies:   FastPWM3

Revision:
55:c4c9fec8539c
Parent:
53:e85efce8c1eb
Child:
56:fe5056ac6740
--- 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);
         }
+        
         */
 
     }