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

2023/3/13第五次参数飞行测试

Browse Source
This commit is contained in:
lijie
2024-03-14 16:50:56 +08:00
parent 6e701f3ae8
commit c1b5b5fa04
8 changed files with 881 additions and 902 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
APP/foc.c
View File

@ -3,14 +3,14 @@
//
#include "foc.h"
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
const float sqrt_3 = 1.7320508075f;
const float Voltage_PowerSupply = 12;
float Voltage_PowerSupply = 12;
float Pole_Pairs = 14;
float Voltage_Limit = 6.9;
float Velocity_Limit = 100;
float V_Acc_Limit = 100;
float dir = 1;
float dir = -1;
FOCTypeDef FOCStruct_X={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
@ -22,50 +22,16 @@ void PID_Iint(void)
FOCStruct_X.ZT_KD = 0.5;
FOCStruct_X.V_KP = 0.1;
FOCStruct_X.V_KI = 1;
FOCStruct_X.V_KI = 2;
FOCStruct_X.V_KD = 0;
///////////
FOCStruct_Y.ZT_KP = 3;
//////////
FOCStruct_Y.ZT_KP = 3.0;
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_KI = 1.4;
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;
// FOCStruct_X.ZT_KI = 0;
// FOCStruct_X.ZT_KD = 0.2;
//
// FOCStruct_X.V_KP = 0.1;
// FOCStruct_X.V_KI = 1;
// FOCStruct_X.V_KD = 0;
////2024/1/22 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;
}
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
{
@ -93,8 +59,8 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
if(Uout<-0.577)Uout=-0.577;
sector = (angle_el / _PI_3) + 1;
T1 = sqrt(3)*sin(sector*_PI_3 - angle_el) * Uout;
T2 = sqrt(3)*sin(angle_el - (sector-1.0)*_PI_3) * Uout;
T1 = sqrt_3*sin(sector*_PI_3 - angle_el) * Uout;
T2 = sqrt_3*sin(angle_el - (sector-1.0)*_PI_3) * Uout;
T0 = 1 - T1 - T2;
// calculate the duty cycles(times)
@ -135,14 +101,14 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
Tb = 0;
Tc = 0;
}
if (motor == motory)
if (motor ==motorx )
{
__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)
else if (motor == motory)
{
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
@ -308,8 +274,7 @@ void PWM_Start()
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
}
void moto_Init()
void moto_gpio_init()
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
@ -332,10 +297,19 @@ void moto_Init()
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,1);
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,1);
}
void moto_Init()
{
Voltage_PowerSupply = get_VCC();
Voltage_Limit = Voltage_PowerSupply / sqrt_3 - 0.1;
printf("Voltage_PowerSupply: %f V\r\n",Voltage_PowerSupply);
moto_gpio_init();
PWM_Start();
HAL_Delay(1000);
alignSensor(motory,&FOCStruct_Y);
alignSensor(motory,&FOCStruct_Y); //<2F><><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>
alignSensor(motorx,&FOCStruct_X);
HAL_Delay(50);
}
@ -348,7 +322,6 @@ int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct)
float moved;
float q = 3.5;
printf("MOT: Align sensor.\r\n");
// find natural direction
// move one electrical revolution forward
HAL_Delay(100);