#include "jj_motion.h"
#include "by_fan_control.h"
#include "by_imu.h"
#include "../3rd-lib/PID-Library/pid.h"
#include "jj_blueteeth.h"
#include "gl_common.h"
PID_TypeDef angle_pid;
PID_TypeDef gyro_z_pid;
PID_TypeDef img_x_pid;
float an_Kp = 8.0f;
float an_Ki = 0.0f;
float an_Kd = 2.0f;
float im_Kp = 1.0f;
float im_Ki = 0.0f;
float im_Kd = 0.0f;
float set_x = 0.0f;
float img_x = 40.0f;
float yaw0  = 0;
float out_M = 0;
float out_yaw;
uint32_t cnt1 = 0;
/**
 * @brief
 *
 * @param fy y轴的加速度
 */
void sport_motion2(uint32_t fy)
{
    cnt1++;
    yaw0 = imu660ra_gyro_z;
    if (cnt1 >= 50) {
        PID_Compute(&angle_pid);
        cnt1 = 0;
    }
    PID_Compute(&gyro_z_pid);
    if (bt_run_flag == 1) {
        by_pwm_power_duty((int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M),
                          (int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M));
    } else {
        by_pwm_power_duty(500, 500, 500, 500);
    }
}
/**
 * @brief 结构体初始化
 *
 */
void sport_pid_init()
{

    PID(&angle_pid, &bt_angle, &out_yaw, &set_x, im_Kp, im_Ki, im_Kd, 0, 0);
    PID(&gyro_z_pid, &yaw0, &out_M, &out_yaw, an_Kp, an_Ki, an_Kd, 1, 1);
    PID_Init(&angle_pid);
    PID_Init(&gyro_z_pid);

    PID_SetOutputLimits(&angle_pid, -500.0f, 500.0f);
    PID_SetOutputLimits(&gyro_z_pid, -400.0f, 400.0f);

    PID_SetMode(&gyro_z_pid, 1);
    PID_SetMode(&angle_pid, 1);
}