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-01 19:07:11 +08:00
parent 50b5cd3f2d
commit 8b5799302b
6 changed files with 41 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
app/by_fan_control.c
View File

@@ -6,11 +6,11 @@
#define FAN_LL_PWM_PIN TIM8_PWM_MAP1_CH1_B6 // 左升力风扇 #define FAN_LL_PWM_PIN TIM8_PWM_MAP1_CH1_B6 // 左升力风扇
#define FAN_RL_PWM_PIN TIM8_PWM_MAP1_CH2_B7 // 右升力风扇 #define FAN_RL_PWM_PIN TIM8_PWM_MAP1_CH2_B7 // 右升力风扇
// M4 // M4
#define FAN_RB_PWM_A_PIN TIM10_PWM_MAP3_CH2_D3 // 左后 #define FAN_RB_PWM_A_PIN TIM10_PWM_MAP3_CH4_D7 // 后
#define FAN_RB_PWM_B_PIN TIM10_PWM_MAP3_CH4_D7 // 左后 #define FAN_RB_PWM_B_PIN TIM10_PWM_MAP3_CH2_D3 // you后
// M1 // M1
#define FAN_LB_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // 右后 #define FAN_LB_PWM_A_PIN TIM4_PWM_MAP1_CH3_D14 // zuo后
#define FAN_LB_PWM_B_PIN TIM4_PWM_MAP1_CH3_D14 // 右后 #define FAN_LB_PWM_B_PIN TIM4_PWM_MAP1_CH1_D12 // 后
// M3 // M3
#define FAN_RS_PWM_B_PIN TIM10_PWM_MAP3_CH1_D1 // 右侧 #define FAN_RS_PWM_B_PIN TIM10_PWM_MAP3_CH1_D1 // 右侧
#define FAN_RS_PWM_A_PIN TIM10_PWM_MAP3_CH3_D5 // you侧 #define FAN_RS_PWM_A_PIN TIM10_PWM_MAP3_CH3_D5 // you侧
@@ -65,9 +65,9 @@ void by_pwm_init(void)
pwm_init(FAN_LS_PWM_B_PIN, 20000, 0); pwm_init(FAN_LS_PWM_B_PIN, 20000, 0);
pwm_init(FAN_RS_PWM_A_PIN, 20000, 0); pwm_init(FAN_RS_PWM_A_PIN, 20000, 0);
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,1000);
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, 0); pwm_init(FAN_RB_PWM_A_PIN, 20000, 1000);
pwm_init(FAN_RB_PWM_B_PIN, 20000, 0); pwm_init(FAN_RB_PWM_B_PIN, 20000, 0);
while (1); while (1);
#endif #endif

51
app/jj_motion.c
View File

