61 lines
1.4 KiB
C
61 lines
1.4 KiB
C
#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);
|
|
}
|