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

fear: 增加异常停车

Browse Source
This commit is contained in:
井明江
2024-07-09 19:32:36 +08:00
parent 73a30fc187
commit 5a0c3ef803
3 changed files with 26 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
app/jj_motion.c
View File

@@ -83,7 +83,7 @@ float set_pos = 0.0f;
float in_speed;
float out_speed;
float last_angle;
float set_speed;
float set_speed0 = 657.0f;
float set_speed1 = 1111.0f;
@@ -98,6 +98,7 @@ int cnt5 = 0;
int cnt6 = 0;
int cnt7 = 0;
int cnt8 = 0;
int cnt9 = 0;
int if_start = 0;
float shock_stop = 0;
uint8_t in_state = 0;
@@ -141,34 +142,46 @@ float sport_get_speed(void)
void sport_motion()
{
imu660ra_get_gyro();
imu660ra_get_acc();
get_vol();
in_gyro = imu660ra_gyro_z;
// 抖动停车
if (fabs(out_gyro) < 700) {
PID_SetOutputLimits(&speed_pid, -0.0f, 2800.0f);
if (fabs(out_gyro) < 900) {
PID_SetOutputLimits(&speed_pid, -0.0f, 3000.0f);
} else {
PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f);
}
////////////////////////////////////////停车任务///////////////////////////////////////////////
// 抖动停车
if (imu660ra_acc_z <= 800) {
cnt7++;
} else {
cnt7 = 0;
}
if (cnt7 >= 100 && shock_stop == 1) {
if (cnt7 >= 50 && shock_stop == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("ting");
bt_printf("up");
}
// 異常值停车
if (fabs(in_angle - last_angle) > 45.f) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("bug");
}
// 斑马线停车,摄像头识别
if (1 == in_stop && if_start == 1) {
stop_flag = 1;
}
if (stop_flag == 1) {
bt_printf("finish");
bt_fly_flag = bt_run_flag = 0;
}
// 出界停车
if (2 == in_stop) {
bt_printf("over");
bt_fly_flag = bt_run_flag = 0;
}
///////////////////////////////////////////////////////////////////////////////////////
/* 动力风扇设置 */
if (1 == bt_run_flag) {
cnt1++;
@@ -234,8 +247,8 @@ void sport_motion()
cnt5 = 0;
}
// 并级pid，限幅
pwm_duty_ls = (int32_t)myclip_f(250 - out_pos, 50.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(250 + out_pos, 50.0f, 500.f);
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_lb = (int32_t)myclip_f(out_speed + out_gyro, 300.0f, 3000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 300.0f, 3000.f);
@@ -276,7 +289,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, -250.0f, 250.0f);
PID_SetOutputLimits(&pos_pid, -180.0f, 180.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是增量