4x-tech/foc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

closed loop test passed

Browse Source
This commit is contained in:
4x-tech
2025-11-13 22:00:00 +08:00
parent ab6d7c0dd9
commit 5b49a0ee24
3 changed files with 56 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
3rd/config.h
View File

@@ -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 */

67
3rd/dfoc.c
View File

@@ -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;

33
empty.c
View File

@@ -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);