lijie/foc_Gimbal
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

将ICM42688改为硬件SPI通讯。

Browse Source
This commit is contained in:
lijie
2024-02-22 10:16:24 +08:00
parent 08cbf4ba74
commit 7749d1d7b7
19 changed files with 1391 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
APP/foc.c
View File

@ -17,12 +17,12 @@ FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
void PID_Iint(void)
{
FOCStruct_X.ZT_KP = 2;
FOCStruct_X.ZT_KP = 4;
FOCStruct_X.ZT_KI = 0;
FOCStruct_X.ZT_KD = 0.2;
FOCStruct_X.ZT_KD = 0.5;
FOCStruct_X.V_KP = 0.1;
FOCStruct_X.V_KI = 1.5;
FOCStruct_X.V_KI = 2;
FOCStruct_X.V_KD = 0;
//2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
FOCStruct_Y.ZT_KP = 2;
@ -33,6 +33,22 @@ void PID_Iint(void)
FOCStruct_Y.V_KI = 1;
FOCStruct_Y.V_KD = 0;
// FOCStruct_X.ZT_KP = 4;
// FOCStruct_X.ZT_KI = 0;
// FOCStruct_X.ZT_KD = 0.5;
//
// FOCStruct_X.V_KP = 0.1;
// FOCStruct_X.V_KI = 2;
// FOCStruct_X.V_KD = 0;
////2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_Y.ZT_KP = 2;
// FOCStruct_Y.ZT_KI = 0;
// FOCStruct_Y.ZT_KD = 0.5;
//
// FOCStruct_Y.V_KP = 0.1;
// FOCStruct_Y.V_KI = 1;
// FOCStruct_Y.V_KD = 0;
////2024/1/23 xY<78><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_X.ZT_KP = 2;
@ -50,7 +66,6 @@ void PID_Iint(void)
// FOCStruct_Y.V_KP = 0.1;
// FOCStruct_Y.V_KI = 1;
// FOCStruct_Y.V_KD = 0;
}
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
{
@ -67,7 +82,8 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
}
else
{// only Uq available - no need for atan2 and sqrt
{
// only Uq available - no need for atan2 and sqrt
Uout = Uq / Voltage_PowerSupply;
// angle normalisation in between 0 and 2pi
// only necessary if using _sin and _cos - approximation functions
@ -121,16 +137,16 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
}
if (motor == motory)
{
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800);
}
else if (motor == motorx)
{
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,Tc*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,Tc*4800);
}
}
@ -160,7 +176,7 @@ void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target)
float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);
float V_Out = V_Pid_P+V_Pid_I;
float V_Out = V_Pid_P + V_Pid_I;
V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);
float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;