59 lines
1.3 KiB
C
59 lines
1.3 KiB
C
//
|
|
// Created by lijie on 2024/1/16.
|
|
//
|
|
#include "mymain.h"
|
|
|
|
void mymain()
|
|
{
|
|
HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //开启串口1接收中断
|
|
setvbuf(stdout, NULL, _IONBF, 0); //解决printf没有\n不发送问题
|
|
HAL_Delay(2000);
|
|
led_init();
|
|
adc_init();
|
|
tle5012b_init();
|
|
Init_ICM42688();
|
|
imu_rest();
|
|
moto_Init();
|
|
PID_Iint();
|
|
float x = 0,y=0;
|
|
unsigned int a =0;
|
|
HAL_TIM_Base_Start(&htim5); //启动定时器
|
|
|
|
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 + 90.f;
|
|
Zitai_closed_loop(motorx,&FOCStruct_X,x);
|
|
Zitai_closed_loop(motory,&FOCStruct_Y,-y);
|
|
// commander_run();
|
|
// x = getAngle(motorx);
|
|
// y = getAngle(motory);
|
|
// printf("%fY%f\n",x,y);
|
|
// HAL_Delay(10);
|
|
// speed_closed_loop(motorx,&FOCStruct_X,_2PI*5);
|
|
// speed_closed_loop(motory,&FOCStruct_Y,_2PI*2);
|
|
// printf("b%d\n",b);
|
|
// if(i%3000 ==0)
|
|
// {
|
|
// printf("%d\n",i);
|
|
// }
|
|
// i++;
|
|
// Data_send(x,y);
|
|
// HAL_Delay(3);
|
|
}
|
|
}
|
|
|
|
|
|
|