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-05 14:25:08 +08:00
parent 34aae6602a
commit dda2649f6d
4 changed files with 27 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
app/jj_motion.c
View File

@@ -6,6 +6,7 @@
#include "jj_blueteeth.h"
#include "by_fan_control.h"
#include "by_imu.h"
#include "by_frame.h"
PID_TypeDef far_angle_pid;
PID_TypeDef pos_pid;
@@ -103,9 +104,9 @@ int32_t pwm_duty_ls;
int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
int stop_have = 0;
float qd_down = 0.0f;
float last_gy = 0.0f;
uint32_t stop_have = 0;
float qd_down = 0.0f;
float last_gy = 0.0f;
/*
0:无状态
1:弯道
@@ -146,16 +147,15 @@ void sport_motion()
if (imu660ra_acc_z <= 800) {
cnt7++;
} else {
cnt7 = 0;
}
if (cnt7 >= 50) {
if (cnt7 >= 100) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("ting");
}
// 斑马线停车
if (1 == in_stop && stop_have == 1) {
if (1 == in_stop) {
stop_flag = 1;
}
if (stop_flag == 1) {
@@ -169,10 +169,10 @@ void sport_motion()
cnt1++;
cnt2++;
cnt5++;
cnt8++;
if (cnt8 >= 10000) {
// 屏蔽斑马线
if (stop_have == 0) {
stop_have = 1;
cnt8=0;
by_frame_send(&stop_have);
}
// pid参数切换
if ((last_state != in_state) && (in_state == 0 || in_state == 2)) { // 直道
@@ -231,8 +231,8 @@ void sport_motion()
// 并级pid，限幅
pwm_duty_ls = (int32_t)myclip_f(300 - out_pos, 100.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(300 + out_pos, 100.0f, 500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 5000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 5000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 6000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 6000.f);
by_pwm_power_duty(500 + pwm_duty_ls, 500 + pwm_duty_rs, pwm_duty_lb, pwm_duty_rb);
@@ -248,14 +248,14 @@ void sport_motion()
cnt3++;
if (in_state == 3) {
if (cnt3 >= 100) // 200ms
if (cnt3 >= 1000) // 200ms
{
bt_printf("to 环内");
cnt3_flag = 2;
cnt3 = 0;
}
if (in_speed > set_speed) {
qd_down = 50;
qd_down = 50;
} else if (in_speed > set_speed - 100) {
qd_down = 20;
} else {
@@ -293,12 +293,12 @@ void sport_motion()
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
} else {
if (in_state == 1 ) {
qd_down = 40;
} else if(in_state == 3){
qd_down = 70;
}else{
qd_down=0;
if (in_state == 1) {
qd_down = 50;
} else if (in_state == 3) {
qd_down = 10;
} else {
qd_down = 0;
}
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
@@ -327,11 +327,11 @@ void sport_pid_init()
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_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_gyro_pid, 20);
PID_SetOutputLimits(&far_gyro_pid, -5000.0f, 5000.0f);
PID_SetOutputLimits(&far_gyro_pid, -6000.0f, 6000.0f);
/* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT);
PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&speed_pid, 20);
PID_SetOutputLimits(&speed_pid, -0.0f, 4500.0f);
PID_SetOutputLimits(&speed_pid, -0.0f, 6000.0f);
}