From 5b49a0ee243d21466cae55c1a0df6dbdb3eb32af Mon Sep 17 00:00:00 2001 From: 4x-tech Date: Thu, 13 Nov 2025 22:00:00 +0800 Subject: [PATCH] closed loop test passed --- 3rd/config.h | 2 +- 3rd/dfoc.c | 67 +++++++++++++++++++++++++++------------------------- empty.c | 33 ++++++++++++++++---------- 3 files changed, 56 insertions(+), 46 deletions(-) diff --git a/3rd/config.h b/3rd/config.h index 51db6bf..008b8a9 100644 --- a/3rd/config.h +++ b/3rd/config.h @@ -3,5 +3,5 @@ #define DEBUG_ENABLED true #define DEBUG_MT_ENABLED false -#define DEBUG_DFOC true +#define DEBUG_DFOC false #endif /* ti_msp_dl_config_h */ \ No newline at end of file diff --git a/3rd/dfoc.c b/3rd/dfoc.c index 6a41d86..0e5f566 100644 --- a/3rd/dfoc.c +++ b/3rd/dfoc.c @@ -15,10 +15,10 @@ #define _1_SQRT3 0.57735026919f #define _2_SQRT3 1.15470053838f -float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0; -float voltage_limit = 8; -float voltage_power_supply = 0; -float zero_electric_Angle = 0.0; +volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0; +const float voltage_limit = 12; +const float voltage_power_supply = 12.0f; +volatile float zero_electric_Angle = 0.0; extern int pp; extern int Dir; @@ -48,7 +48,7 @@ float constrain(float amt, float low, float high) //将角度归化到0-2PI float normalizeAngle(float angle) { - float a = fmod(angle, 2 * PI); + volatile float a = fmod(angle, 2 * PI); return ((a >= 0) ? a : (a + 2 * PI)); } @@ -59,14 +59,14 @@ float electricAngle(void) void SetPwm(float Ua, float Ub, float Uc) { - float U_a = 0.0; - float U_b = 0.0; - float U_c = 0.0; + volatile float U_a = 0.0; + volatile float U_b = 0.0; + volatile float U_c = 0.0; U_a = constrain(Ua, 0.0f, voltage_limit); U_b = constrain(Ub, 0.0f, voltage_limit); U_c = constrain(Uc, 0.0f, voltage_limit); - + //printf("Ua : %f -- Ub : %f -- Uc : %f -- U_a : %f -- U_b : %f -- U_c : %f \n",Ua,Ub,Uc,U_a,U_b,U_c); dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f); dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f); dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f); @@ -74,19 +74,14 @@ void SetPwm(float Ua, float Ub, float Uc) PWM_Channel1(dc_a * 1000.0f); // 频率15k PWM_Channel2(dc_b * 1000.0f); PWM_Channel3(dc_c * 1000.0f); - if(DEBUG_ENABLED & DEBUG_DFOC) - { - printf("dc_a %f \n", dc_a); - printf("dc_b %f \n", dc_b); - printf("dc_c %f \n", dc_c); - } + } //FOC核心算法，克拉克逆变换/帕克逆变换 -float test_angle = 0.0; -float last_test_angle = 0.0; +volatile float test_angle = 0.0; +volatile float last_test_angle = 0.0; void SetPhaseVoltage(float Uq, float Ud, float angle_el) { // angle_el = normalizeAngle(angle_el); @@ -101,21 +96,23 @@ void SetPhaseVoltage(float Uq, float Ud, float angle_el) SetPwm(Ua, Ub, Uc); + + last_test_angle = angle_el; } void Check_Sensor(void) { - SetPhaseVoltage(3, 0, _3PI_2); - delay_ms(3000); + //SetPhaseVoltage(3, 0, _3PI_2); + //delay_ms(3000); zero_electric_Angle = electricAngle(); - SetPhaseVoltage(0, 0, _3PI_2); - delay_ms(500); + // SetPhaseVoltage(0, 0, _3PI_2); + //delay_ms(500); } void FOC_Init(float power) { - voltage_power_supply = power; + //voltage_power_supply = power; //PWM_Init(); //CurrSense_Init(); //AS5600_Init(); @@ -127,28 +124,28 @@ void FOC_Init(float power) // 单角度环 void Set_Angle(float Target) { - float angle = GetAngle(); - float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI); + volatile float angle = GetAngle(); + volatile float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI); SetPhaseVoltage(Uq, 0, electricAngle()); } -double shaft_angle; -double openloop_timestamp; +volatile double openloop_timestamp; float velocityopenloop(float target) { - float Uq = 0.0; - double Ts = 0.0; + volatile float Uq = 0.0; + volatile double Ts = 0.0; + volatile double shaft_angle; - uint32_t now_ts = DL_SYSTICK_getValue(); + volatile uint32_t now_ts = DL_SYSTICK_getValue(); if(now_ts < openloop_timestamp) { - Ts = (openloop_timestamp - now_ts) / 32.0f * 1e-6f; + Ts = (openloop_timestamp - now_ts) / 80.0f * 1e-6f; } else { - Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 32.0f * 1e-6f; + Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 80.0f * 1e-6f; } if(Ts < 0 || Ts >= 0.005) @@ -158,10 +155,16 @@ float velocityopenloop(float target) shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts); + if(DEBUG_ENABLED) + { + //printf("shaft_angle : %f -- target : %f -- Ts : %f",shaft_angle,target,Ts); + } + + Uq = voltage_limit; SetPhaseVoltage(Uq, 0, shaft_angle); - + printf("shaft_angle : %f \n",shaft_angle); openloop_timestamp = now_ts; return Uq; diff --git a/empty.c b/empty.c index 6994ceb..ba8b589 100644 --- a/empty.c +++ b/empty.c @@ -47,32 +47,39 @@ extern bool gIsI2cError; const float num_f = 0.123456f; volatile uint16_t count = 0 ; -const float Target = 10.0; //串口目标值 +const float Target = 10; //串口目标值 -int pp = 7; //电机极对数 -int Dir = 1; //电机编码器方向 +const int pp = 7; //电机极对数 +const int Dir = -1; //电机编码器方向 int main(void) { SYSCFG_DL_init(); NVIC_EnableIRQ(UART_0_INST_INT_IRQN); DL_TimerA_startCounter(PWM_0_INST); - FOC_Init(12); + FOC_Init(12.6); + DL_SYSTICK_resetValue(); + while(1) { DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN); //delay_ms(10); - volatile float angle_rad = GetAngle(); - if(DEBUG_ENABLED) - { - printf("angle %.5f \n", angle_rad); - } - - // volatile uint32_t tick = DL_SYSTICK_getValue(); - // printf("systick is %x \n",tick); - velocityopenloop(Target); + if(DEBUG_ENABLED) + { + //volatile float angle_rad = GetAngle(); + //printf("angle %.5f \n", angle_rad); + } + if(DEBUG_ENABLED) + { + volatile uint32_t tick = DL_SYSTICK_getValue(); + //printf("systick is %x \n",tick); + } + //开环 + //velocityopenloop(Target); + //闭环 + Set_Angle(Target);