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

pref:弯道速度按曲率分段控制

Browse Source 
增加斑马线积分按键更改距离
优化按键调参加减方向
This commit is contained in:
井明江
2024-08-14 14:30:32 +08:00
parent 06004e9e0b
commit c825014599
10 changed files with 140 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
app/jj_motion.c
View File

@@ -93,7 +93,7 @@ float set_speed3 = 666.f;
float set_speed4 = 720.f;
int cnt1 = 0;
int cnt2 = 0;
int cnt3 = 0;
int cnt3 = 0;
int cnt4 = 0; //
int cnt5 = 0;
int cnt6 = 0; //
@@ -115,7 +115,8 @@ int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
float start_dis = 0;
uint8_t go_cnt = 0;//0 停车 1 测试侧向风扇 2 启动 3成功运行
float set_dis=1.f;
uint8_t go_cnt = 0; // 0 停车 1 测试侧向风扇 2 启动 3成功运行
float bug_sw = 0;
float out_sw = 0;
float sho_sw = 0;
@@ -157,12 +158,12 @@ void sport_motion()
////////////////////////////////////////停车任务///////////////////////////////////////////////
// 抖动停车，计次50
if (imu660ra_acc_z <= 900) {
if (imu660ra_acc_z <= 800) {
cnt7++;
} else {
cnt7 = 0;
}
if (cnt7 >= 10 && (uint8_t)sho_sw == 1) {
if (cnt7 >= 50 && (uint8_t)sho_sw == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("up");
go_cnt = 0;
@@ -183,7 +184,7 @@ void sport_motion()
go_cnt = 0;
}
// 出界停车,已经积分路过斑马线，保证斑马线不会触发
if (2 == in_stop && (start_dis >= 1 || bt_run_flag == 0) && (uint8_t)out_sw == 1) {
if (2 == in_stop && (start_dis >= set_dis|| bt_run_flag == 0) && (uint8_t)out_sw == 1) {
bt_printf("out");
go_cnt = 0;
bt_fly_flag = bt_run_flag = 0;
@@ -195,7 +196,7 @@ void sport_motion()
cnt2++;
cnt5++;
// 斑马线积分,系数随便给的
start_dis += in_speed * 0.000001;
start_dis += in_speed * 0.0000018f;
if (if_start == 0) {
cnt8++;
@@ -239,7 +240,11 @@ void sport_motion()
}
if (in_state == 1) {
set_speed = set_speed0 - fabs(rate_K * powf(myclip_f(in_pos, -50.f, 50.f), 2));
if (fabs(in_pos) <= 12.0f) {
set_speed = set_speed0 - fabs(rate_K / 3 * myclip_f(in_pos, -50.f, 50.f));
} else {
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
}
}
// 记录上一次状态
last_state = in_state;
@@ -260,8 +265,9 @@ void sport_motion()
cnt5 = 0;
}
// 并级pid，限幅
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_ls = (int32_t)myclip_f(240 - out_pos, 100.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(240 + out_pos, 100.0f, 500.f);
// 并级pid的缺点补偿
if (out_speed >= 1000) {
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);
@@ -289,17 +295,18 @@ void sport_motion()
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
if (bt_run_flag == 0) {
// 测试状态
if (go_cnt == 1) {
by_pwm_power_duty(550, 550, 3000, 3000);
}
else if (2 == go_cnt) {
by_pwm_power_duty(550, 550, 4000, 4000);
if (1 == go_cnt) {
by_pwm_power_duty(600, 600, 3000, 3000);
} else if (2 == go_cnt) {
cnt3++;
if(cnt3>=2000)
{
bt_run_flag=1;
go_cnt=3;
cnt3=0;
if (cnt3 < 2000) {
by_pwm_power_duty(600, 600, 3000, 3000);
} else if (cnt3 >= 2000 && cnt3 < 3000) {
by_pwm_power_duty(600, 600, 4000, 4000);
} else if (cnt3 >= 3000) {
bt_run_flag = 1;
go_cnt = 3;
cnt3 = 0;
}
}
}
@@ -323,7 +330,7 @@ void sport_pid_init()
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, -320.0f, 320.0f);
PID_SetOutputLimits(&pos_pid, -260.0f, 260.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是增量