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

适配新班子

Browse Source
This commit is contained in:
井明江
2024-06-30 14:44:35 +08:00
parent ac05ce6f32
commit 50b5cd3f2d
3 changed files with 42 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
app/by_fan_control.c
View File

@@ -12,11 +12,11 @@
#define FAN_LB_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // 右后 #define FAN_LB_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // 右后
#define FAN_LB_PWM_B_PIN TIM4_PWM_MAP1_CH3_D14 // 右后 #define FAN_LB_PWM_B_PIN TIM4_PWM_MAP1_CH3_D14 // 右后
// M3 // M3
#define FAN_RS_PWM_B_PIN TIM10_PWM_MAP3_CH3_D5 // 左侧 #define FAN_RS_PWM_B_PIN TIM10_PWM_MAP3_CH1_D1 // 右侧
#define FAN_RS_PWM_A_PIN TIM10_PWM_MAP3_CH1_D1 // 左侧 #define FAN_RS_PWM_A_PIN TIM10_PWM_MAP3_CH3_D5 // you侧
// M2 // M2
#define FAN_LS_PWM_B_PIN TIM4_PWM_MAP1_CH4_D15 // 右侧 #define FAN_LS_PWM_B_PIN TIM4_PWM_MAP1_CH4_D15 // 左侧
#define FAN_LS_PWM_A_PIN TIM4_PWM_MAP1_CH2_D13 // 右侧 #define FAN_LS_PWM_A_PIN TIM4_PWM_MAP1_CH2_D13 // 左侧
int32_t pwm_duty_ls_g; int32_t pwm_duty_ls_g;
int32_t pwm_duty_rs_g; int32_t pwm_duty_rs_g;
@@ -67,7 +67,7 @@ void by_pwm_init(void)
pwm_init(FAN_RS_PWM_B_PIN, 20000, 0); pwm_init(FAN_RS_PWM_B_PIN, 20000, 0);
pwm_init(FAN_LB_PWM_A_PIN, 20000, 0); pwm_init(FAN_LB_PWM_A_PIN, 20000, 0);
pwm_init(FAN_LB_PWM_B_PIN, 20000, 0); pwm_init(FAN_LB_PWM_B_PIN, 20000, 0);
pwm_init(FAN_RB_PWM_A_PIN, 20000, 1000); pwm_init(FAN_RB_PWM_A_PIN, 20000, 0);
pwm_init(FAN_RB_PWM_B_PIN, 20000, 0); pwm_init(FAN_RB_PWM_B_PIN, 20000, 0);
while (1); while (1);
#endif #endif
@@ -105,10 +105,10 @@ 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, -9000, 9000); bpwm_duty_ls = clip_s32(bpwm_duty_ls, -0, 7000);
bpwm_duty_rs = clip_s32(bpwm_duty_rs, -9000, 9000); bpwm_duty_rs = clip_s32(bpwm_duty_rs, -0, 7000);
bpwm_duty_lb = clip_s32(bpwm_duty_lb, -9000, 9000); bpwm_duty_lb = clip_s32(bpwm_duty_lb, -0, 7000);
bpwm_duty_rb = clip_s32(bpwm_duty_rb, -9000, 9000); bpwm_duty_rb = clip_s32(bpwm_duty_rb, -0, 7000);
// pwm_duty_ls_g = pwm_duty_ls; // pwm_duty_ls_g = pwm_duty_ls;
// pwm_duty_rs_g = pwm_duty_rs; // pwm_duty_rs_g = pwm_duty_rs;

4
app/jj_blueteeth.c
View File

@@ -40,10 +40,10 @@ void jj_bt_run()
bt_run_flag = !bt_run_flag; bt_run_flag = !bt_run_flag;
break; break;
case Fly_up: case Fly_up:
bt_fly += 25; bt_fly += 10;
break; break;
case Fly_dowm: case Fly_dowm:
bt_fly -= 25; bt_fly -= 10;
break; break;
case Speed_up: case Speed_up:
bt_run += 50; bt_run += 50;

38
app/jj_motion.c
View File

