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

更改限幅

Browse Source
This commit is contained in:
井明江
2024-08-09 17:00:24 +08:00
parent 936f775dbd
commit 53d06467d6
5 changed files with 47 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app/by_fan_control.c
View File

@@ -70,8 +70,8 @@ void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2)
*/ */
void by_pwm_power_duty(int32_t bpwm_duty_ls, int32_t bpwm_duty_rs, int32_t bpwm_duty_lb, int32_t bpwm_duty_rb) void by_pwm_power_duty(int32_t bpwm_duty_ls, int32_t bpwm_duty_rs, int32_t bpwm_duty_lb, int32_t bpwm_duty_rb)
{ {
bpwm_duty_ls = clip_s32(bpwm_duty_ls, 500, 1000); bpwm_duty_ls = clip_s32(bpwm_duty_ls, 500, 980);
bpwm_duty_rs = clip_s32(bpwm_duty_rs, 500, 1000); bpwm_duty_rs = clip_s32(bpwm_duty_rs, 500, 980);
bpwm_duty_lb = clip_s32(bpwm_duty_lb, 2500, 6000); bpwm_duty_lb = clip_s32(bpwm_duty_lb, 2500, 6000);
bpwm_duty_rb = clip_s32(bpwm_duty_rb, 2500, 6000); bpwm_duty_rb = clip_s32(bpwm_duty_rb, 2500, 6000);
pwm_set_duty(FAN_LS_PWM_PIN, bpwm_duty_ls); pwm_set_duty(FAN_LS_PWM_PIN, bpwm_duty_ls);

74
app/jj_motion.c
View File

@@ -64,7 +64,7 @@ float ygy_Kp0 = 2.20f;
float ygy_Ki0 = 0.0f; float ygy_Ki0 = 0.0f;
float ygy_Kd0 = 0.0f; float ygy_Kd0 = 0.0f;
// 圆环侧面角度环 // 圆环侧面角度环
float yuc_Kp=4.4f; float yuc_Kp = 4.4f;
// 速度环 // 速度环
float sp_Kp = 3.40f; float sp_Kp = 3.40f;
float sp_Ki = 3.6f; float sp_Ki = 3.6f;
@@ -81,7 +81,7 @@ float in_pos;
float out_pos; float out_pos;
float set_pos = 0.0f; float set_pos = 0.0f;
// float set_gyro = 0.0f; // float set_gyro = 0.0f;
float rate_K = 0;
float in_speed; float in_speed;
float out_speed; float out_speed;
float last_angle; float last_angle;
@@ -100,6 +100,7 @@ int cnt6 = 0;
int cnt7 = 0; int cnt7 = 0;
int cnt8 = 0; int cnt8 = 0;
int cnt9 = 0; int cnt9 = 0;
int cnt10 = 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;
@@ -110,10 +111,11 @@ int32_t pwm_duty_ls;
int32_t pwm_duty_rs; int32_t pwm_duty_rs;
int32_t pwm_duty_lb; int32_t pwm_duty_lb;
int32_t pwm_duty_rb; int32_t pwm_duty_rb;
float qd_down = 0.0f; float qd_down = 0.0f;
float last_gy = 0.0f; float last_gy = 0.0f;
int32_t last_lb = 0; int32_t stop_cnt = 0;
int32_t last_rb = 0; int32_t last_lb = 0;
int32_t last_rb = 0;
/* /*
0:无状态 0:无状态
1:弯道 1:弯道
@@ -150,11 +152,6 @@ void sport_motion()
get_vol(); get_vol();
in_gyro = imu660ra_gyro_z; in_gyro = imu660ra_gyro_z;
if (fabs(out_gyro) < 700) {
PID_SetOutputLimits(&speed_pid, -0.0f, 2500.0f);
} else {
PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f);
}
////////////////////////////////////////停车任务/////////////////////////////////////////////// ////////////////////////////////////////停车任务///////////////////////////////////////////////
// 抖动停车 // 抖动停车
if (imu660ra_acc_z <= 800) { if (imu660ra_acc_z <= 800) {
@@ -162,12 +159,12 @@ void sport_motion()
} else { } else {
cnt7 = 0; cnt7 = 0;
} }
if (cnt7 >= 50 && ((uint32_t)shock_stop ==1||(uint32_t)shock_stop == 2)) { if (cnt7 >= 50 && ((uint32_t)shock_stop == 1 || (uint32_t)shock_stop == 2)) {
bt_fly_flag = bt_run_flag = 0; bt_fly_flag = bt_run_flag = 0;
bt_printf("up"); bt_printf("up");
} }
// 異常值停车 // 異常值停车
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 && ((uint32_t)shock_stop == 1 || (uint32_t)shock_stop == 3)) {
bt_fly_flag = bt_run_flag = 0; bt_fly_flag = bt_run_flag = 0;
bt_printf("bug"); bt_printf("bug");
} }
@@ -181,8 +178,14 @@ void sport_motion()
} }
// 出界停车 // 出界停车
if (2 == in_stop) { if (2 == in_stop) {
bt_printf("out"); stop_cnt++;
bt_fly_flag = bt_run_flag = 0; if (stop_cnt >= 5) {
bt_printf("out");
stop_cnt = 0;
bt_fly_flag = bt_run_flag = 0;
}
} else {
stop_cnt = 0;
} }
//////////////////////////////////////////////// 动力风扇设置 *///////////////////////////////////////////////////////// //////////////////////////////////////////////// 动力风扇设置 */////////////////////////////////////////////////////////
if (1 == bt_run_flag) { if (1 == bt_run_flag) {
@@ -191,7 +194,7 @@ void sport_motion()
cnt5++; cnt5++;
if (if_start == 0) { if (if_start == 0) {
cnt8++; cnt8++;
if (cnt8 >= 10000) { if (cnt8 >= 5000) {
if_start = 1; if_start = 1;
} }
} }
@@ -200,19 +203,18 @@ void sport_motion()
bt_printf("to 直道"); bt_printf("to 直道");
set_speed = set_speed1; set_speed = set_speed1;
PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1); PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1);
PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1); PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1);
PID_SetTunings(&pos_pid, po_Kp1, po_Ki1, po_Kd1); PID_SetTunings(&pos_pid, po_Kp1, po_Ki1, po_Kd1);
} } else if ((last_state != in_state) && in_state == 1) { // 弯道
if ((last_state != in_state) && in_state == 1) { // 弯道
bt_printf("to 入弯"); bt_printf("to 入弯");
set_speed = set_speed0; set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
PID_SetTunings(&far_angle_pid, an_Kp0, an_Ki0, an_Kd0); PID_SetTunings(&far_angle_pid, an_Kp0, an_Ki0, an_Kd0);
PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0); PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0);
PID_SetTunings(&pos_pid, cn_Kp1, cn_Ki1, cn_Kd1); PID_SetTunings(&pos_pid, cn_Kp1, cn_Ki1, cn_Kd1);
} } else if ((last_state != in_state) && (in_state == 3 || in_state == 4)) { // 圆环
if ((last_state != in_state) && (in_state == 3 || in_state == 4)) { // 圆环
bt_printf("to 圆环"); bt_printf("to 圆环");
if (in_state == 3) if (in_state == 3)
set_speed = set_speed2; set_speed = set_speed2;
@@ -222,8 +224,7 @@ void sport_motion()
PID_SetTunings(&far_angle_pid, yu_Kp0, yu_Ki0, yu_Kd0); PID_SetTunings(&far_angle_pid, yu_Kp0, yu_Ki0, yu_Kd0);
PID_SetTunings(&far_gyro_pid, ygy_Kp0, ygy_Ki0, ygy_Kd0); PID_SetTunings(&far_gyro_pid, ygy_Kp0, ygy_Ki0, ygy_Kd0);
PID_SetTunings(&pos_pid, yuc_Kp, cn_Ki1, cn_Kd1); PID_SetTunings(&pos_pid, yuc_Kp, cn_Ki1, cn_Kd1);
} } else if ((last_state != in_state) && in_state == 5) { //
if ((last_state != in_state) && in_state == 5) { //
bt_printf("to 障碍"); bt_printf("to 障碍");
set_speed = set_speed4; set_speed = set_speed4;
@@ -231,6 +232,11 @@ void sport_motion()
PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd); PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd);
PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd); PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd);
} }
if (in_state == 1) {
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
}
last_state = in_state; last_state = in_state;
// pid计算 // pid计算
if (cnt1 >= 2) { if (cnt1 >= 2) {
@@ -240,17 +246,17 @@ void sport_motion()
if (cnt2 >= 10) { if (cnt2 >= 10) {
in_speed = sport_get_speed(); in_speed = sport_get_speed();
cnt2 = 0; cnt2 = 0;
PID_Compute(&far_angle_pid);
} }
if (cnt5 >= 20) { if (cnt5 >= 20) {
PID_Compute(&far_angle_pid);
PID_Compute(&pos_pid); PID_Compute(&pos_pid);
PID_Compute(&speed_pid); PID_Compute(&speed_pid);
cnt5 = 0; cnt5 = 0;
} }
// 并级pid，限幅 // 并级pid，限幅
pwm_duty_ls = (int32_t)myclip_f(320 - out_pos, 140.0f, 500.f); pwm_duty_ls = (int32_t)myclip_f(180 - out_pos, 100.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(320 + out_pos, 140.0f, 500.f); pwm_duty_rs = (int32_t)myclip_f(180 + out_pos, 100.0f, 500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 1000.0f, 3000.f); 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); pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 1000.0f, 3000.f);
if (pwm_duty_lb - last_lb > 1000) { if (pwm_duty_lb - last_lb > 1000) {
@@ -273,8 +279,10 @@ void sport_motion()
} else { } else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500); by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
// 防止起步拉满 // 防止起步拉满
if (bt_run_flag == 0) if (bt_run_flag == 0) {
by_pwm_power_duty(0, 0, 4000, 4000);
by_pwm_power_duty(680, 680, 4000, 4000);
}
} }
} }
@@ -287,23 +295,23 @@ void sport_pid_init()
/* 角度控制 */ /* 角度控制 */
PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp1, an_Ki1, an_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp1, an_Ki1, an_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC); PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_angle_pid, 20); PID_SetSampleTime(&far_angle_pid, 10);
PID_SetOutputLimits(&far_angle_pid, -4000.0f, 4000.0f); PID_SetOutputLimits(&far_angle_pid, -6000.0f, 6000.0f);
// 侧面位置环 // 侧面位置环
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, -180.0f, 180.0f); PID_SetOutputLimits(&pos_pid, -320.0f, 320.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是增量
PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC); PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_gyro_pid, 20); PID_SetSampleTime(&far_gyro_pid, 20);
PID_SetOutputLimits(&far_gyro_pid, -1500.0f, 1500.0f); PID_SetOutputLimits(&far_gyro_pid, -2100.0f, 2100.0f);
/* 速度控制 */ /* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT); 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_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&speed_pid, 20); PID_SetSampleTime(&speed_pid, 20);
PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f); PID_SetOutputLimits(&speed_pid, -0.0f, 2500.0f);
} }

1
app/jj_motion.h
View File

@@ -60,6 +60,7 @@ extern float set_speed4;
extern float shock_stop; extern float shock_stop;
extern float last_angle; extern float last_angle;
extern float yuc_Kp; extern float yuc_Kp;
extern float rate_K;
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;

2
app/jj_param.c
View File

@@ -28,7 +28,7 @@ void jj_param_eeprom_init(void)
PARAM_REG(can_Kp1, &cn_Kp1, EFLOAT, 1, "cn_P1:"); // 注冊 PARAM_REG(can_Kp1, &cn_Kp1, EFLOAT, 1, "cn_P1:"); // 注冊
PARAM_REG(can_Ki1, &yuc_Kp, EFLOAT, 1, "yu_P1:"); // 注冊 PARAM_REG(can_Ki1, &yuc_Kp, EFLOAT, 1, "yu_P1:"); // 注冊
PARAM_REG(can_Kd1, &cn_Kd1, EFLOAT, 1, "cn_D1:"); PARAM_REG(can_Kd1, &rate_K, EFLOAT, 1, "ra_K:");
PARAM_REG(fly_pwm, &bt_fly, EFLOAT, 1, "fly:"); PARAM_REG(fly_pwm, &bt_fly, EFLOAT, 1, "fly:");

2
app/page/page_dparam.c
View File

@@ -62,6 +62,8 @@ static void Loop()
ips200_show_float(80, 160, out_pos, 4, 1); ips200_show_float(80, 160, out_pos, 4, 1);
ips200_show_string(0, 180, "vol"); ips200_show_string(0, 180, "vol");
ips200_show_float(80, 180, (float)now_vol, 4,2); ips200_show_float(80, 180, (float)now_vol, 4,2);
ips200_show_string(0, 200, "setsp");
ips200_show_float(80, 200, set_speed, 4,2);
ips200_show_string(180, 0, "ls"); ips200_show_string(180, 0, "ls");
ips200_show_float(220, 0, pwm_duty_ls, 4, 1); ips200_show_float(220, 0, pwm_duty_ls, 4, 1);