Diff: main.cpp

Revision:

34:47a55f96fbc4

Parent:

32:ccac5da77844

--- a/main.cpp	Wed Aug 30 18:10:27 2017 +0000
+++ b/main.cpp	Mon Oct 02 00:55:39 2017 +0000
@@ -10,13 +10,12 @@
 #define SETUP_MODE 4
 #define ENCODER_MODE 5
 
-#define VERSION_NUM "1.0.1"
+#define VERSION_NUM "TEST_MODE"
 
 
 float __float_reg[64];                                                          // Floats stored in flash
 int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
-
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -31,7 +30,8 @@
 #include "FlashWriter.h"
 #include "user_config.h"
 #include "PreferenceWriter.h"
-
+#include "FastMath.h"
+using namespace FastMath;
  
 PreferenceWriter prefs(6);
 
@@ -59,6 +59,9 @@
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
+volatile int cal_counter = 0;
+volatile float rise = 0;
+volatile float avg_current = 0;
 
  #define P_MIN -12.5f
  #define P_MAX 12.5f
@@ -189,7 +192,7 @@
     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
     wait(.001);
     controller.i_d_ref = 0;
-    controller.i_q_ref = 0;                                                     // Current Setpoints
+    controller.i_q_ref = 2;                                                     // Current Setpoints
     GPIOC->ODR |= (1 << 5);                                                     // Turn on status LED
     state_change = 0;
     printf("\n\r Entering Motor Mode \n\r");
@@ -240,7 +243,63 @@
             
             case CALIBRATION_MODE:                                              // Run encoder calibration procedure
                 if(state_change){
-                    calibrate();
+                    gpio.enable->write(1);
+
+                    //calibrate();
+                    state_change = 0;
+                    GPIOC->ODR |= (1 << 5);  
+                    TIM1->CCR3 = (PWM_ARR)*(0.5f);                                        // Set duty cycles
+                    TIM1->CCR2 = (PWM_ARR)*(0.5f);
+                    TIM1->CCR1 = (PWM_ARR)*(0.5f);
+                    wait_us(200);
+                    cal_counter = 1;
+                    rise = 0;
+                    avg_current = 0;
+                    }
+                if(cal_counter>0){
+                    if(cal_counter<1000){
+                        TIM1->CCR3 = (PWM_ARR)*(0.5f);                                        // Set duty cycles
+                        TIM1->CCR2 = (PWM_ARR)*(0.5f);
+                        TIM1->CCR1 = (PWM_ARR)*(0.5f);
+                        }
+                    if(cal_counter>=1000){
+                        float s = FastSin(0); 
+                        float c = FastCos(0);
+                        float dtc_u, dtc_v, dtc_w;
+                        float i_a, i_b, i_c, i_d, i_q = 0.0f;
+                        controller.v_d = 3.0f;
+                        controller.v_q = 0.0f;
+                        float v_u = c*controller.v_d - s*controller.v_q;                // Faster Inverse DQ0 transform
+                        float v_v = (0.86602540378f*s-.5f*c)*controller.v_d - (-0.86602540378f*c-.5f*s)*controller.v_q;
+                        float v_w = (-0.86602540378f*s-.5f*c)*controller.v_d - (0.86602540378f*c-.5f*s)*controller.v_q;
+                        svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation
+                        TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u);                                        // Set duty cycles
+                        TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
+                        TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
+                        i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset);    // Calculate phase currents from ADC readings
+                        i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset);
+                        i_a = -i_b - i_c;              
+                        i_d = 0.6666667f*(c*i_a + (0.86602540378f*s-.5f*c)*i_b + (-0.86602540378f*s-.5f*c)*i_c);   ///Faster DQ0 Transform
+                        i_q = 0.6666667f*(-s*i_a - (-0.86602540378f*c-.5f*s)*i_b - (0.86602540378f*c-.5f*s)*i_c);
+                        float current = sqrt(i_d*i_d + i_q*i_q);
+                        avg_current += .000025f*current;
+                        if(cal_counter<1020){rise += .05f*current;}
+                        }
+                    cal_counter++;
+                    if(cal_counter>41000){ 
+                        cal_counter = 0;
+                        GPIOC->ODR &= !(1 << 5);
+                        wait_us(200);
+                        gpio.enable->write(0);
+                        float L = controller.v_d*.0005f/(2.0f*rise);
+                        float R = controller.v_d/avg_current;
+                        printf("%f  \n\r", rise);
+                        calibrate();
+                        printf("Inductance:   %f\n\r", L);
+                        printf("Resistance:   %f\n\r", R);
+                        
+                        }
+                    
                     }
                 break;
              
@@ -260,8 +319,16 @@
                 //TIM1->CCR2 = 0x708*(1.0f);     
                 
                 //controller.i_q_ref = controller.t_ff/KT_OUT;   
-                
-                torque_control(&controller);     
+                if(count >40){
+                     controller.i_q_ref = 20;
+                     //wait(.001);
+                    //printf(" Started commutating\n\r");
+                     }
+                if(count>80){
+                    controller.i_q_ref = -20;
+                    count = 0;
+                    }
+                //torque_control(&controller);     
                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
                     controller.i_d_ref = 0;
                     controller.i_q_ref = 0;
@@ -272,14 +339,10 @@
                 //toggle.write(0);
                 controller.timeout += 1;
                 
-                if(count == 1){
-                     count = 0;
-                     //wait(.001);
-                    //printf(" Started commutating\n\r");
-                     }
-                     }
+                }
                      
                 break;
+                
             case SETUP_MODE:
                 if(state_change){
                     enter_setup_state();
@@ -289,6 +352,7 @@
                 print_encoder();
                 break;
                 }
+            
                  
       }
   TIM1->SR = 0x0;                                                               // reset the status register