108 lines
2.8 KiB
C
108 lines
2.8 KiB
C
#include "jj_motion.h"
|
|
#include "by_fan_control.h"
|
|
#include "by_imu.h"
|
|
#include "jj_blueteeth.h"
|
|
#include "../3rd-lib/PID-Library/pid.h"
|
|
#include "zf_driver_encoder.h"
|
|
|
|
PID_TypeDef far_angle_pid;
|
|
PID_TypeDef far_gyro_pid;
|
|
PID_TypeDef near_pos_pid;
|
|
PID_TypeDef speed_pid;
|
|
|
|
float an_Kp = 8.0f;
|
|
float an_Ki = 1.0f;
|
|
float an_Kd = 2.0f;
|
|
float in_angle;
|
|
float set_angle = 0.0f;
|
|
float out_angle;
|
|
|
|
float gy_Kp = 1.0f;
|
|
float gy_Ki = 0.0f;
|
|
float gy_Kd = 0.0f;
|
|
float in_gyro;
|
|
float out_gyro;
|
|
//float set_gyro = 0.0f;
|
|
|
|
float po_Kp = 1.0f;
|
|
float po_Ki = 0.0f;
|
|
float po_Kd = 0.0f;
|
|
float in_pos;
|
|
float out_pos;
|
|
float set_pos = 0.0f;
|
|
|
|
float sp_Kp = 1.0f;
|
|
float sp_Ki = 0.0f;
|
|
float sp_Kd = 0.0f;
|
|
float in_speed;
|
|
float out_speed;
|
|
float set_speed = 0.0f;
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
*/
|
|
int cnt1 = 0;
|
|
void sport_motion(void)
|
|
{
|
|
|
|
cnt1++;
|
|
in_gyro = imu660ra_gyro_z; // 陀螺仪输入
|
|
in_angle = 0; // 图像远端输入
|
|
in_pos = 0; // 图像近端输入
|
|
in_speed = encoder_get_count(TIM5_ENCOEDER) / 1024 / 0.01 * 0.25; // 速度输入,m/s
|
|
encoder_clear_count(TIM5_ENCOEDER); // 清除计数
|
|
PID_Compute(&far_gyro_pid);
|
|
|
|
if (cnt1 >= 10) {
|
|
PID_Compute(&far_angle_pid);
|
|
PID_Compute(&speed_pid);
|
|
cnt1 = 0;
|
|
}
|
|
|
|
PID_Compute(&near_pos_pid);
|
|
|
|
if (bt_run_flag == 1) {
|
|
by_pwm_power_duty((int32_t)(500 + out_pos + out_gyro),
|
|
(int32_t)(500 - out_pos - out_gyro),
|
|
(int32_t)(500 + out_speed + out_gyro),
|
|
(int32_t)(500 + out_speed - out_gyro));
|
|
} else {
|
|
by_pwm_power_duty(500, 500, 500, 500);
|
|
}
|
|
if (bt_fly_flag == 0) {
|
|
bt_fly = 0;
|
|
}
|
|
|
|
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
|
|
}
|
|
/**
|
|
* @brief 结构体初始化
|
|
*
|
|
*/
|
|
void sport_pid_init()
|
|
{
|
|
|
|
PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp, an_Ki, an_Kd, 0, 0);
|
|
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp, gy_Ki, gy_Kd, 1, 1);
|
|
PID_Init(&far_angle_pid);
|
|
PID_Init(&far_gyro_pid);
|
|
|
|
PID_SetOutputLimits(&far_angle_pid, -500.0f, 500.0f);
|
|
PID_SetOutputLimits(&far_gyro_pid, -500.0f, 500.0f);
|
|
|
|
PID_SetMode(&far_gyro_pid, 1);
|
|
PID_SetMode(&far_angle_pid, 1);
|
|
|
|
PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, 0, 0);
|
|
PID(&speed_pid, &in_speed, &out_speed, &out_speed, sp_Kp, sp_Ki, sp_Kd, 1, 1);
|
|
PID_Init(&near_pos_pid);
|
|
PID_Init(&speed_pid);
|
|
|
|
PID_SetOutputLimits(&near_pos_pid, -500.0f, 500.0f);
|
|
PID_SetOutputLimits(&speed_pid, -400.0f, 400.0f);
|
|
|
|
PID_SetMode(&near_pos_pid, 1);
|
|
PID_SetMode(&speed_pid, 1);
|
|
}
|