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

feat:

Browse Source 
增加发车和停车菜单
增加停车相关条件开关功能
pref:
弯道速度控制一次函数->二次函数
删除图像下发无意义的0状态
fix:
修复下位机多次复位才能触发上位机复位
This commit is contained in:
井明江
2024-08-12 18:08:46 +08:00
parent 92eaf372ff
commit 06004e9e0b
14 changed files with 378 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
app/jj_motion.c
View File

@@ -86,23 +86,23 @@ float in_speed;
float out_speed;
float last_angle;
float set_speed;
float set_speed0 = 657.0f;
float set_speed1 = 1111.0f;
float set_speed2 = 815.f;
float set_speed3 = 666.f;
float set_speed4 = 720.f;
int cnt1 = 0;
int cnt2 = 0;
int cnt3 = 0; //
int cnt4 = 0; //
int cnt5 = 0;
int cnt6 = 0; //
int cnt7 = 0;
int cnt8 = 0;
int cnt9 = 0; //
int cnt10 = 0; //
int if_start = 0;
float shock_stop = 0;
float set_speed0 = 657.0f;
float set_speed1 = 1111.0f;
float set_speed2 = 815.f;
float set_speed3 = 666.f;
float set_speed4 = 720.f;
int cnt1 = 0;
int cnt2 = 0;
int cnt3 = 0;
int cnt4 = 0; //
int cnt5 = 0;
int cnt6 = 0; //
int cnt7 = 0;
int cnt8 = 0;
int cnt9 = 0; //
int cnt10 = 0; //
int if_start = 0;
uint8_t in_state = 0;
uint8_t in_stop = 0;
uint8_t last_state = 0;
@@ -111,12 +111,14 @@ int32_t pwm_duty_ls;
int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
float start_dis = 0;
int32_t game_over_flag = 0; // 任务结束
uint8_t go_flag = 0;
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 bug_sw = 0;
float out_sw = 0;
float sho_sw = 0;
/*
0:无状态
1:弯道
@@ -154,33 +156,36 @@ void sport_motion()
in_gyro = imu660ra_gyro_z;
////////////////////////////////////////停车任务///////////////////////////////////////////////
// 抖动停车
if (imu660ra_acc_z <= 800) {
// 抖动停车，计次50
if (imu660ra_acc_z <= 900) {
cnt7++;
} else {
cnt7 = 0;
}
if (cnt7 >= 50 && ((uint32_t)shock_stop == 1 || (uint32_t)shock_stop == 2)) {
if (cnt7 >= 10 && (uint8_t)sho_sw == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("up");
go_cnt = 0;
}
// 異常值停车
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 && (uint8_t)bug_sw == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("bug");
go_cnt = 0;
}
// 斑马线停车,摄像头识别
// 斑马线停车,摄像头识别，屏蔽发车前的十秒钟
if (1 == in_stop && if_start == 1) {
stop_flag = 1;
}
if (stop_flag == 1) {
bt_printf("finish");
bt_fly_flag = bt_run_flag = 0;
game_over_flag = 1;
go_cnt = 0;
}
// 出界停车,已经积分路过斑马线，保证斑马线不会触发
if (2 == in_stop && start_dis >= 1) {
if (2 == in_stop && (start_dis >= 1 || bt_run_flag == 0) && (uint8_t)out_sw == 1) {
bt_printf("out");
go_cnt = 0;
bt_fly_flag = bt_run_flag = 0;
}
@@ -189,15 +194,17 @@ void sport_motion()
cnt1++;
cnt2++;
cnt5++;
// 斑马线积分,系数随便给的
start_dis += in_speed * 0.000001;
if (if_start == 0) {
cnt8++;
start_dis += in_speed * 0.000001; // 斑马线积分
if (cnt8 >= 5000) {
if_start = 1;
}
}
// pid参数切换
if ((last_state != in_state) && (in_state == 0 || in_state == 2)) { // 直道
if ((last_state != in_state) && (in_state == 2)) { // 直道
bt_printf("to 直道");
set_speed = set_speed1;
@@ -232,9 +239,9 @@ void sport_motion()
}
if (in_state == 1) {
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -100.f, 100.f));
set_speed = set_speed0 - fabs(rate_K * powf(myclip_f(in_pos, -50.f, 50.f), 2));
}
// 记录上一次状态
last_state = in_state;
// pid计算
if (cnt1 >= 2) {
@@ -278,28 +285,26 @@ void sport_motion()
by_pwm_power_duty(0, 0, 0, 0);
}
///////////////////////////////////////////////////* 升力风扇设置 */ /////////////////////////////////////////////////////////////////////////////////////////////////
if (bt_fly_flag == 0) {
if (game_over_flag == 0) {
by_pwm_update_duty(500, 500);
} else {
by_pwm_update_duty(700, 700);
if (bt_fly_flag == 1) {
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);
cnt3++;
if(cnt3>=2000)
{
bt_run_flag=1;
go_cnt=3;
cnt3=0;
}
}
}
} else {
if (in_state == 1) {
by_pwm_update_duty(bt_fly + 500 + in_pos, bt_fly + 500 + in_pos);
} else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
}
// 防止起步拉满
if (bt_run_flag == 0&&go_flag==1) {
by_pwm_power_duty(550, 550, 3000, 3000);
}
else if(bt_run_flag == 0&&go_flag==2)
{
by_pwm_power_duty(550, 550, 4000, 4000);
}
by_pwm_update_duty(500, 500);
}
}