@@ -79,6 +79,8 @@ int cnt2 = 0;
int cnt3 = 0; int cnt3 = 0;
int cnt4 = 0; int cnt4 = 0;
int cnt5 = 0; int cnt5 = 0;
int cnt6 = 0;
uint8_t cnt3_flag = 0; uint8_t cnt3_flag = 0;
uint8_t in_state = 0; uint8_t in_state = 0;
uint8_t in_stop = 0; uint8_t in_stop = 0;
@@ -88,8 +90,9 @@ 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;
/* /*
0:无状态 0:无状态
1:弯道 1:弯道
@@ -195,19 +198,20 @@ void sport_motion()
} }
// 输出限幅 // 输出限幅
if (in_state == 0 || in_state == 2) { if (in_state == 0 || in_state == 2) {
pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 7000.f); pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 6500.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 7000.f); pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 6500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f); pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f); pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f);
} else if (in_state == 1 || in_state == 3 || in_state == 4 || in_state == 5) { } else if (in_state == 1 || in_state == 3 || in_state == 4 || in_state == 5) {
pwm_duty_ls = (int32_t)myclip_f(-out_cal, 0.0f, 7000.f); pwm_duty_ls = (int32_t)myclip_f(-out_cal, 0.0f, 6500.f);
pwm_duty_rs = (int32_t)myclip_f(out_cal, 0.0f, 7000.f); pwm_duty_rs = (int32_t)myclip_f(out_cal, 0.0f, 6500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f); pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f); pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f);
} }
by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb); by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb);
} else { } else {
by_pwm_power_duty(0, 0, 0, 0); by_pwm_power_duty(0, 0, 0, 0);
} }
@@ -241,9 +245,11 @@ void sport_motion()
} }
} }
if (in_speed > set_speed - 100) { if (in_speed > set_speed - 100) {
qd_down = 6 * fabs(in_pos); qd_down = 8* fabs(in_pos);
} else { } else if(in_speed > set_speed - 250){
qd_down = 0; qd_down = 4* fabs(in_pos);
}else{
qd_down=0;
} }
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down); by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
} else { } else {
@@ -267,22 +273,22 @@ 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, 20);
PID_SetOutputLimits(&far_angle_pid, -4000.0f, 7000.0f); PID_SetOutputLimits(&far_angle_pid, -4000.0f, 4000.0f);
// 直道侧面位置环 // 直道侧面位置环
PID(&str_angle_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); PID(&str_angle_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&str_angle_pid, _PID_MODE_AUTOMATIC); PID_SetMode(&str_angle_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&str_angle_pid, 20); PID_SetSampleTime(&str_angle_pid, 20);
PID_SetOutputLimits(&str_angle_pid, -7000.0f, 7000.0f); PID_SetOutputLimits(&str_angle_pid, -6500.0f, 6500.0f);
// 弯道侧面曲率环 // 弯道侧面曲率环
PID(&tur_cal_pid, &in_angle, &out_cal, &set_pos, cn_Kp1, cn_Ki1, cn_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); PID(&tur_cal_pid, &in_angle, &out_cal, &set_pos, cn_Kp1, cn_Ki1, cn_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&tur_cal_pid, _PID_MODE_AUTOMATIC); PID_SetMode(&tur_cal_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&tur_cal_pid, 20); PID_SetSampleTime(&tur_cal_pid, 20);
PID_SetOutputLimits(&tur_cal_pid, -7000.0f, 7000.0f); PID_SetOutputLimits(&tur_cal_pid, -6500.0f, 6500.0f);
// 圆环 // // 圆环
PID(&cir_pos_pid, &in_angle, &out_ci, &set_pos, ci_Kp0, ci_Ki0, ci_Kd0, _PID_P_ON_E, _PID_CD_REVERSE); // PID(&cir_pos_pid, &in_angle, &out_ci, &set_pos, ci_Kp0, ci_Ki0, ci_Kd0, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&cir_pos_pid, _PID_MODE_AUTOMATIC); // PID_SetMode(&cir_pos_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&cir_pos_pid, 20); // PID_SetSampleTime(&cir_pos_pid, 20);
PID_SetOutputLimits(&cir_pos_pid, -7000.0f, 7000.0f); // PID_SetOutputLimits(&cir_pos_pid, -7000.0f, 7000.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);