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

新桨叶完赛

Browse Source
This commit is contained in:
井明江
2024-07-01 19:07:11 +08:00
parent 50b5cd3f2d
commit 8b5799302b
6 changed files with 41 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
app/jj_motion.c
View File

@@ -18,23 +18,23 @@ float ci_Ki0 = 0;
float ci_Kd0 = 0;
float out_ci;
// 弯道后面风扇角度环
float an_Kp0 = 45.0f;
float an_Kp0 = 54.0f;
float an_Ki0 = 0.0f;
float an_Kd0 = 0.0f;
// 直道后面风扇角度环
float an_Kp1 = 35.0f;
float an_Kp1 = 45.0f;
float an_Ki1 = 0.0f;
float an_Kd1 = 0.0f;
// 圆环后面风扇角度环
float yu_Kp0 = 51.0f;
float yu_Kp0 = 53.0f;
float yu_Ki0 = 0.0f;
float yu_Kd0 = 0.0f;
// 弯道侧面风扇角度环
float cn_Kp1 = 400.0f;
float cn_Kp1 = 300.0f;
float cn_Ki1 = 0.f;
float cn_Kd1 = 0.0f;
// 直道侧面风扇角度环
float po_Kp1 = 400.0f;
float po_Kp1 = 250.0f;
float po_Ki1 = 0.0f;
float po_Kd1 = 0.0f;
// 期望和当前量
@@ -46,15 +46,15 @@ float out_pos;
float set_pos = 0.0f;
float out_cal;
// 弯道角速度环
float gy_Kp0 = 4.80f;
float gy_Kp0 = 8.80f;
float gy_Ki0 = 0.0f;
float gy_Kd0 = 0.0f;
// 直道角速度环
float gy_Kp1 = 6.0f;
float gy_Kp1 = 7.0f;
float gy_Ki1 = 0.0f;
float gy_Kd1 = 0.0f;
// 圆环角速度环
float ygy_Kp0 = 6.0f;
float ygy_Kp0 = 7.0f;
float ygy_Ki0 = 0.0f;
float ygy_Kd0 = 0.0f;
// 当前和输入量
@@ -63,7 +63,7 @@ float out_gyro;
// float set_gyro = 0.0f;
// 速度环
float sp_Kp = 10.0f;
float sp_Ki = 100.f;
float sp_Ki = 80.f;
float sp_Kd = 0.0f;
float in_speed;
@@ -73,7 +73,7 @@ float set_speed;
float set_speed0 = 900.0f;
float set_speed1 = 1100.0f;
float set_speed2 = 830.f;
float set_speed3 = 1000.f;
float set_speed3 = 900.f;
int cnt1 = 0;
int cnt2 = 0;
int cnt3 = 0;
@@ -198,14 +198,14 @@ void sport_motion()
}
// 输出限幅
if (in_state == 0 || in_state == 2) {
pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 6500.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 6500.f);
pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 7000.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 7000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f);
} else if (in_state == 1 || in_state == 3 || in_state == 4 || in_state == 5) {
pwm_duty_ls = (int32_t)myclip_f(-out_cal, 0.0f, 6500.f);
pwm_duty_rs = (int32_t)myclip_f(out_cal, 0.0f, 6500.f);
pwm_duty_ls = (int32_t)myclip_f(-out_cal, 0.0f, 7000.f);
pwm_duty_rs = (int32_t)myclip_f(out_cal, 0.0f, 7000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f);
}
@@ -223,14 +223,14 @@ void sport_motion()
cnt3++;
if (in_state == 3) {
if (cnt3 >= 300) // 200ms
if (cnt3 >= 400) // 200ms
{
bt_printf("to 环内");
cnt3_flag = 2;
cnt3 = 0;
}
} else if (in_state == 1) {
if (cnt3 >= 450) // 200ms
if (cnt3 >= 500) // 200ms
{
bt_printf("to 弯道");
cnt3_flag = 2;
@@ -245,19 +245,24 @@ void sport_motion()
}
}
if (in_speed > set_speed - 100) {
qd_down = 8* fabs(in_pos);
} else if(in_speed > set_speed - 250){
qd_down = 4* fabs(in_pos);
}else{
qd_down=0;
qd_down = 5 * fabs(in_pos) + 50;
} else if (in_speed > set_speed - 200) {
qd_down = 2.5 * fabs(in_pos)+10;
} else {
qd_down = 0;
}
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
} else {
if(in_state==1) {
qd_down = 15;
}else{
qd_down=0;
}
if (in_state == 4) {
by_pwm_update_duty(bt_fly + 515, bt_fly + 515);
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
set_speed = set_speed3;
} else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
}
}
}
@@ -278,12 +283,12 @@ void sport_pid_init()
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);
PID_SetSampleTime(&str_angle_pid, 20);
PID_SetOutputLimits(&str_angle_pid, -6500.0f, 6500.0f);
PID_SetOutputLimits(&str_angle_pid, -6000.0f, 6000.0f);
// 弯道侧面曲率环
PID(&tur_cal_pid, &in_angle, &out_cal, &set_pos, cn_Kp1, cn_Ki1, cn_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&tur_cal_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&tur_cal_pid, 20);
PID_SetOutputLimits(&tur_cal_pid, -6500.0f, 6500.0f);
PID_SetOutputLimits(&tur_cal_pid, -6000.0f, 6000.0f);
// // 圆环
// PID(&cir_pos_pid, &in_angle, &out_ci, &set_pos, ci_Kp0, ci_Ki0, ci_Kd0, _PID_P_ON_E, _PID_CD_REVERSE);
// PID_SetMode(&cir_pos_pid, _PID_MODE_AUTOMATIC);