diff --git a/app/by_fan_control.c b/app/by_fan_control.c index e5a115a..77612e4 100644 --- a/app/by_fan_control.c +++ b/app/by_fan_control.c @@ -2,21 +2,21 @@ #include "by_fan_control.h" #include "zf_common_headfile.h" #include "jj_blueteeth.h" - +#include "jj_motion.h" #define FAN_LL_PWM_PIN TIM8_PWM_MAP1_CH1_B6 // 左升力风扇 #define FAN_RL_PWM_PIN TIM8_PWM_MAP1_CH2_B7 // 右升力风扇 // M4 -#define FAN_LS_PWM_A_PIN TIM10_PWM_MAP3_CH1_D1 // 左侧动力风扇 -#define FAN_LS_PWM_B_PIN TIM10_PWM_MAP3_CH2_D3 // 左侧动力风扇 +#define FAN_LB_PWM_A_PIN TIM10_PWM_MAP3_CH2_D3 // 左侧动力风扇 +#define FAN_LB_PWM_B_PIN TIM10_PWM_MAP3_CH4_D7 // 左侧动力风扇 // M1 -#define FAN_RS_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // 右侧动力风扇 -#define FAN_RS_PWM_B_PIN TIM4_PWM_MAP1_CH2_D13 // 右侧动力风扇 +#define FAN_RB_PWM_A_PIN TIM4_PWM_MAP1_CH3_D14 // 右侧动力风扇 +#define FAN_RB_PWM_B_PIN TIM4_PWM_MAP1_CH1_D12 // 右侧动力风扇 // M3 -#define FAN_LB_PWM_A_PIN TIM10_PWM_MAP3_CH3_D5 // 左后动力风扇 -#define FAN_LB_PWM_B_PIN TIM10_PWM_MAP3_CH4_D7 // 左后动力风扇 +#define FAN_LS_PWM_A_PIN TIM10_PWM_MAP3_CH1_D1 // 左后动力风扇 +#define FAN_LS_PWM_B_PIN TIM10_PWM_MAP3_CH3_D5 // 左后动力风扇 // M2 -#define FAN_RB_PWM_A_PIN TIM4_PWM_MAP1_CH3_D14 // 右后动力风扇 -#define FAN_RB_PWM_B_PIN TIM4_PWM_MAP1_CH4_D15 // 右后动力风扇 +#define FAN_RS_PWM_A_PIN TIM4_PWM_MAP1_CH4_D15 // 右后动力风扇 +#define FAN_RS_PWM_B_PIN TIM4_PWM_MAP1_CH2_D13 // 右后动力风扇 int32_t pwm_duty_ls_g; int32_t pwm_duty_rs_g; @@ -47,42 +47,44 @@ void by_pwm_init(void) pwm_init(FAN_RL_PWM_PIN, 50, 500); // 测试鸣叫 - pwm_init(FAN_LS_PWM_A_PIN, 1500, 100); - pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RS_PWM_A_PIN, 1500, 100); - pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 1500, 100); - pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 1500, 100); - pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); - system_delay_ms(300); - pwm_init(FAN_LS_PWM_A_PIN, 2000, 100); - pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RS_PWM_A_PIN, 2000, 100); - pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 2000, 100); - pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 2000, 100); - pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); - system_delay_ms(300); - pwm_init(FAN_LS_PWM_A_PIN, 2500, 100); - pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RS_PWM_A_PIN, 2500, 100); - pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 2500, 100); - pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 2500, 100); - pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); - system_delay_ms(300); + // pwm_init(FAN_LS_PWM_A_PIN, 1500, 1000); + // pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_RS_PWM_A_PIN, 1500, 1000); + // pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_LB_PWM_A_PIN, 1500, 1000); + // pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_RB_PWM_A_PIN, 1500, 1000); + // pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); + // system_delay_ms(100); + // pwm_init(FAN_LS_PWM_A_PIN, 2000, 1000); + // pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_RS_PWM_A_PIN, 2000, 1000); + // pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_LB_PWM_A_PIN, 2000, 1000); + // pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_RB_PWM_A_PIN, 2000, 1000); + // pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); + // system_delay_ms(100); + // pwm_init(FAN_LS_PWM_A_PIN, 2500, 1000); + // pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_RS_PWM_A_PIN, 2500, 1000); + // pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_LB_PWM_A_PIN, 2500, 1000); + // pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); + // pwm_init(FAN_RB_PWM_A_PIN, 2500, 1000); + // pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); + // system_delay_ms(100); - pwm_init(FAN_LS_PWM_A_PIN, 4000, 0); - pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RS_PWM_A_PIN, 4000, 0); - pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 4000, 0); - pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 4000, 0); - pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); + pwm_init(FAN_LS_PWM_A_PIN, 12000, 0); + pwm_init(FAN_LS_PWM_B_PIN, 12000, 0); + pwm_init(FAN_RS_PWM_A_PIN, 12000, 0); + pwm_init(FAN_RS_PWM_B_PIN, 12000, 0); + pwm_init(FAN_LB_PWM_A_PIN, 12000, 0); + pwm_init(FAN_LB_PWM_B_PIN, 12000, 0); + pwm_init(FAN_RB_PWM_A_PIN, 12000, 0); + pwm_init(FAN_RB_PWM_B_PIN, 12000, 0); + // while (1) + // ; // system_delay_ms(3000); // // pwm_init(FAN_LS_PWM_PIN, 50, 1000); // 动力风扇左 1 // // pwm_init(FAN_RS_PWM_PIN, 50, 1000); // 动力风扇右 1 @@ -118,47 +120,49 @@ void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2) * @param pwm_duty_lb * @param pwm_duty_rb */ -void by_pwm_power_duty(int32_t pwm_duty_ls, int32_t pwm_duty_rs, int32_t pwm_duty_lb, int32_t pwm_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) { - pwm_duty_ls = clip_s32(pwm_duty_ls, 0, 8000); - pwm_duty_rs = clip_s32(pwm_duty_rs, 0, 8000); - pwm_duty_lb = clip_s32(pwm_duty_lb, 0, 8000); - pwm_duty_rb = clip_s32(pwm_duty_rb, 0, 8000); + bpwm_duty_ls = clip_s32(bpwm_duty_ls, 0, 5000); + bpwm_duty_rs = clip_s32(bpwm_duty_rs, 0, 5000); + bpwm_duty_lb = clip_s32(bpwm_duty_lb, -3000, 8000); + bpwm_duty_rb = clip_s32(bpwm_duty_rb, -3000, 8000); - pwm_duty_ls_g = pwm_duty_ls; - pwm_duty_rs_g = pwm_duty_rs; - pwm_duty_lb_g = pwm_duty_lb; - pwm_duty_rb_g = pwm_duty_rb; + // pwm_duty_ls_g = pwm_duty_ls; + // pwm_duty_rs_g = pwm_duty_rs; + // pwm_duty_lb_g = pwm_duty_lb; + // pwm_duty_rb_g = pwm_duty_rb; - if (pwm_duty_ls >= 0) { - pwm_set_duty(FAN_LS_PWM_A_PIN, pwm_duty_ls); + if (bpwm_duty_ls >= 0) { + pwm_set_duty(FAN_LS_PWM_A_PIN, bpwm_duty_ls); pwm_set_duty(FAN_LS_PWM_B_PIN, 0); - } else { - pwm_set_duty(FAN_LS_PWM_A_PIN, 0); - pwm_set_duty(FAN_LS_PWM_B_PIN, -1 * pwm_duty_ls); } + // else { + // pwm_set_duty(FAN_LS_PWM_A_PIN, 0); + // pwm_set_duty(FAN_LS_PWM_B_PIN, -1 * pwm_duty_ls); + // } - if (pwm_duty_rs >= 0) { - pwm_set_duty(FAN_RS_PWM_A_PIN, pwm_duty_rs); + if (bpwm_duty_rs >= 0) { + pwm_set_duty(FAN_RS_PWM_A_PIN, bpwm_duty_rs); pwm_set_duty(FAN_RS_PWM_B_PIN, 0); - } else { - pwm_set_duty(FAN_RS_PWM_A_PIN, 0); - pwm_set_duty(FAN_RS_PWM_B_PIN, -1 * pwm_duty_rs); } + // else { + // pwm_set_duty(FAN_RS_PWM_A_PIN, 0); + // pwm_set_duty(FAN_RS_PWM_B_PIN, -1 * pwm_duty_rs); + // } - if (pwm_duty_lb >= 0) { - pwm_set_duty(FAN_LB_PWM_A_PIN, pwm_duty_lb); + if (bpwm_duty_lb >= 0) { + pwm_set_duty(FAN_LB_PWM_A_PIN, bpwm_duty_lb); pwm_set_duty(FAN_LB_PWM_B_PIN, 0); - } else { + } else if (bpwm_duty_lb < -1000 && out_speed < -2000.0f) { pwm_set_duty(FAN_LB_PWM_A_PIN, 0); - pwm_set_duty(FAN_LB_PWM_B_PIN, -1 * pwm_duty_lb); + pwm_set_duty(FAN_LB_PWM_B_PIN, -1 * bpwm_duty_lb); } - if (pwm_duty_rb >= 0) { - pwm_set_duty(FAN_RB_PWM_A_PIN, pwm_duty_rb); + if (bpwm_duty_rb >= 0) { + pwm_set_duty(FAN_RB_PWM_A_PIN, bpwm_duty_rb); pwm_set_duty(FAN_RB_PWM_B_PIN, 0); - } else { + } else if (bpwm_duty_rb < -1000 && out_speed < -2000.0f) { pwm_set_duty(FAN_RB_PWM_A_PIN, 0); - pwm_set_duty(FAN_RB_PWM_B_PIN, -1 * pwm_duty_rb); + pwm_set_duty(FAN_RB_PWM_B_PIN, -1 * bpwm_duty_rb); } } diff --git a/app/jj_blueteeth.c b/app/jj_blueteeth.c index 8ebb6a6..b53a820 100644 --- a/app/jj_blueteeth.c +++ b/app/jj_blueteeth.c @@ -6,7 +6,7 @@ uint8_t bt_buffer; // 接收字符存入 uint8_t bt_run_flag = 0; uint8_t bt_fly_flag = 0; uint32_t bt_run = 0; -uint32_t bt_fly = 500; +uint32_t bt_fly = 300; enum bt_order { Fly_ctrl = 0x01, // 起飞转换 Fly_up = 0x02, // 起飞程度增加 @@ -40,10 +40,10 @@ void jj_bt_run() bt_run_flag = !bt_run_flag; break; case Fly_up: - bt_fly += 50; + bt_fly += 25; break; case Fly_dowm: - bt_fly -= 50; + bt_fly -= 25; break; case Speed_up: bt_run += 50; diff --git a/app/jj_motion.c b/app/jj_motion.c index d20a0b2..fad0e00 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -1,5 +1,5 @@ #include "jj_motion.h" - +#include #include "zf_common_headfile.h" #include "pid.h" @@ -51,12 +51,15 @@ float sp_Kd = 0.0f; float in_speed; float out_speed; -float set_speed0 = 460.0f; -float set_speed1 = 460.0f; -int cnt1 = 0; -int cnt2 = 0; -uint8_t in_state = 0; -uint8_t in_stop = 0; +float set_speed0 = 200.0f; +float set_speed1 = 300.0f; +int cnt1 = 0; +int cnt2 = 0; +int cnt3 = 0; +uint8_t cnt3_flag = 0; +uint8_t in_state = 0; +uint8_t in_stop = 0; +uint8_t last_state = 0; uint32_t pwm_duty_ls; uint32_t pwm_duty_rs; @@ -86,43 +89,70 @@ float sport_get_speed(void) void sport_motion(void) { + float temp; + float tt; + if (in_state == 1) { + temp = 2.5f;//fabs(tanf(myclip_f(in_angle, -45, 45)/ 180 * PI)); + tt = myclip_f(fabs(in_angle), 0, 50.f); + } else { + temp = 1.f; + tt = 0; + } + imu660ra_get_gyro(); + in_gyro = imu660ra_gyro_z * 0.8f + in_gyro * 0.2f; // 陀螺仪输入 + if (1 == in_stop) { bt_fly_flag = bt_run_flag = 0; } - if (0 == in_state || 2 == in_state) { - PID_SetTunings(&far_gyro_pid, an_Kp1, an_Ki1, an_Kd1); + // pid参数切换 + if (last_state == 1 && (in_state == 2 || in_state == 0)) { // 直道 + cnt3_flag = 0; + cnt3 = 0; + bt_printf("to 0"); + PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1); PID_SetTunings(&near_pos_pid, po_Kp1, po_Ki1, po_Kd1); PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1); - } else { - PID_SetTunings(&far_gyro_pid, an_Kp0, an_Ki0, an_Kd0); + // PID_SetPoints(&speed_pid, &set_speed1); + } + if (last_state != 1 && in_state == 1) { // 弯道 + bt_printf("to 1"); + cnt3_flag = 1; + PID_SetTunings(&far_angle_pid, an_Kp0, an_Ki0, an_Kd0); PID_SetTunings(&near_pos_pid, po_Kp0, po_Ki0, po_Kd0); PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0); + // PID_SetPoints(&speed_pid, &set_speed0); + } + last_state = in_state; + if (cnt3_flag == 1 && in_state == 1) { + cnt3++; + if (cnt3 >= 200) // 200ms + { + bt_printf("to 2"); + cnt3_flag = 2; + cnt3 = 0; + } } /* 动力风扇设置 */ if (1 == bt_run_flag) { cnt1++; cnt2++; - imu660ra_get_gyro(); - in_gyro = imu660ra_gyro_z; // 陀螺仪输入 PID_Compute(&far_gyro_pid); - + PID_Compute(&near_pos_pid); if (cnt1 >= 10) { - in_speed = -1 * sport_get_speed(); + in_speed = sport_get_speed(); PID_Compute(&speed_pid); - - PID_Compute(&near_pos_pid); - cnt1 = 0; } if (cnt2 >= 20) { cnt2 = 0; PID_Compute(&far_angle_pid); } - pwm_duty_ls = (uint32_t)myclip_f(-1.5f * out_pos + out_gyro, 0.0f, 8000.f); - pwm_duty_rs = (uint32_t)myclip_f(1.5f * out_pos - out_gyro, 0.0f, 8000.f); - pwm_duty_lb = (uint32_t)myclip_f(1.f * out_speed + out_gyro, 0.0f, 8000.f); - pwm_duty_rb = (uint32_t)myclip_f(1.f * out_speed - out_gyro, 0.0f, 8000.f); + + pwm_duty_ls = (int32_t)myclip_f(-1.5f * out_pos + temp * out_gyro, 0.0f, 6000.f); + pwm_duty_rs = (int32_t)myclip_f(1.5f * out_pos - temp * out_gyro, 0.0f, 6000.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((int32_t)pwm_duty_ls, (int32_t)pwm_duty_rs, (int32_t)pwm_duty_lb, (int32_t)pwm_duty_rb); } else { @@ -133,7 +163,21 @@ void sport_motion(void) if (bt_fly_flag == 0) { by_pwm_update_duty(0 + 500, 0 + 500); } else { - by_pwm_update_duty(bt_fly + 500, bt_fly + 500); + + if (cnt3_flag == 2) // 超时 + { + if (in_angle > 0) { + by_pwm_update_duty(bt_fly + 500 - 1.f * tt, bt_fly + 500 - 0.5f * tt); + } else { + by_pwm_update_duty(bt_fly + 500 - 0.5f * tt, bt_fly + 500 - 1.f * tt); + } + } else { + if (in_angle > 0) { + by_pwm_update_duty(bt_fly + 500 - 3.f * tt, bt_fly + 500 - 1.f * tt); + } else { + by_pwm_update_duty(bt_fly + 500 - 1.f * tt, bt_fly + 500 - 3.f * tt); + } + } } } @@ -144,30 +188,30 @@ void sport_motion(void) 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_M, _PID_CD_REVERSE); PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&far_angle_pid, 20); PID_SetOutputLimits(&far_angle_pid, -3000.0f, 3000.0f); // PID_Init(&far_angle_pid); /* 角速度控制 */ - PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); + PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_M, _PID_CD_REVERSE); PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&far_gyro_pid, 1); - PID_SetOutputLimits(&far_gyro_pid, -4000.0f, 4000.0f); + PID_SetOutputLimits(&far_gyro_pid, -3000.0f, 3000.0f); // PID_Init(&far_gyro_pid); /* 近点控制 */ PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_DIRECT); PID_SetMode(&near_pos_pid, _PID_MODE_AUTOMATIC); - PID_SetSampleTime(&near_pos_pid, 10); - PID_SetOutputLimits(&near_pos_pid, -4000.0f, 4000.0f); + PID_SetSampleTime(&near_pos_pid, 1); + PID_SetOutputLimits(&near_pos_pid, -3000.0f, 3000.0f); // PID_Init(&near_pos_pid); /* 速度控制 */ - PID(&speed_pid, &in_speed, &out_speed, &set_speed1, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_E, _PID_CD_DIRECT); + PID(&speed_pid, &in_speed, &out_speed, &set_speed1, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT); PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&speed_pid, 10); - PID_SetOutputLimits(&speed_pid, 0.0f, 2000.0f); + PID_SetOutputLimits(&speed_pid, -2500.0f, 3000.0f); // PID_Init(&speed_pid); } diff --git a/app/main.c b/app/main.c index 9aa5756..ab7784b 100644 --- a/app/main.c +++ b/app/main.c @@ -35,7 +35,7 @@ #include "by_frame.h" #include "by_rt_button.h" #include "by_fan_control.h" -uint8_t last_state; + int main(void) { TYPE_UNION test_data[BY_FRAME_DATA_NUM]; @@ -50,7 +50,7 @@ int main(void) jj_param_eeprom_init(); jj_bt_init(); by_rb_init(); - // by_pwm_init(); + by_pwm_init(); by_buzzer_init(); by_frame_init(); @@ -70,9 +70,5 @@ int main(void) in_angle = test_data[0].f32; in_state = test_data[2].u8[0]; in_stop = test_data[2].u8[1]; - if (last_state != in_state) { - bt_printf("changing to%u\r\n", in_state); - } - last_state = in_state; } } diff --git a/libraries/zf_device/zf_device_imu660ra.c b/libraries/zf_device/zf_device_imu660ra.c index 40dc5f8..36941cb 100644 --- a/libraries/zf_device/zf_device_imu660ra.c +++ b/libraries/zf_device/zf_device_imu660ra.c @@ -266,7 +266,7 @@ uint8 imu660ra_init(void) } imu660ra_write_register(IMU660RA_PWR_CTRL, 0x0E); // 开启性能模式 使能陀螺仪、加速度、温度传感器 imu660ra_write_register(IMU660RA_ACC_CONF, 0xA7); // 加速度采集配置 性能模式 正常采集 50Hz 采样频率 - imu660ra_write_register(IMU660RA_GYR_CONF, 0xAC); // 陀螺仪采集配置 性能模式 正常采集 1600Hz 采样频率 + imu660ra_write_register(IMU660RA_GYR_CONF, 0xA9); // 陀螺仪采集配置 性能模式 正常采集 1600Hz 采样频率 // IMU660RA_ACC_SAMPLE 寄存器 // 设置为 0x00 加速度计量程为 ±2 g 获取到的加速度计数据除以 16384 可以转化为带物理单位的数据 单位 g(m/s^2)