From 77b61cb90e7de50782c1ef9b2b6b422ba2d52519 Mon Sep 17 00:00:00 2001 From: 4x-tech Date: Tue, 18 Nov 2025 15:25:57 +0800 Subject: [PATCH] test velocityopenloop failed --- 3rd/config.h | 4 +- 3rd/dfoc.c | 238 ++++++++++++++++++++++----------------------------- empty.c | 15 ++-- empty.syscfg | 1 - 4 files changed, 115 insertions(+), 143 deletions(-) diff --git a/3rd/config.h b/3rd/config.h index 65dc185..e6fc993 100644 --- a/3rd/config.h +++ b/3rd/config.h @@ -1,7 +1,7 @@ #ifndef CONFIG_H #define CONFIG_H -#define DEBUG_ENABLED true -#define DEBUG_MT_ENABLED true +#define DEBUG_ENABLED false +#define DEBUG_MT_ENABLED false #define DEBUG_DFOC_ENABLED false #endif \ No newline at end of file diff --git a/3rd/dfoc.c b/3rd/dfoc.c index 53e290d..bd9c19d 100644 --- a/3rd/dfoc.c +++ b/3rd/dfoc.c @@ -1,186 +1,156 @@ -#include -#include -#include "pwm.h" -#include "mt6701.h" +#include "config.h" #include "delay.h" #include "lowpass_filter.h" +#include "mt6701.h" #include "pid_control.h" +#include "pwm.h" #include +#include +#include #include -#include "config.h" -#define PI 3.14159265359f -#define _3PI_2 4.71238898f -#define _1_SQRT3 0.57735026919f -#define _2_SQRT3 1.15470053838f +#define PI 3.14159265359f +#define _3PI_2 4.71238898f +#define _1_SQRT3 0.57735026919f +#define _2_SQRT3 1.15470053838f -volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0; -const float voltage_limit = 8; +volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, + dc_c = 0; +const float voltage_limit = 5.5; const float voltage_power_supply = 12.0f; volatile float zero_electric_Angle = 0.0; extern int pp; extern int Dir; - - -void Motor_en() -{ - // GPIO_InitTypeDef GPIO_InitStructure; - // - // RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); - // - // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; - // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - // GPIO_Init(GPIOA,&GPIO_InitStructure); - // - // GPIO_SetBits(GPIOA, GPIO_Pin_8); +void Motor_en() { + // GPIO_InitTypeDef GPIO_InitStructure; + // + // RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); + // + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + // GPIO_Init(GPIOA,&GPIO_InitStructure); + // + // GPIO_SetBits(GPIOA, GPIO_Pin_8); } -//限制幅值 -float constrain(float amt, float low, float high) -{ - return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt))); +// 限制幅值 +float constrain(float amt, float low, float high) { + return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt))); } -//将角度归化到0-2PI -float normalizeAngle(float angle) -{ - volatile float a = fmod(angle, 2 * PI); - return ((a >= 0) ? a : (a + 2 * PI)); +// 将角度归化到0-2PI +float normalizeAngle(float angle) { + volatile float a = fmod(angle, 2 * PI); + return ((a >= 0) ? a : (a + 2 * PI)); } -float electricAngle(void) -{ - return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle); +float electricAngle(void) { + return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle); } -void SetPwm(float Ua, float Ub, float Uc) -{ - 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); - - PWM_Channel1(dc_a * 1000.0f); // 频率15k - PWM_Channel2(dc_b * 1000.0f); - PWM_Channel3(dc_c * 1000.0f); - +void SetPwm(float Ua, float Ub, float Uc) { + 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); + PWM_Channel1(dc_a * 500.0f); // 频率15k + PWM_Channel2(dc_b * 500.0f); + PWM_Channel3(dc_c * 500.0f); } -//FOC核心算法，克拉克逆变换/帕克逆变换 +// FOC核心算法，克拉克逆变换/帕克逆变换 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); - test_angle = angle_el - last_test_angle; +void SetPhaseVoltage(float Uq, float Ud, float angle_el) { + // angle_el = normalizeAngle(angle_el); + test_angle = angle_el - last_test_angle; - Ualpha = -Uq * sin(angle_el); - Ubeta = Uq * cos(angle_el); + Ualpha = -Uq * sin(angle_el); + Ubeta = Uq * cos(angle_el); - Ua = Ualpha + voltage_power_supply / 2; - Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; - Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2; + Ua = Ualpha + voltage_power_supply / 2; + Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; + Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2; - SetPwm(Ua, Ub, Uc); + SetPwm(Ua, Ub, Uc); - - - last_test_angle = angle_el; + last_test_angle = angle_el; } -void Check_Sensor(void) -{ - //SetPhaseVoltage(3, 0, _3PI_2); - //delay_ms(3000); - zero_electric_Angle = electricAngle(); - // SetPhaseVoltage(0, 0, _3PI_2); - //delay_ms(500); +void Check_Sensor(void) { + // SetPhaseVoltage(3, 0, _3PI_2); + // delay_ms(3000); + zero_electric_Angle = electricAngle(); + // SetPhaseVoltage(0, 0, _3PI_2); + // delay_ms(500); } -void FOC_Init(float power) -{ - //voltage_power_supply = power; - //PWM_Init(); - //CurrSense_Init(); - //AS5600_Init(); +void FOC_Init(float power) { + // voltage_power_supply = power; + // PWM_Init(); + // CurrSense_Init(); + // AS5600_Init(); - Check_Sensor(); + Check_Sensor(); } - // 单角度环 -void Set_Angle(float Target) -{ - volatile float langle = GetAngle(); - if(DEBUG_ENABLED & DEBUG_DFOC_ENABLED) - { - printf("angle readback in dfoc.c is %f \n", langle); - } - volatile float Uq = PID_Controller(0.067, 0.01, 0, (Target - Dir * langle) * 180 / PI); - //volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * 180 / PI); - if(DEBUG_ENABLED& DEBUG_DFOC_ENABLED) - { - printf("Uq is %f \n", Uq); - } - SetPhaseVoltage(Uq, 0, electricAngle()); +void Set_Angle(float Target) { + volatile float langle = GetAngle(); + if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) { + printf("angle readback in dfoc.c is %f \n", langle); + } + volatile float Uq = + PID_Controller(0.067, 0.01, 0, (Target - Dir * langle) * 180 / PI); + // volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * + // 180 / PI); + if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) { + printf("Uq is %f \n", Uq); + } + SetPhaseVoltage(Uq, 0, electricAngle()); } volatile double openloop_timestamp; -float velocityopenloop(float target) -{ - volatile float Uq = 0.0; - volatile double Ts = 0.0; - volatile double shaft_angle; +float velocityopenloop(float target) { + volatile float Uq = 0.0; + volatile double Ts = 0.0; + volatile double shaft_angle; - volatile 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) / 80.0f * 1e-6f; + } else { + Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 80.0f * 1e-6f; + } - if(now_ts < openloop_timestamp) - { - Ts = (openloop_timestamp - now_ts) / 80.0f * 1e-6f; - } - else - { - Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 80.0f * 1e-6f; - } + if (Ts < 0 || Ts >= 0.005) { + Ts = 0.001f; + } - if(Ts < 0 || Ts >= 0.005) - { - Ts = 0.001f; - } + shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts); - shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts); + if (DEBUG_ENABLED) { + printf("shaft_angle : %f -- Ts : %f \n", shaft_angle, Ts); + } - if(DEBUG_ENABLED) - { - //printf("shaft_angle : %f -- target : %f -- Ts : %f \n",shaft_angle,target,Ts); - } + Uq = voltage_limit; + SetPhaseVoltage(Uq, 0, shaft_angle); + openloop_timestamp = now_ts; - Uq = voltage_limit; - - SetPhaseVoltage(Uq, 0, shaft_angle); - printf("shaft_angle : %f \n", shaft_angle); - openloop_timestamp = now_ts; - - return Uq; + return Uq; } - - - - - - diff --git a/empty.c b/empty.c index 8105e35..38bcad8 100644 --- a/empty.c +++ b/empty.c @@ -48,7 +48,7 @@ extern bool gIsI2cError; const float num_f = 0.123456f; volatile uint16_t count = 0; -volatile float Target = 0; // 串口目标值 +volatile float Target = 15; // 串口目标值 const int pp = 7; // 电机极对数 const int Dir = -1; // 电机编码器方向 @@ -116,15 +116,18 @@ int main(void) { // delay_ms(10); // 开环 - // velocityopenloop(Target); - // 闭环 - Set_Angle(Target); + velocityopenloop(Target); + + // //test MT6701 + // MT6701_get_angle_degree(); + // printf("angle degree is %f \n", angle_f); + + // // 闭环 + // Set_Angle(Target); if (gCheckUART) { gCheckUART = false; parse_uart_cmd(); - // Set_Angle(Target); } - delay_ms(10); } } diff --git a/empty.syscfg b/empty.syscfg index 8e9db4a..e2015b7 100644 --- a/empty.syscfg +++ b/empty.syscfg @@ -74,7 +74,6 @@ I2C1.sclPinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric3"; PWM1.$name = "PWM_0"; PWM1.ccIndex = [0,1,2]; PWM1.clockDivider = 2; -PWM1.timerCount = 2000; PWM1.pwmMode = "CENTER_ALIGN"; PWM1.PWM_CHANNEL_0.$name = "ti_driverlib_pwm_PWMTimerCC0"; PWM1.PWM_CHANNEL_1.$name = "ti_driverlib_pwm_PWMTimerCC1";