50 lines
1.1 KiB
C
50 lines
1.1 KiB
C
//
|
|
// Created by hu123456 on 2024/1/16.
|
|
//
|
|
|
|
#include "mymain.h"
|
|
|
|
void mymain()
|
|
{
|
|
HAL_Delay(2000);
|
|
led_init();
|
|
tle5012b_init();
|
|
Init_ICM42688();
|
|
imu_rest();
|
|
moto_Init();
|
|
PID_Iint();
|
|
setvbuf(stdout, NULL, _IONBF, 0); //解决printf没有\n不发送问题
|
|
float x = 0,y=0;
|
|
unsigned int i =0;
|
|
HAL_TIM_Base_Start(&htim5); //启动定时器
|
|
while(i<3300*3)
|
|
{
|
|
Angel_closed_loop(motorx,&FOCStruct_X,-0.2);
|
|
Angel_closed_loop(motory,&FOCStruct_Y,2.3);
|
|
i++;
|
|
}
|
|
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);
|
|
x = getAngle(motorx);
|
|
y = getAngle(motory);
|
|
// printf("%fY%f\n",x,y);
|
|
// 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,0,0);
|
|
// HAL_Delay(3);
|
|
}
|
|
} |