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 in_speed;
float out_speed; float out_speed;
float last_angle;
float set_speed; float set_speed;
float set_speed0 = 657.0f; float set_speed0 = 657.0f;
float set_speed1 = 1111.0f; float set_speed1 = 1111.0f;
@@ -98,6 +98,7 @@ int cnt5 = 0;
int cnt6 = 0; int cnt6 = 0;
int cnt7 = 0; int cnt7 = 0;
int cnt8 = 0; int cnt8 = 0;
int cnt9 = 0;
int if_start = 0; int if_start = 0;
float shock_stop = 0; float shock_stop = 0;
uint8_t in_state = 0; uint8_t in_state = 0;
@@ -141,34 +142,46 @@ float sport_get_speed(void)
void sport_motion() void sport_motion()
{ {
imu660ra_get_gyro(); imu660ra_get_gyro();
imu660ra_get_acc(); imu660ra_get_acc();
get_vol(); get_vol();
in_gyro = imu660ra_gyro_z; in_gyro = imu660ra_gyro_z;
// 抖动停车
if (fabs(out_gyro) < 700) { if (fabs(out_gyro) < 900) {
PID_SetOutputLimits(&speed_pid, -0.0f, 2800.0f); PID_SetOutputLimits(&speed_pid, -0.0f, 3000.0f);
} else { } else {
PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f); PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f);
} }
////////////////////////////////////////停车任务///////////////////////////////////////////////
// 抖动停车
if (imu660ra_acc_z <= 800) { if (imu660ra_acc_z <= 800) {
cnt7++; cnt7++;
} else { } else {
cnt7 = 0; cnt7 = 0;
} }
if (cnt7 >= 100 && shock_stop == 1) { if (cnt7 >= 50 && shock_stop == 1) {
bt_fly_flag = bt_run_flag = 0; 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) { if (1 == in_stop && if_start == 1) {
stop_flag = 1; stop_flag = 1;
} }
if (stop_flag == 1) { if (stop_flag == 1) {
bt_printf("finish");
bt_fly_flag = bt_run_flag = 0; 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) { if (1 == bt_run_flag) {
cnt1++; cnt1++;
@@ -234,8 +247,8 @@ void sport_motion()
cnt5 = 0; cnt5 = 0;
} }
// 并级pid，限幅 // 并级pid，限幅
pwm_duty_ls = (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(250 + out_pos, 50.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_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); 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(&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_SetMode(&pos_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pos_pid, 20); 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是增量 PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); // m是增量

1
app/jj_motion.h
View File

@@ -58,6 +58,7 @@ extern float set_speed3;
extern float set_speed; extern float set_speed;
extern float set_speed4; extern float set_speed4;
extern float shock_stop; extern float shock_stop;
extern float last_angle;
extern int32_t pwm_duty_ls; extern int32_t pwm_duty_ls;
extern int32_t pwm_duty_rs; extern int32_t pwm_duty_rs;
extern int32_t pwm_duty_lb; extern int32_t pwm_duty_lb;

1
app/main.c
View File

@@ -71,6 +71,7 @@ int main(void)
Page_Run(); Page_Run();
by_frame_parse(&test_data[0].u32); by_frame_parse(&test_data[0].u32);
jj_bt_run(); jj_bt_run();
last_angle=in_angle;
in_pos = 1000.f * test_data[1].f32; in_pos = 1000.f * test_data[1].f32;
in_angle = test_data[0].f32; in_angle = test_data[0].f32;
in_state = test_data[2].u8[0]; in_state = test_data[2].u8[0];