//
// Created by hu123456 on 2024/1/16.
//

#include "mymain.h"

void mymain()
{
    HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //开启串口1接收中断
    HAL_Delay(2000);
    led_init();
    adc_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<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;
//    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);
//        commander_run();
//        x = getAngle(motorx);
//        y = getAngle(motory);
//        x= get_VCC();
//        printf("%fY%f\n",x,y);
//        HAL_Delay(2);
//        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);
    }
}