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