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

9
APP/foc.h
View File

@ -24,9 +24,11 @@ typedef struct
double Angle_Prev;
unsigned long Time_Prev;
}FOCTypeDef;
extern FOCTypeDef FOCStruct_X;
extern FOCTypeDef FOCStruct_Y;
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el);
int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct);
void PID_Iint(void);
double _normalizeAngle(float angle);
@ -34,7 +36,10 @@ double LPF_velocity(double x,FOCTypeDef *FOCStruct);
void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target);
void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error);
void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target);
void moto_gpio_init();
void PWM_Start();
void moto_Init();
#endif //FOC_N_FOC_H

40
APP/mymain.c
View File

@ -1,12 +1,12 @@
//
// Created by hu123456 on 2024/1/16.
// Created by lijie on 2024/1/16.
//
#include "mymain.h"
void mymain()
{
HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
setvbuf(stdout, NULL, _IONBF, 0); //<2F><><EFBFBD><EFBFBD>printfû<66><C3BB>\n<><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
HAL_Delay(2000);
led_init();
adc_init();
@ -15,34 +15,32 @@ void mymain()
imu_rest();
moto_Init();
PID_Iint();
setvbuf(stdout, NULL, _IONBF, 0); //<2F><><EFBFBD><EFBFBD>printfû<66><C3BB>\n<><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float x = 0,y=0;
unsigned int i =0;
unsigned int a =0;
HAL_TIM_Base_Start(&htim5); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
// while(i<2000*1)
// {
//// MpuGetData();
//// GetAngle(&ICM42688,&Angle);
// Angel_closed_loop(motorx,&FOCStruct_X,3.0);
// Angel_closed_loop(motory,&FOCStruct_Y,2.2);
// i++;
// }
// i=0;
while(a<2200*6)
{
MpuGetData();
GetAngle(&ICM42688,&Angle);
// Angel_closed_loop(motorx,&FOCStruct_X,x);
// Angel_closed_loop(motory,&FOCStruct_Y,y);
a++;
}
// LED_ON;
while(1)
{
// MpuGetData();
// GetAngle(&ICM42688,&Angle);
// x = Angle.pitch;
// y = Angle.roll;
// Zitai_closed_loop(motorx,&FOCStruct_X,x);
// Zitai_closed_loop(motory,&FOCStruct_Y,-y);
MpuGetData();
GetAngle(&ICM42688,&Angle);
x = Angle.pitch;
y = Angle.roll + 90.f;
Zitai_closed_loop(motorx,&FOCStruct_X,x);
Zitai_closed_loop(motory,&FOCStruct_Y,-y);
// commander_run();
// x = getAngle(motorx);
// y = getAngle(motory);
// x= get_VCC();
// printf("%fY%f\n",x,y);
// HAL_Delay(2);
// HAL_Delay(10);
// speed_closed_loop(motorx,&FOCStruct_X,_2PI*5);
// speed_closed_loop(motory,&FOCStruct_Y,_2PI*2);
// printf("b%d\n",b);

5
APP/tle5012b.c
View File

@ -175,11 +175,12 @@ double getAngle(unsigned char motor)
{
TLE_CS1_ENABLE;
angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
// printf("angle_data:%d\n",angle_data);
d_angle = angle_data - motory_angle_data_prev;
if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
motory_angle_data_prev = angle_data;
TLE_CS1_DISABLE;
return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) /2.f;
}
else
{
@ -189,7 +190,7 @@ double getAngle(unsigned char motor)
if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
motorx_angle_data_prev = angle_data;
TLE_CS2_DISABLE;
return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) /2.f;
}
}