auto-measurements
Dependencies: FastPWM3 mbed-dev
Fork of Hobbyking_Cheetah_Compact by
Diff: main.cpp
- Revision:
- 34:47a55f96fbc4
- Parent:
- 32:ccac5da77844
diff -r 3ab3cce8b44d -r 47a55f96fbc4 main.cpp --- 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