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

更改限幅

Browse Source
This commit is contained in:
井明江
2024-08-09 17:00:24 +08:00
parent 936f775dbd
commit 53d06467d6
5 changed files with 47 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
app/jj_motion.c
View File

@@ -64,7 +64,7 @@ float ygy_Kp0 = 2.20f;
float ygy_Ki0 = 0.0f;
float ygy_Kd0 = 0.0f;
// 圆环侧面角度环
float yuc_Kp=4.4f;
float yuc_Kp = 4.4f;
// 速度环
float sp_Kp = 3.40f;
float sp_Ki = 3.6f;
@@ -81,7 +81,7 @@ float in_pos;
float out_pos;
float set_pos = 0.0f;
// float set_gyro = 0.0f;
float rate_K = 0;
float in_speed;
float out_speed;
float last_angle;
@@ -100,6 +100,7 @@ int cnt6 = 0;
int cnt7 = 0;
int cnt8 = 0;
int cnt9 = 0;
int cnt10 = 0;
int if_start = 0;
float shock_stop = 0;
uint8_t in_state = 0;
@@ -110,10 +111,11 @@ int32_t pwm_duty_ls;
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;
int32_t last_lb = 0;
int32_t last_rb = 0;
float qd_down = 0.0f;
float last_gy = 0.0f;
int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
/*
0:无状态
1:弯道
@@ -150,11 +152,6 @@ void sport_motion()
get_vol();
in_gyro = imu660ra_gyro_z;
if (fabs(out_gyro) < 700) {
PID_SetOutputLimits(&speed_pid, -0.0f, 2500.0f);
} else {
PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f);
}
////////////////////////////////////////停车任务///////////////////////////////////////////////
// 抖动停车
if (imu660ra_acc_z <= 800) {
@@ -162,12 +159,12 @@ void sport_motion()
} else {
cnt7 = 0;
}
if (cnt7 >= 50 && ((uint32_t)shock_stop ==1||(uint32_t)shock_stop == 2)) {
if (cnt7 >= 50 && ((uint32_t)shock_stop == 1 || (uint32_t)shock_stop == 2)) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("up");
}
// 異常值停车
if (fabs(in_angle - last_angle) > 45.f&&((uint32_t)shock_stop == 1||(uint32_t)shock_stop == 3)) {
if (fabs(in_angle - last_angle) > 45.f && ((uint32_t)shock_stop == 1 || (uint32_t)shock_stop == 3)) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("bug");
}
@@ -181,8 +178,14 @@ void sport_motion()
}
// 出界停车
if (2 == in_stop) {
bt_printf("out");
bt_fly_flag = bt_run_flag = 0;
stop_cnt++;
if (stop_cnt >= 5) {
bt_printf("out");
stop_cnt = 0;
bt_fly_flag = bt_run_flag = 0;
}
} else {
stop_cnt = 0;
}
//////////////////////////////////////////////// 动力风扇设置 */////////////////////////////////////////////////////////
if (1 == bt_run_flag) {
@@ -191,7 +194,7 @@ void sport_motion()
cnt5++;
if (if_start == 0) {
cnt8++;
if (cnt8 >= 10000) {
if (cnt8 >= 5000) {
if_start = 1;
}
}
@@ -200,19 +203,18 @@ void sport_motion()
bt_printf("to 直道");
set_speed = set_speed1;
PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1);
PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1);
PID_SetTunings(&pos_pid, po_Kp1, po_Ki1, po_Kd1);
}
if ((last_state != in_state) && in_state == 1) { // 弯道
} else if ((last_state != in_state) && in_state == 1) { // 弯道
bt_printf("to 入弯");
set_speed = set_speed0;
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
PID_SetTunings(&far_angle_pid, an_Kp0, an_Ki0, an_Kd0);
PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0);
PID_SetTunings(&pos_pid, cn_Kp1, cn_Ki1, cn_Kd1);
}
if ((last_state != in_state) && (in_state == 3 || in_state == 4)) { // 圆环
} else if ((last_state != in_state) && (in_state == 3 || in_state == 4)) { // 圆环
bt_printf("to 圆环");
if (in_state == 3)
set_speed = set_speed2;
@@ -222,8 +224,7 @@ void sport_motion()
PID_SetTunings(&far_angle_pid, yu_Kp0, yu_Ki0, yu_Kd0);
PID_SetTunings(&far_gyro_pid, ygy_Kp0, ygy_Ki0, ygy_Kd0);
PID_SetTunings(&pos_pid, yuc_Kp, cn_Ki1, cn_Kd1);
}
if ((last_state != in_state) && in_state == 5) { //
} else if ((last_state != in_state) && in_state == 5) { //
bt_printf("to 障碍");
set_speed = set_speed4;
@@ -231,6 +232,11 @@ void sport_motion()
PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd);
PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd);
}
if (in_state == 1) {
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
}
last_state = in_state;
// pid计算
if (cnt1 >= 2) {
@@ -240,17 +246,17 @@ void sport_motion()
if (cnt2 >= 10) {
in_speed = sport_get_speed();
cnt2 = 0;
PID_Compute(&far_angle_pid);
}
if (cnt5 >= 20) {
PID_Compute(&far_angle_pid);
PID_Compute(&pos_pid);
PID_Compute(&speed_pid);
cnt5 = 0;
}
// 并级pid，限幅
pwm_duty_ls = (int32_t)myclip_f(320 - out_pos, 140.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(320 + out_pos, 140.0f, 500.f);
pwm_duty_ls = (int32_t)myclip_f(180 - out_pos, 100.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(180 + out_pos, 100.0f, 500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 1000.0f, 3000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 1000.0f, 3000.f);
if (pwm_duty_lb - last_lb > 1000) {
@@ -273,8 +279,10 @@ void sport_motion()
} else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
// 防止起步拉满
if (bt_run_flag == 0)
by_pwm_power_duty(0, 0, 4000, 4000);
if (bt_run_flag == 0) {
by_pwm_power_duty(680, 680, 4000, 4000);
}
}
}
@@ -287,23 +295,23 @@ 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_SetSampleTime(&far_angle_pid, 10);
PID_SetOutputLimits(&far_angle_pid, -6000.0f, 6000.0f);
// 侧面位置环
PID(&pos_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&pos_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pos_pid, 20);
PID_SetOutputLimits(&pos_pid, -180.0f, 180.0f);
PID_SetOutputLimits(&pos_pid, -320.0f, 320.0f);
/* 角速度控制 */
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, -1500.0f, 1500.0f);
PID_SetOutputLimits(&far_gyro_pid, -2100.0f, 2100.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, 1800.0f);
PID_SetOutputLimits(&speed_pid, -0.0f, 2500.0f);
}