150 lines
4.1 KiB
C
150 lines
4.1 KiB
C
#include "jj_motion.h"
|
|
|
|
#include "zf_common_headfile.h"
|
|
#include "pid.h"
|
|
|
|
#include "jj_blueteeth.h"
|
|
#include "by_fan_control.h"
|
|
#include "by_imu.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;
|
|
|
|
int cnt1 = 0;
|
|
int cnt2 = 0;
|
|
float sport_get_speed(void)
|
|
{
|
|
#define ALPHA (0.97f)
|
|
static float speed_now = 0;
|
|
static float speed_last = 0;
|
|
|
|
speed_now = ALPHA * (float)encoder_get_count(TIM5_ENCOEDER) + (1.0f - ALPHA) * speed_last;
|
|
speed_last = speed_now;
|
|
|
|
encoder_clear_count(TIM5_ENCOEDER);
|
|
|
|
return speed_now;
|
|
|
|
#undef ALPHA
|
|
}
|
|
|
|
void sport_motion(void)
|
|
{
|
|
|
|
cnt1++;
|
|
cnt2++;
|
|
|
|
imu660ra_get_gyro();
|
|
in_gyro = imu660ra_gyro_z; // 陀螺仪输入
|
|
// in_angle = 0; // 图像远端输入
|
|
// in_pos = 0; // 图像近端输入
|
|
// 清除计数
|
|
PID_Compute(&far_gyro_pid);
|
|
|
|
if (cnt1 >= 10) {
|
|
in_speed = -1 * sport_get_speed();
|
|
PID_Compute(&speed_pid);
|
|
|
|
PID_Compute(&near_pos_pid);
|
|
|
|
cnt1 = 0;
|
|
}
|
|
|
|
if (cnt2 >= 20) {
|
|
cnt2 = 0;
|
|
PID_Compute(&far_angle_pid);
|
|
}
|
|
|
|
/* 动力风扇设置 */
|
|
if (1 == bt_run_flag) {
|
|
if (out_gyro > 0) {
|
|
by_pwm_power_duty((int32_t)(500 - 1 * out_pos + 0 * out_gyro),
|
|
(int32_t)(500 + 1 * out_pos - 0 * out_gyro),
|
|
(int32_t)(500 + out_speed + out_gyro),
|
|
(int32_t)(500 + out_speed - out_gyro));
|
|
|
|
} else if (out_gyro <= 0) {
|
|
by_pwm_power_duty((int32_t)(500 - 1 * out_pos + 0 * out_gyro),
|
|
(int32_t)(500 + 1 * out_pos - 0 * out_gyro),
|
|
(int32_t)(500 + out_speed + out_gyro),
|
|
(int32_t)(500 + out_speed - out_gyro));
|
|
}
|
|
|
|
// by_pwm_power_duty(500+out_gyro, 500-out_gyro, 500+out_gyro , 500-out_gyro);
|
|
} else {
|
|
by_pwm_power_duty(500, 500, 500, 500);
|
|
}
|
|
|
|
/* 升力风扇设置 */
|
|
if (bt_fly_flag == 0) {
|
|
by_pwm_update_duty(0 + 500, 0 + 500);
|
|
} else {
|
|
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, _PID_P_ON_E, _PID_CD_REVERSE);
|
|
PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC);
|
|
PID_SetSampleTime(&far_angle_pid, 20);
|
|
PID_SetOutputLimits(&far_angle_pid, -3000.0f, 3000.0f);
|
|
// PID_Init(&far_angle_pid);
|
|
|
|
/* 角速度控制 */
|
|
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp, gy_Ki, gy_Kd, _PID_P_ON_E, _PID_CD_REVERSE);
|
|
PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
|
|
PID_SetSampleTime(&far_gyro_pid, 1);
|
|
PID_SetOutputLimits(&far_gyro_pid, -3000.0f, 3000.0f);
|
|
// PID_Init(&far_gyro_pid);
|
|
|
|
/* 近点控制 */
|
|
PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, _PID_P_ON_E, _PID_CD_DIRECT);
|
|
PID_SetMode(&near_pos_pid, _PID_MODE_AUTOMATIC);
|
|
PID_SetSampleTime(&near_pos_pid, 10);
|
|
PID_SetOutputLimits(&near_pos_pid, -3000.0f, 3000.0f);
|
|
// PID_Init(&near_pos_pid);
|
|
|
|
/* 速度控制 */
|
|
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_E, _PID_CD_DIRECT);
|
|
PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
|
|
PID_SetSampleTime(&speed_pid, 10);
|
|
PID_SetOutputLimits(&speed_pid, 0.0f, 2500.0f);
|
|
// PID_Init(&speed_pid);
|
|
}
|