btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

适配新班子

Browse Source
This commit is contained in:
井明江
2024-06-28 18:23:53 +08:00
parent fad2a71e3a
commit ac05ce6f32
5 changed files with 41 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
app/jj_motion.c
View File

@@ -18,19 +18,19 @@ float ci_Ki0 = 0;
float ci_Kd0 = 0;
float out_ci;
// 弯道后面风扇角度环
float an_Kp0 = 40.0f;
float an_Kp0 = 45.0f;
float an_Ki0 = 0.0f;
float an_Kd0 = 0.0f;
// 直道后面风扇角度环
float an_Kp1 = 42.0f;
float an_Kp1 = 35.0f;
float an_Ki1 = 0.0f;
float an_Kd1 = 0.0f;
// 圆环后面风扇角度环
float yu_Kp0 = 60.0f;
float yu_Kp0 = 51.0f;
float yu_Ki0 = 0.0f;
float yu_Kd0 = 0.0f;
// 弯道侧面风扇角度环
float cn_Kp1 = 300.0f;
float cn_Kp1 = 400.0f;
float cn_Ki1 = 0.f;
float cn_Kd1 = 0.0f;
// 直道侧面风扇角度环
@@ -46,15 +46,15 @@ float out_pos;
float set_pos = 0.0f;
float out_cal;
// 弯道角速度环
float gy_Kp0 = 4.50f;
float gy_Kp0 = 4.80f;
float gy_Ki0 = 0.0f;
float gy_Kd0 = 0.0f;
// 直道角速度环
float gy_Kp1 = 3.2f;
float gy_Kp1 = 6.0f;
float gy_Ki1 = 0.0f;
float gy_Kd1 = 0.0f;
// 圆环角速度环
float ygy_Kp0 = 4.0f;
float ygy_Kp0 = 6.0f;
float ygy_Ki0 = 0.0f;
float ygy_Kd0 = 0.0f;
// 当前和输入量
@@ -70,9 +70,9 @@ float in_speed;
float out_speed;
float set_speed;
float set_speed0 = 800.0f;
float set_speed0 = 900.0f;
float set_speed1 = 1100.0f;
float set_speed2 = 810.f;
float set_speed2 = 830.f;
float set_speed3 = 1000.f;
int cnt1 = 0;
int cnt2 = 0;
@@ -89,7 +89,7 @@ int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
float qd_down = 0.0f;
float last_gy=0.0f;
/*
0:无状态
1:弯道
@@ -126,12 +126,13 @@ void sport_motion()
imu660ra_get_acc();
get_vol();
in_gyro = imu660ra_gyro_z;
// last_gy=in_gyro;
// 抖动停车
if (imu660ra_acc_z <= 600) {
// if (imu660ra_acc_z <= 300) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("ting");
}
// bt_fly_flag = bt_run_flag = 0;
// bt_printf("ting");
// }
// 斑马线停车
if (1 == in_stop) {
stop_flag = 1;
@@ -266,7 +267,7 @@ void sport_pid_init()
PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp1, an_Ki1, an_Kd1, _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, -4000.0f, 4000.0f);
PID_SetOutputLimits(&far_angle_pid, -4000.0f, 7000.0f);
// 直道侧面位置环
PID(&str_angle_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&str_angle_pid, _PID_MODE_AUTOMATIC);
@@ -286,11 +287,11 @@ void sport_pid_init()
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); // m是增量
PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_gyro_pid, 20);
PID_SetOutputLimits(&far_gyro_pid, -6000.0f, 6000.0f);
PID_SetOutputLimits(&far_gyro_pid, -7000.0f, 7000.0f);
/* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT);
PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&speed_pid, 20);
PID_SetOutputLimits(&speed_pid, -0.0f, 6000.0f);
PID_SetOutputLimits(&speed_pid, -0.0f, 7000.0f);
}