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