@@ -18,23 +18,23 @@ float ci_Ki0 = 0;
float ci_Kd0 = 0; float ci_Kd0 = 0;
float out_ci; float out_ci;
// 弯道后面风扇角度环 // 弯道后面风扇角度环
float an_Kp0 = 45.0f; float an_Kp0 = 54.0f;
float an_Ki0 = 0.0f; float an_Ki0 = 0.0f;
float an_Kd0 = 0.0f; float an_Kd0 = 0.0f;
// 直道后面风扇角度环 // 直道后面风扇角度环
float an_Kp1 = 35.0f; float an_Kp1 = 45.0f;
float an_Ki1 = 0.0f; float an_Ki1 = 0.0f;
float an_Kd1 = 0.0f; float an_Kd1 = 0.0f;
// 圆环后面风扇角度环 // 圆环后面风扇角度环
float yu_Kp0 = 51.0f; float yu_Kp0 = 53.0f;
float yu_Ki0 = 0.0f; float yu_Ki0 = 0.0f;
float yu_Kd0 = 0.0f; float yu_Kd0 = 0.0f;
// 弯道侧面风扇角度环 // 弯道侧面风扇角度环
float cn_Kp1 = 400.0f; float cn_Kp1 = 300.0f;
float cn_Ki1 = 0.f; float cn_Ki1 = 0.f;
float cn_Kd1 = 0.0f; float cn_Kd1 = 0.0f;
// 直道侧面风扇角度环 // 直道侧面风扇角度环
float po_Kp1 = 400.0f; float po_Kp1 = 250.0f;
float po_Ki1 = 0.0f; float po_Ki1 = 0.0f;
float po_Kd1 = 0.0f; float po_Kd1 = 0.0f;
// 期望和当前量 // 期望和当前量
@@ -46,15 +46,15 @@ float out_pos;
float set_pos = 0.0f; float set_pos = 0.0f;
float out_cal; float out_cal;
// 弯道角速度环 // 弯道角速度环
float gy_Kp0 = 4.80f; float gy_Kp0 = 8.80f;
float gy_Ki0 = 0.0f; float gy_Ki0 = 0.0f;
float gy_Kd0 = 0.0f; float gy_Kd0 = 0.0f;
// 直道角速度环 // 直道角速度环
float gy_Kp1 = 6.0f; float gy_Kp1 = 7.0f;
float gy_Ki1 = 0.0f; float gy_Ki1 = 0.0f;
float gy_Kd1 = 0.0f; float gy_Kd1 = 0.0f;
// 圆环角速度环 // 圆环角速度环
float ygy_Kp0 = 6.0f; float ygy_Kp0 = 7.0f;
float ygy_Ki0 = 0.0f; float ygy_Ki0 = 0.0f;
float ygy_Kd0 = 0.0f; float ygy_Kd0 = 0.0f;
// 当前和输入量 // 当前和输入量
@@ -63,7 +63,7 @@ float out_gyro;
// float set_gyro = 0.0f; // float set_gyro = 0.0f;
// 速度环 // 速度环
float sp_Kp = 10.0f; float sp_Kp = 10.0f;
float sp_Ki = 100.f; float sp_Ki = 80.f;
float sp_Kd = 0.0f; float sp_Kd = 0.0f;
float in_speed; float in_speed;
@@ -73,7 +73,7 @@ float set_speed;
float set_speed0 = 900.0f; float set_speed0 = 900.0f;
float set_speed1 = 1100.0f; float set_speed1 = 1100.0f;
float set_speed2 = 830.f; float set_speed2 = 830.f;
float set_speed3 = 1000.f; float set_speed3 = 900.f;
int cnt1 = 0; int cnt1 = 0;
int cnt2 = 0; int cnt2 = 0;
int cnt3 = 0; int cnt3 = 0;
@@ -198,14 +198,14 @@ 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, 6500.f); pwm_duty_ls = (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_rs = (int32_t)myclip_f(out_pos, 0.0f, 7000.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, 6500.f); pwm_duty_ls = (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_rs = (int32_t)myclip_f(out_cal, 0.0f, 7000.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);
} }
@@ -223,14 +223,14 @@ void sport_motion()
cnt3++; cnt3++;
if (in_state == 3) { if (in_state == 3) {
if (cnt3 >= 300) // 200ms if (cnt3 >= 400) // 200ms
{ {
bt_printf("to 环内"); bt_printf("to 环内");
cnt3_flag = 2; cnt3_flag = 2;
cnt3 = 0; cnt3 = 0;
} }
} else if (in_state == 1) { } else if (in_state == 1) {
if (cnt3 >= 450) // 200ms if (cnt3 >= 500) // 200ms
{ {
bt_printf("to 弯道"); bt_printf("to 弯道");
cnt3_flag = 2; cnt3_flag = 2;
@@ -245,19 +245,24 @@ void sport_motion()
} }
} }
if (in_speed > set_speed - 100) { if (in_speed > set_speed - 100) {
qd_down = 8* fabs(in_pos); qd_down = 5 * fabs(in_pos) + 50;
} else if(in_speed > set_speed - 250){ } else if (in_speed > set_speed - 200) {
qd_down = 4* fabs(in_pos); qd_down = 2.5 * fabs(in_pos)+10;
} else { } else {
qd_down = 0; 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 {
if(in_state==1) {
qd_down = 15;
}else{
qd_down=0;
}
if (in_state == 4) { if (in_state == 4) {
by_pwm_update_duty(bt_fly + 515, bt_fly + 515); by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
set_speed = set_speed3; set_speed = set_speed3;
} else { } else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500); by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
} }
} }
} }
@@ -278,12 +283,12 @@ void sport_pid_init()
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, -6500.0f, 6500.0f); PID_SetOutputLimits(&str_angle_pid, -6000.0f, 6000.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, -6500.0f, 6500.0f); PID_SetOutputLimits(&tur_cal_pid, -6000.0f, 6000.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);

1
app/jj_motion.h
View File

@@ -45,6 +45,7 @@ extern float out_speed;
extern float set_speed0; extern float set_speed0;
extern float set_speed1; extern float set_speed1;
extern float set_speed2; extern float set_speed2;
extern float set_speed3;
extern float set_speed; extern float set_speed;
extern int32_t pwm_duty_ls; extern int32_t pwm_duty_ls;
extern int32_t pwm_duty_rs; extern int32_t pwm_duty_rs;

2
app/jj_param.c
View File

@@ -61,7 +61,7 @@ void jj_param_eeprom_init(void)
PARAM_REG(speed_Kd, &sp_Kd, EFLOAT, 1, "sp_D:"); PARAM_REG(speed_Kd, &sp_Kd, EFLOAT, 1, "sp_D:");
PARAM_REG(param_set_speed2, &set_speed2, EFLOAT, 1, "rate2:"); PARAM_REG(param_set_speed2, &set_speed2, EFLOAT, 1, "rate2:");
PARAM_REG(param_set_speed3, &set_speed3, EFLOAT, 1, "rate3:");
jj_param_read(); // 注冊 jj_param_read(); // 注冊
} }
/** /**

1
app/jj_param.h
View File

@@ -61,6 +61,7 @@ typedef enum {
speed_Kd, speed_Kd,
param_set_speed2, param_set_speed2,
param_set_speed3,
DATA_IN_FLASH_NUM, DATA_IN_FLASH_NUM,
delta_x, delta_x,

2
app/jj_voltage.c
View File

@@ -9,5 +9,5 @@ void vol_init(void)
} }
void get_vol() void get_vol()
{ {
now_vol=0.003377f*adc_convert(GET_VOL_PIN)+0.16f; now_vol=0.003377f*adc_convert(GET_VOL_PIN)+0.36f;
} }