From f1825c01af62dd9578d3e7db4590917c0aa954a1 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:57:24 +0800 Subject: [PATCH 1/8] =?UTF-8?q?fix(DM=5FMC01):=20ADC1=20=E6=94=B9=E4=B8=BA?= =?UTF-8?q?=E5=8D=95=E9=80=9A=E9=81=93=E9=87=87=E6=A0=B7=E5=B9=B6=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3=E7=94=B5=E6=B1=A0=E7=94=B5=E5=8E=8B=E8=AF=BB=E5=8F=96?= =?UTF-8?q?=E6=B5=81=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- boards/base/DM_MC01_general/Core/Src/adc.c | 22 ++----------------- .../base/DM_MC01_general/DM_MC01_general.ioc | 10 ++------- .../drivers/DM_MC01/include/bsp_batteryvol.h | 2 ++ boards/drivers/DM_MC01/src/bsp_batteryvol.cpp | 7 ++++-- 4 files changed, 11 insertions(+), 30 deletions(-) diff --git a/boards/base/DM_MC01_general/Core/Src/adc.c b/boards/base/DM_MC01_general/Core/Src/adc.c index 3379a46b..de2ebbd2 100644 --- a/boards/base/DM_MC01_general/Core/Src/adc.c +++ b/boards/base/DM_MC01_general/Core/Src/adc.c @@ -45,13 +45,13 @@ void MX_ADC1_Init(void) hadc1.Instance = ADC1; hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; hadc1.Init.Resolution = ADC_RESOLUTION_12B; - hadc1.Init.ScanConvMode = ENABLE; + hadc1.Init.ScanConvMode = DISABLE; hadc1.Init.ContinuousConvMode = DISABLE; hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc1.Init.NbrOfConversion = 3; + hadc1.Init.NbrOfConversion = 1; hadc1.Init.DMAContinuousRequests = DISABLE; hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; if (HAL_ADC_Init(&hadc1) != HAL_OK) @@ -68,24 +68,6 @@ void MX_ADC1_Init(void) { Error_Handler(); } - - /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. - */ - sConfig.Channel = ADC_CHANNEL_VREFINT; - sConfig.Rank = 2; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. - */ - sConfig.Channel = ADC_CHANNEL_0; - sConfig.Rank = 3; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - Error_Handler(); - } /* USER CODE BEGIN ADC1_Init 2 */ /* USER CODE END ADC1_Init 2 */ diff --git a/boards/base/DM_MC01_general/DM_MC01_general.ioc b/boards/base/DM_MC01_general/DM_MC01_general.ioc index fd584b29..7c74d4e8 100644 --- a/boards/base/DM_MC01_general/DM_MC01_general.ioc +++ b/boards/base/DM_MC01_general/DM_MC01_general.ioc @@ -1,17 +1,11 @@ #MicroXplorer Configuration settings - do not modify ADC1.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_TEMPSENSOR -ADC1.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_VREFINT -ADC1.Channel-2\#ChannelRegularConversion=ADC_CHANNEL_0 ADC1.ClockPrescaler=ADC_CLOCK_SYNC_PCLK_DIV4 -ADC1.IPParameters=Rank-0\#ChannelRegularConversion,master,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler,NbrOfConversion,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion -ADC1.NbrOfConversion=3 +ADC1.IPParameters=Rank-0\#ChannelRegularConversion,master,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler,NbrOfConversion +ADC1.NbrOfConversion=1 ADC1.NbrOfConversionFlag=1 ADC1.Rank-0\#ChannelRegularConversion=1 -ADC1.Rank-1\#ChannelRegularConversion=2 -ADC1.Rank-2\#ChannelRegularConversion=3 ADC1.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES -ADC1.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES -ADC1.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES ADC1.master=1 CAD.formats=[] CAD.pinconfig=Project naming diff --git a/boards/drivers/DM_MC01/include/bsp_batteryvol.h b/boards/drivers/DM_MC01/include/bsp_batteryvol.h index 50f22289..396b68e5 100644 --- a/boards/drivers/DM_MC01/include/bsp_batteryvol.h +++ b/boards/drivers/DM_MC01/include/bsp_batteryvol.h @@ -32,6 +32,8 @@ namespace bsp { void Start(); void Stop(); uint32_t Read(); + + // 读取处理后的电池电压值 float GetBatteryVol(); static float calcBatteryPercentage(float voltage); float GetBatteryPercentage(); diff --git a/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp b/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp index 2ef78697..b9ebbb56 100644 --- a/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp +++ b/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp @@ -24,12 +24,13 @@ namespace bsp { BatteryVol::BatteryVol(ADC_HandleTypeDef* hadc, uint32_t channel, uint32_t rank, uint32_t sampling_time) { adc_ = new bADC(hadc, channel, rank, sampling_time); - adc_vrefint_ = new bADC(&hadc1, ADC_CHANNEL_VREFINT, 2, ADC_SAMPLETIME_3CYCLES); - adc_vrefint_->Start(); + adc_vrefint_ = new bADC(&hadc1, ADC_CHANNEL_VREFINT, 1, ADC_SAMPLETIME_3CYCLES); + } void BatteryVol::InitVREF() { unsigned char i = 0; unsigned int total_adc = 0; + adc_vrefint_->Start(); for (i = 0; i < 200; i++) { total_adc += adc_vrefint_->Read(); } @@ -43,8 +44,10 @@ namespace bsp { adc_->Stop(); } uint32_t BatteryVol::Read() { + adc_->Start(); return adc_->Read(); } + float BatteryVol::GetBatteryVol() { #ifdef DJI_BOARD_TYPE_C_GENERAL return (float)Read() * voltage_vrefint_proportion * 10.090909090909090909090909090909f; From 90c6a24b661521639a6bfc75c0d1455c8e674997 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:57:27 +0800 Subject: [PATCH 2/8] =?UTF-8?q?feat(DM=5FMC01):=20=E5=AE=9A=E4=B9=89=20BOA?= =?UTF-8?q?RD=5FTIM=5FSYS=20=E4=BB=A5=E6=94=AF=E6=8C=81=E9=AB=98=E7=B2=BE?= =?UTF-8?q?=E5=BA=A6=E5=AE=9A=E6=97=B6=E5=99=A8=E5=88=9D=E5=A7=8B=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- boards/base/DM_MC01_general/Core/Inc/tim.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/boards/base/DM_MC01_general/Core/Inc/tim.h b/boards/base/DM_MC01_general/Core/Inc/tim.h index 8bd40e41..7b71b50f 100644 --- a/boards/base/DM_MC01_general/Core/Inc/tim.h +++ b/boards/base/DM_MC01_general/Core/Inc/tim.h @@ -44,6 +44,8 @@ extern TIM_HandleTypeDef htim8; /* USER CODE BEGIN Private defines */ +#define BOARD_TIM_SYS htim5 + /* USER CODE END Private defines */ void MX_TIM1_Init(void); From cb6254dabbe3841f97c1edae4ff0c27ea80c90f7 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:57:30 +0800 Subject: [PATCH 3/8] =?UTF-8?q?docs:=20=E8=A1=A5=E5=85=85=20VT13=20?= =?UTF-8?q?=E9=94=AE=E4=BD=8D=E5=8A=9F=E8=83=BD=E4=B8=8E=20AHRS=20?= =?UTF-8?q?=E6=A0=A1=E5=87=86=E6=8E=A5=E5=8F=A3=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- boards/algorithm/src/AHRS.cpp | 1 + boards/drivers/include/dji_remote.h | 36 ++++++++++++++--------------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/boards/algorithm/src/AHRS.cpp b/boards/algorithm/src/AHRS.cpp index 52baea85..cc662a6f 100644 --- a/boards/algorithm/src/AHRS.cpp +++ b/boards/algorithm/src/AHRS.cpp @@ -138,6 +138,7 @@ namespace control { cailb_done_ = false; } + // 检测imu是否成功初始化 bool AHRS::IsCailbrated() { return cailb_done_; } diff --git a/boards/drivers/include/dji_remote.h b/boards/drivers/include/dji_remote.h index 6d35d2e9..c09b19db 100644 --- a/boards/drivers/include/dji_remote.h +++ b/boards/drivers/include/dji_remote.h @@ -36,22 +36,22 @@ namespace remote { typedef union { uint16_t code; struct { - uint16_t W : 1; - uint16_t S : 1; - uint16_t A : 1; - uint16_t D : 1; - uint16_t SHIFT : 1; - uint16_t CTRL : 1; - uint16_t Q : 1; - uint16_t E : 1; - uint16_t R : 1; - uint16_t F : 1; - uint16_t G : 1; - uint16_t Z : 1; - uint16_t X : 1; - uint16_t C : 1; - uint16_t V : 1; - uint16_t B : 1; + uint16_t W : 1; // 前 + uint16_t S : 1; // 后 + uint16_t A : 1; // 左 + uint16_t D : 1; // 右 + uint16_t SHIFT : 1; // boost + uint16_t CTRL : 1; // 切换 小陀螺/自瞄/坦克/标准 模式 + uint16_t Q : 1; // 小陀螺加速/减速 + uint16_t E : 1; // 小陀螺加速/减速 + uint16_t R : 1; // + uint16_t F : 1; // + uint16_t G : 1; // + uint16_t Z : 1; // 摩擦轮 + uint16_t X : 1; // + uint16_t C : 1; // 清除自定义UI + uint16_t V : 1; // 强制刷新自定义UI + uint16_t B : 1; // } __packed bit; } __packed keyboard_t; @@ -89,8 +89,8 @@ namespace remote { result.x = x; result.y = y; result.z = roll; - result.l = l; - result.r = r; + result.l = l; // shoot + result.r = r; // unload return result; } } __packed vt13_mouse_t; From 53a4c3ddf5199122fd003341fa34e3d4f73f1c90 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:57:32 +0800 Subject: [PATCH 4/8] =?UTF-8?q?feat(Hero/chassis):=20=E9=80=82=E9=85=8D?= =?UTF-8?q?=E6=96=B0=E9=85=8D=E7=94=B5=E6=9D=BF=20CAN=20=E5=88=86=E9=85=8D?= =?UTF-8?q?=E5=B9=B6=E6=8E=A5=E5=85=A5=E7=94=B5=E6=B1=A0=E7=94=B5=E5=8E=8B?= =?UTF-8?q?=E9=87=87=E6=A0=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- programs/Hero/chassis/src/main.cpp | 51 ++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 16 deletions(-) diff --git a/programs/Hero/chassis/src/main.cpp b/programs/Hero/chassis/src/main.cpp index ff474966..a91d0aa9 100644 --- a/programs/Hero/chassis/src/main.cpp +++ b/programs/Hero/chassis/src/main.cpp @@ -28,9 +28,16 @@ #include "chassis.h" #include "cmsis_os.h" #include "supercap.h" +#include "bsp_batteryvol.h" +#include "bsp_os.h" -bsp::CAN* can1 = nullptr; -bsp::CAN* can2 = nullptr; + + +// bridge_can 是与顶部大疆c板进行通信的can通道 +// chassis_can 是操控底盘四个电机的can通道 +// damiao的can相对于大疆是反着的！！！一定要注意！ +bsp::CAN* bridge_can = nullptr; +bsp::CAN* chassis_can = nullptr; driver::DjiMotorBase* fl_motor = nullptr; driver::DjiMotorBase* fr_motor = nullptr; driver::DjiMotorBase* bl_motor = nullptr; @@ -41,15 +48,22 @@ driver::SuperCap* super_cap = nullptr; control::Chassis* chassis = nullptr; communication::CanBridge* can_bridge = nullptr; +bsp::BatteryVol* battery_vol = nullptr; + void RM_RTOS_Init() { HAL_Delay(100); - print_use_uart(&huart5); - can2 = new bsp::CAN(&hcan2, false); - can1 = new bsp::CAN(&hcan1, true); - fl_motor = new driver::Motor3508(can2, 0x202); - fr_motor = new driver::Motor3508(can2, 0x201); - bl_motor = new driver::Motor3508(can2, 0x203); - br_motor = new driver::Motor3508(can2, 0x204); + // 设置高精度定时器以能够获取微秒级别的精度的运行时间数据 + bsp::SetHighresClockTimer(&BOARD_TIM_SYS); + print_use_rtt(); + // print_use_uart(&huart4,false,921600); + print("helloworld!"); + + chassis_can = new bsp::CAN(&hcan1, true); + bridge_can = new bsp::CAN(&hcan2, false); + fl_motor = new driver::Motor3508(chassis_can, 0x202); + fr_motor = new driver::Motor3508(chassis_can, 0x201); + bl_motor = new driver::Motor3508(chassis_can, 0x203); + br_motor = new driver::Motor3508(chassis_can, 0x204); control::ConstrainedPID::PID_Init_t omega_pid_init = { .kp = 2500, @@ -57,11 +71,11 @@ void RM_RTOS_Init() { .kd = 0, .max_out = 30000, .max_iout = 10000, - .deadband = 0, // 死区 - .A = 3 * PI, // 变速积分所能达到的最大值为A+B - .B = 2 * PI, // 启动变速积分的死区 - .output_filtering_coefficient = 0.1, // 输出滤波系数 - .derivative_filtering_coefficient = 0, // 微分滤波系数 + .deadband = 0, // 死区 + .A = 3 * PI, // 变速积分所能达到的最大值为A+B + .B = 2 * PI, // 启动变速积分的死区 + .output_filtering_coefficient = 0.1, // 输出滤波系数 + .derivative_filtering_coefficient = 0, // 微分滤波系数 .mode = control::ConstrainedPID::Integral_Limit | // 积分限幅 control::ConstrainedPID::OutputFilter | // 输出滤波 control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 @@ -85,7 +99,7 @@ void RM_RTOS_Init() { br_motor->SetTransmissionRatio(19); driver::supercap_init_t supercap_init = { - .can = can1, + .can = bridge_can, .tx_id = 0x02e, .tx_settings_id = 0x02f, .rx_id = 0x030, @@ -101,7 +115,7 @@ void RM_RTOS_Init() { super_cap->SetMaxDischargePower(250.0f); super_cap->SetPerferBuffer(50.0f); - can_bridge = new communication::CanBridge(can1, 0x52); + can_bridge = new communication::CanBridge(bridge_can, 0x52); driver::DjiMotorBase* motors[control::FourWheel::motor_num]; motors[control::FourWheel::front_left] = fl_motor; @@ -124,6 +138,8 @@ void RM_RTOS_Init() { can_bridge->RegisterRxCallback(0x72, chassis->CanBridgeUpdateEventPowerLimitWrapper, chassis); can_bridge->RegisterRxCallback(0x73, chassis->CanBridgeUpdateEventCurrentPowerWrapper, chassis); + battery_vol = new bsp::BatteryVol(&hadc1, ADC_CHANNEL_0, 1, ADC_SAMPLETIME_3CYCLES); + HAL_Delay(300); init_buzzer(); } @@ -135,7 +151,10 @@ void RM_RTOS_Default_Task(const void* args) { Buzzer_Sing(Mario); while (true) { + chassis->UpdatePowerVoltage(battery_vol->GetBatteryVol()); chassis->Update(); osDelay(10); + print("helloworld\r\n"); + Buzzer_Sing(Mario); } } From 417d88557a181ae7f0ebbdae0d4b3cd95dfff0a6 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:57:35 +0800 Subject: [PATCH 5/8] =?UTF-8?q?feat(Hero/gimbal):=20=E5=BA=95=E7=9B=98?= =?UTF-8?q?=E4=B8=8E=E4=BA=91=E5=8F=B0=E4=BB=BB=E5=8A=A1=E6=8E=A5=E5=85=A5?= =?UTF-8?q?=20VT13=20=E5=9B=BE=E4=BC=A0=E9=81=A5=E6=8E=A7=E8=BE=93?= =?UTF-8?q?=E5=85=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- programs/Hero/gimbal/include/chassis_task.h | 4 +- programs/Hero/gimbal/src/chassis_task.cpp | 314 +++++++------------- programs/Hero/gimbal/src/gimbal_task.cpp | 24 +- programs/Hero/gimbal/src/referee_task.cpp | 12 +- 4 files changed, 138 insertions(+), 216 deletions(-) diff --git a/programs/Hero/gimbal/include/chassis_task.h b/programs/Hero/gimbal/include/chassis_task.h index 100a6033..153d4b04 100644 --- a/programs/Hero/gimbal/include/chassis_task.h +++ b/programs/Hero/gimbal/include/chassis_task.h @@ -46,10 +46,12 @@ void chassisTask(void* arg); void init_chassis(); void kill_chassis(); +extern control::ChassisCanBridgeSender* chassis; extern float chassis_vx; extern float chassis_vy; -extern float chassis_vz; +extern float chassis_vt; extern bool chassis_boost_flag; const float chassis_vx_max = 660.0f; const float chassis_vy_max = 660.0f; const float chassis_vz_max = 660.0f; + diff --git a/programs/Hero/gimbal/src/chassis_task.cpp b/programs/Hero/gimbal/src/chassis_task.cpp index b9ecd311..4ff6094d 100644 --- a/programs/Hero/gimbal/src/chassis_task.cpp +++ b/programs/Hero/gimbal/src/chassis_task.cpp @@ -21,12 +21,13 @@ #include "chassis_task.h" osThreadId_t chassisTaskHandle; +const float chassis_max_xy_speed = 2 * PI * 10; +const float chassis_max_t_speed = 2 * PI * 5; + float chassis_vx = 0; float chassis_vy = 0; -float chassis_vz = 0; +float chassis_vt = 0; bool chassis_boost_flag = true; -const float speed_offset = 660; -const float speed_offset_boost = 1320; communication::CanBridge* can_bridge = nullptr; control::ChassisCanBridgeSender* chassis = nullptr; @@ -45,47 +46,8 @@ void chassisTask(void* arg) { osDelay(1); } - float relative_angle = yaw_motor->GetThetaDelta(gimbal_param->yaw_offset_); - - // float last_speed = 0; - float sin_yaw, cos_yaw, vx_set = 0, vy_set = 0, vz_set = 0, vx_set_org = 0, vy_set_org = 0; - float offset_yaw = 0; - float spin_speed = 350; - float manual_mode_yaw_pid_args[3] = {400, 0.5, 20}; - float manual_mode_yaw_pid_max_iout = 100; - float manual_mode_yaw_pid_max_out = 500; - control::ConstrainedPID* manual_mode_pid = new control::ConstrainedPID( - manual_mode_yaw_pid_args, - manual_mode_yaw_pid_max_iout, - manual_mode_yaw_pid_max_out - ); - manual_mode_pid->Reset(); - float yaw_pid_error = 0; - float manual_mode_pid_output = 0; - float current_speed_offset = speed_offset; - remote::keyboard_t keyboard; - remote::keyboard_t last_keyboard; - RampSource* vx_ramp = new RampSource(0, -chassis_vx_max / 2, chassis_vx_max / 2, 0.01); - RampSource* vy_ramp = new RampSource(0, -chassis_vy_max / 2, chassis_vy_max / 2, 0.01); - RampSource* vz_ramp = new RampSource(0, -chassis_vz_max / 2, chassis_vz_max / 2, 0.01); - BoolEdgeDetector* w_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* s_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* a_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* d_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* ctrl_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* q_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* e_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* x_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* ch0_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* ch1_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* ch2_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* ch3_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* ch4_edge = new BoolEdgeDetector(false); - chassis->Enable(); - const float ratio = 1.0f / 660.0f * 10 * PI; - while (true) { if (remote_mode == REMOTE_MODE_KILL) { kill_chassis(); @@ -95,181 +57,119 @@ void chassisTask(void* arg) { chassis->Enable(); continue; } - // 更新状态机 - { - last_keyboard = keyboard; - if (dbus->IsOnline()) { - keyboard = dbus->keyboard; - ch0_edge->input(dbus->ch0 != 0); - ch1_edge->input(dbus->ch1 != 0); - ch2_edge->input(dbus->ch2 != 0); - ch3_edge->input(dbus->ch3 != 0); - ch4_edge->input(dbus->ch4 != 0); - } else if (refereerc->IsOnline()) { - keyboard = refereerc->remote_control.keyboard; - ch0_edge->input(false); - ch1_edge->input(false); - ch2_edge->input(false); - ch3_edge->input(false); - ch4_edge->input(false); - } - } - { - w_edge->input(keyboard.bit.W); - s_edge->input(keyboard.bit.S); - a_edge->input(keyboard.bit.A); - d_edge->input(keyboard.bit.D); - ctrl_edge->input(keyboard.bit.CTRL); - q_edge->input(keyboard.bit.Q); - e_edge->input(keyboard.bit.E); - x_edge->input(keyboard.bit.X); - } - - // 更新云台角度 - relative_angle = yaw_motor->GetThetaDelta(gimbal_param->yaw_offset_); - // 计算角度的sin/cos - sin_yaw = arm_sin_f32(relative_angle); - cos_yaw = arm_cos_f32(relative_angle); - // 检测到ctrl，切换速度模式 - if (ctrl_edge->posEdge()) { - if (chassis_boost_flag) { - chassis_boost_flag = false; - vx_ramp->SetMax(chassis_vx_max / 2); - vy_ramp->SetMax(chassis_vy_max / 2); - vz_ramp->SetMax(chassis_vz_max / 2); - vx_ramp->SetMin(-chassis_vx_max / 2); - vy_ramp->SetMin(-chassis_vy_max / 2); - vz_ramp->SetMin(-chassis_vz_max / 2); - current_speed_offset = speed_offset; - } else { - chassis_boost_flag = true; - vx_ramp->SetMax(chassis_vx_max); - vy_ramp->SetMax(chassis_vy_max); - vz_ramp->SetMax(chassis_vz_max); - vx_ramp->SetMin(-chassis_vx_max); - vy_ramp->SetMin(-chassis_vy_max); - vz_ramp->SetMin(-chassis_vz_max); - current_speed_offset = speed_offset_boost; - } + remote::keyboard_t keyboard; + if (dbus->IsOnline()) { + keyboard = dbus->keyboard; + } else if (refereerc->IsOnline()) { + keyboard = refereerc->vt13_packet.keyboard; } - // 平移速度控制 - if (ch0_edge->get()) { - vx_set_org = dbus->ch0; - } else if (ch0_edge->negEdge() || x_edge->posEdge()) { - vx_set_org = 0; - vx_ramp->SetCurrent(0); - } else if (d_edge->get()) { - vx_set_org = vx_ramp->Calc(current_speed_offset); - } else if (a_edge->get()) { - vx_set_org = vx_ramp->Calc(-current_speed_offset); + + // 以云台为基准的（整车的）运动速度，范围为[-1, 1] + float car_vx, car_vy, car_vt; + if (dbus->ch0 || dbus->ch1 || dbus->ch2 || dbus->ch3 || dbus->ch4) { + // 优先使用遥控器 + const float speed_scale = 0.5; + car_vx = (float)dbus->ch0 / dbus->ROCKER_MAX * speed_scale; + car_vy = (float)dbus->ch1 / dbus->ROCKER_MAX * speed_scale; + car_vt = (float)dbus->ch4 / dbus->ROCKER_MAX * speed_scale; + } else if (refereerc->IsOnline() && + (refereerc->vt13_packet.remote.ch0 != remote::vt13_remote_t::ROCKER_MID || + refereerc->vt13_packet.remote.ch1 != remote::vt13_remote_t::ROCKER_MID || + refereerc->vt13_packet.remote.ch4 != remote::vt13_remote_t::ROCKER_MID)) { + // 使用VT13遥控器摇杆 + const float speed_scale = 0.5; + car_vx = + (float)(refereerc->vt13_packet.remote.ch0 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE * speed_scale; + car_vy = + (float)(refereerc->vt13_packet.remote.ch1 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE * speed_scale; + car_vt = + (float)(refereerc->vt13_packet.remote.ch4 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE * speed_scale; } else { - if (vx_set_org > 0) { - vx_set_org = vx_ramp->Calc(-current_speed_offset); - } else if (vx_set_org < 0) { - vx_set_org = vx_ramp->Calc(current_speed_offset); - } + // 使用键盘 + const float keyboard_speed = keyboard.bit.SHIFT ? 1 : 0.5; + const float keyboard_spin_speed = 1; + car_vx = (keyboard.bit.D - keyboard.bit.A) * keyboard_speed; + car_vy = (keyboard.bit.W - keyboard.bit.S) * keyboard_speed; + car_vt = (keyboard.bit.E - keyboard.bit.Q) * keyboard_spin_speed; } - // 前进速度控制 - if (ch1_edge->get()) { - vy_set_org = dbus->ch1; - } else if (ch1_edge->negEdge() || x_edge->posEdge()) { - vy_set_org = 0; - vy_ramp->SetCurrent(0); - } else if (w_edge->get()) { - vy_set_org = vy_ramp->Calc(current_speed_offset); - } else if (s_edge->get()) { - vy_set_org = vy_ramp->Calc(-current_speed_offset); - } else { - if (vy_set_org > 0) { - vy_set_org = vy_ramp->Calc(-current_speed_offset); - } else if (vy_set_org < 0) { - vy_set_org = vy_ramp->Calc(current_speed_offset); - } + + // 云台相对底盘的角度，通过云台和底盘连接的电机获取 + float A = yaw_motor->GetTheta() - gimbal_param->yaw_offset_; + // 云台当前相对云台零点的角度，通过IMU获取 + float B = imu->INS_angle[0]; + // 云台目标相对云台零点的角度，直接读取gimbal class获取 + float C = gimbal->getYawTarget() - gimbal_param->yaw_offset_; + float chassis_target_diff = C - B + A; + chassis_target_diff = -chassis_target_diff; + chassis_target_diff = pitch_diff = wrap(chassis_target_diff, -PI, PI); + + // 底盘以底盘自己为基准的运动速度 + float sin_yaw = arm_sin_f32(chassis_target_diff); + float cos_yaw = arm_cos_f32(chassis_target_diff); + chassis_vx = cos_yaw * car_vx + sin_yaw * car_vy; + chassis_vy = -sin_yaw * car_vx + cos_yaw * car_vy; + chassis_vt = 0; + + if (remote_mode == REMOTE_MODE_ADVANCED) { + // 手动模式下，遥控器直接控制底盘速度 + chassis_vx = car_vx; + chassis_vy = car_vy; + chassis_vt = car_vt; } - // 旋转速度控制（如果需要） - if (ch4_edge->get()) { - offset_yaw = dbus->ch4; - } else if (ch4_edge->negEdge() || x_edge->posEdge()) { - offset_yaw = 0; - vz_ramp->SetCurrent(0); - } else if (e_edge->get()) { - offset_yaw = vz_ramp->Calc(current_speed_offset); - } else if (q_edge->get()) { - offset_yaw = vz_ramp->Calc(-current_speed_offset); - } else { - if (offset_yaw > 0) { - offset_yaw = vz_ramp->Calc(-current_speed_offset); - } else if (offset_yaw < 0) { - offset_yaw = vz_ramp->Calc(current_speed_offset); + + if (remote_mode == REMOTE_MODE_FOLLOW) { + // 读取底盘和云台yaw轴角度差，控制底盘转向云台的方向 + const float angle_threshold = 0.02f; + float chassis_vt_pid_error = chassis_target_diff; + if (fabs(chassis_vt_pid_error) < angle_threshold) { + chassis_vt_pid_error = 0; } + + static control::ConstrainedPID* chassis_vt_pid = + new control::ConstrainedPID(2 / (2 * PI), 0, 0, 0.5, 1); + float vt = chassis_vt_pid->ComputeOutput(chassis_vt_pid_error); + if (chassis_vt_pid_error != 0) + chassis_vt = vt; } - if (abs(vx_set_org) < 0.05f) { - vx_set_org = 0; - } - if (abs(vy_set_org) < 0.05f) { - vy_set_org = 0; - } - if (abs(offset_yaw) < 0.05f) { - offset_yaw = 0; - } - chassis_vx = vx_set_org * ratio; - chassis_vy = vy_set_org * ratio; - chassis_vz = offset_yaw * ratio; - vx_set = cos_yaw * chassis_vx + sin_yaw * chassis_vy; - vy_set = -sin_yaw * chassis_vx + cos_yaw * chassis_vy; - switch (remote_mode) { - case REMOTE_MODE_FOLLOW: - yaw_pid_error = yaw_motor->GetThetaDelta(gimbal_param->yaw_offset_); - manual_mode_pid_output = manual_mode_pid->ComputeOutput(yaw_pid_error); - vz_set = manual_mode_pid_output * ratio; - chassis->SetSpeed(vx_set, vy_set, vz_set); - osDelay(1); - chassis->SetPower( - true, - referee->game_robot_status.chassis_power_limit, - referee->power_heat_data.chassis_power, - referee->power_heat_data.chassis_power_buffer - ); - osDelay(1); - break; - case REMOTE_MODE_SPIN: + if (remote_mode == REMOTE_MODE_SPIN) { + // 小陀螺模式，拨盘用来控制底盘加速度 + static float spin_speed = 1; + spin_speed = spin_speed + car_vt * 0.01; + spin_speed = clip(spin_speed, -1, 1); + chassis_vt = spin_speed; + } - if (offset_yaw != 0) { - spin_speed = spin_speed + offset_yaw; - offset_yaw = 0; - spin_speed = clip(spin_speed, -660, 660); - } - vz_set = spin_speed * ratio; - chassis->SetSpeed(vx_set, vy_set, vz_set); - osDelay(1); - chassis->SetPower( - true, - referee->game_robot_status.chassis_power_limit, - referee->power_heat_data.chassis_power, - referee->power_heat_data.chassis_power_buffer - ); - osDelay(1); - break; - case REMOTE_MODE_ADVANCED: - vz_set = chassis_vz; + // 进行缩放 + chassis_vx *= chassis_max_xy_speed; + chassis_vy *= chassis_max_xy_speed; + chassis_vt *= chassis_max_t_speed; + + static const float move_ease_ratio = 0.9; + static const float turn_ease_ratio = 0.9; + static Ease chassis_ease_vx(0, move_ease_ratio); + static Ease chassis_ease_vy(0, move_ease_ratio); + static Ease chassis_ease_vt(0, turn_ease_ratio); + chassis_vx = chassis_ease_vx.Calc(chassis_vx); + chassis_vy = chassis_ease_vy.Calc(chassis_vy); + chassis_vt = chassis_ease_vt.Calc(chassis_vt); + + chassis->SetSpeed(chassis_vx, chassis_vy, chassis_vt); + osDelay(1); - chassis->SetSpeed(chassis_vx, chassis_vy, vz_set); - osDelay(1); - chassis->SetPower( - true, - referee->game_robot_status.chassis_power_limit, - referee->power_heat_data.chassis_power, - referee->power_heat_data.chassis_power_buffer - ); - osDelay(1); - break; - default: - // Not Support - kill_chassis(); - } - chassis_vz = vz_set; +#ifdef HAS_REFEREE + uint8_t buffer_percent = referee->power_heat_data.chassis_power_buffer * 100 / 60; + uint8_t max_watt = referee->game_robot_status.chassis_power_limit; +#else + uint8_t buffer_percent = 50; + uint8_t max_watt = 100; +#endif + // 如果传输的电压为0，底盘不会保存这个值，随后底盘的chassis_task会采样自己的电压值并更新。 + chassis->UpdatePower(true, max_watt, 0, buffer_percent); osDelay(CHASSIS_OS_DELAY); } } diff --git a/programs/Hero/gimbal/src/gimbal_task.cpp b/programs/Hero/gimbal/src/gimbal_task.cpp index 75f47ff9..2906d047 100644 --- a/programs/Hero/gimbal/src/gimbal_task.cpp +++ b/programs/Hero/gimbal/src/gimbal_task.cpp @@ -76,7 +76,7 @@ void gimbalTask(void* arg) { yaw_curr = imu->INS_angle[0]; float pitch_target = 0, yaw_target = 0; - float actural_chassis_turn_speed = chassis_vz / 6.0f; + float actural_chassis_turn_speed = chassis_vt / 6.0f; while (true) { if (remote_mode == REMOTE_MODE_KILL) { kill_gimbal(); @@ -103,17 +103,35 @@ void gimbalTask(void* arg) { if (dbus->IsOnline()) { if (dbus->mouse.y != 0) { pitch_ratio = dbus->mouse.y / 32767.0 * 7.5 / 7.0; + } else if (refereerc->IsOnline() && (refereerc->vt13_packet.mouse.x != 0 || + refereerc->vt13_packet.mouse.y != 0)) { + // DBUS在线但摇杆/鼠标无输入时，使用VT13鼠标 + pitch_ratio = refereerc->vt13_packet.mouse.y / 32767.0 * 7.5 / 7.0; } else { pitch_ratio = -dbus->ch3 / 18000.0 / 7.0; } if (dbus->mouse.x != 0) { yaw_ratio = -dbus->mouse.x / 32767.0 * 7.5 / 7.0; + } else if (refereerc->IsOnline() && (refereerc->vt13_packet.mouse.x != 0 || + refereerc->vt13_packet.mouse.y != 0)) { + yaw_ratio = -refereerc->vt13_packet.mouse.x / 32767.0 * 7.5 / 7.0; } else { yaw_ratio = -dbus->ch2 / 18000.0 / 7.0; } } else if (refereerc->IsOnline()) { - pitch_ratio = refereerc->remote_control.mouse.y / 32767.0 * 7.5 / 7.0; - yaw_ratio = -refereerc->remote_control.mouse.x / 32767.0 * 7.5 / 7.0; + // DBUS离线时使用VT13：优先摇杆，否则鼠标 + if (refereerc->vt13_packet.remote.ch3 != remote::vt13_remote_t::ROCKER_MID || + refereerc->vt13_packet.remote.ch2 != remote::vt13_remote_t::ROCKER_MID) { + pitch_ratio = + (float)(refereerc->vt13_packet.remote.ch2 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE / 18000.0 * 660.0 / 7.0; + yaw_ratio = + (float)-(refereerc->vt13_packet.remote.ch3 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE / 18000.0 * 660.0 / 7.0; + } else { + pitch_ratio = -refereerc->vt13_packet.mouse.y / 32767.0 * 7.5 / 7.0; + yaw_ratio = -refereerc->vt13_packet.mouse.x / 32767.0 * 7.5 / 7.0; + } } else { pitch_ratio = 0; yaw_ratio = 0; diff --git a/programs/Hero/gimbal/src/referee_task.cpp b/programs/Hero/gimbal/src/referee_task.cpp index 222a398f..69548b4d 100644 --- a/programs/Hero/gimbal/src/referee_task.cpp +++ b/programs/Hero/gimbal/src/referee_task.cpp @@ -26,13 +26,15 @@ communication::Referee* referee = nullptr; communication::Referee* refereerc = nullptr; void init_referee() { - referee_uart = new bsp::UART(&huart6); + referee_uart = new bsp::UART(&BOARD_UART1); referee_uart->SetupRx(300); referee_uart->SetupTx(300); referee = new communication::Referee(referee_uart); - // refereerc_uart = new bsp::UART(&huart1); - // refereerc_uart->SetupRx(300); - // refereerc_uart->SetupTx(300); - // refereerc = new communication::Referee(refereerc_uart); + // 启动裁判系统图传链路（VT13遥控器） + refereerc_uart = new bsp::UART(&BOARD_UART2); + refereerc_uart->SetBaudrate(921600); + refereerc_uart->SetupRx(300); + refereerc_uart->SetupTx(300, false); + refereerc = new communication::Referee(refereerc_uart); } From fee38a402ff2454ef2afa6aadda2ef77c48dd5c7 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:59:47 +0800 Subject: [PATCH 6/8] =?UTF-8?q?feat(Hero/gimbal):=20=E6=9B=B4=E6=96=B0=20U?= =?UTF-8?q?I=20=E5=87=86=E6=98=9F=E4=BD=8D=E7=BD=AE=E5=B9=B6=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=E5=80=92=E8=BD=A6=E8=BE=85=E5=8A=A9=E7=BA=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- programs/Hero/gimbal/src/ui_task.cpp | 238 +++++++++++++++++---------- 1 file changed, 152 insertions(+), 86 deletions(-) diff --git a/programs/Hero/gimbal/src/ui_task.cpp b/programs/Hero/gimbal/src/ui_task.cpp index 6e086279..180d762c 100644 --- a/programs/Hero/gimbal/src/ui_task.cpp +++ b/programs/Hero/gimbal/src/ui_task.cpp @@ -25,13 +25,39 @@ communication::UserInterface* UI = nullptr; communication::ChassisGUI* chassisGUI = nullptr; communication::CrossairGUI* crossairGui = nullptr; communication::GimbalGUI* gimbalGUI = nullptr; -communication::CapGUI* batteryGUI = nullptr; +// communication::CapGUI* batteryGUI = nullptr; communication::StringGUI* modeGUI = nullptr; communication::StringGUI* wheelGUI = nullptr; communication::StringGUI* boostGUI = nullptr; communication::StringGUI* shootUnlockGUI = nullptr; communication::DiagGUI* diagGUI = nullptr; +// ============ Guideline Configuration (reverse-camera style) ============ +// Offsets relative to screen center (GUIDELINE_CENTER_X, GUIDELINE_CENTER_Y) +// Adjust these values to match robot physical outline +// +// \ / ← start points (upper, near crosshair) +// \ / +// \ / +// \ / ← end points (lower, near screen bottom) +// +// Left guideline +const int16_t GUIDELINE_LEFT_START_X_OFFSET = -50-240-140; // Start X (upper, close to center) +const int16_t GUIDELINE_LEFT_START_Y_OFFSET = -100; // Start Y (negative = below center) +const int16_t GUIDELINE_LEFT_END_X_OFFSET = -200-240-140; // End X (lower, spread outward) +const int16_t GUIDELINE_LEFT_END_Y_OFFSET = -440; // End Y (near screen bottom) +// Right guideline +const int16_t GUIDELINE_RIGHT_START_X_OFFSET = 50+230+180; // Start X (upper, close to center) +const int16_t GUIDELINE_RIGHT_START_Y_OFFSET = -100; // Start Y (negative = below center) +const int16_t GUIDELINE_RIGHT_END_X_OFFSET = 200+230+180; // End X (lower, spread outward) +const int16_t GUIDELINE_RIGHT_END_Y_OFFSET = -440; // End Y (near screen bottom) + +const int16_t GUIDELINE_CENTER_X = 960; +const int16_t GUIDELINE_CENTER_Y = 540; + +communication::graphic_data_t guidelineLeft; +communication::graphic_data_t guidelineRight; + void UI_Delay(uint32_t delay) { osDelay(delay); } @@ -60,21 +86,35 @@ void uiTask(void* arg) { chassisGUI->Init2(); osDelay(110); // Initialize crosshair GUI - crossairGui = new communication::CrossairGUI(UI, 988, 540, -70, -540, -540, -540, -540, -84); + crossairGui = new communication::CrossairGUI(UI, 968, 510, -70, -540, -540, -540, -540, -84); osDelay(110); - // Initialize supercapacitor GUI - char batteryStr[15] = "BATTERY"; - batteryGUI = new communication::CapGUI(UI, batteryStr); - osDelay(110); - batteryGUI->InitName(); + // Initialize guideline GUI (reverse-camera style diagonal lines) + UI->LineDraw(&guidelineLeft, "gl", UI_Graph_Add, 1, UI_Color_Yellow, 2, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_END_Y_OFFSET); + UI->LineDraw(&guidelineRight, "gr", UI_Graph_Add, 1, UI_Color_Yellow, 2, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_END_Y_OFFSET); + UI->GraphRefresh(2, guidelineLeft, guidelineRight); osDelay(110); + // Initialize supercapacitor GUI + // char batteryStr[15] = "BATTERY"; + // batteryGUI = new communication::CapGUI(UI, batteryStr); + // osDelay(110); + // batteryGUI->InitName(); + // osDelay(110); + // Initialize Gimbal GUI - gimbalGUI = new communication::GimbalGUI(UI); - osDelay(110); - gimbalGUI->Init2(); - osDelay(110); + // gimbalGUI = new communication::GimbalGUI(UI); + // osDelay(110); + // gimbalGUI->Init2(); + // osDelay(110); // Initialize self-diagnosis GUI char diagStr[29] = ""; @@ -88,10 +128,10 @@ void uiTask(void* arg) { // Initialize flywheel status GUI char wheelOnStr[15] = "FLYWHEEL ON"; char wheelOffStr[15] = "FLYWHEEL OFF"; - wheelGUI = new communication::StringGUI(UI, wheelOffStr, 1500, 430, UI_Color_Pink); - char boostModeStr[15] = "BOOST!"; - char boostOffStr[15] = " "; - boostGUI = new communication::StringGUI(UI, boostOffStr, 870, 630, UI_Color_Pink, 30); + wheelGUI = new communication::StringGUI(UI, wheelOffStr, 1500, 450, UI_Color_Pink, 30); + // char boostModeStr[15] = "BOOST!"; + // char boostOffStr[15] = " "; + // boostGUI = new communication::StringGUI(UI, boostOffStr, 870, 630, UI_Color_Pink, 30); // osDelay(110); char shootUnlockStr[15] = "UNLOCK"; char shootLockStr[15] = " "; @@ -101,23 +141,23 @@ void uiTask(void* arg) { osDelay(110); wheelGUI->Init(); osDelay(110); - boostGUI->Init(); - osDelay(110); - shootUnlockGUI->Init(); - osDelay(110); + // boostGUI->Init(); + // osDelay(110); + // shootUnlockGUI->Init(); + // osDelay(110); modeGUI->InitString(); osDelay(110); wheelGUI->InitString(); osDelay(110); - boostGUI->InitString(); - osDelay(110); + // boostGUI->InitString(); + // osDelay(110); shootUnlockGUI->InitString(); float relative_angle = 0; - float pitch_angle = 0; - float power_percent = 1; + // float pitch_angle = 0; + // float power_percent = 1; RemoteMode last_mode = REMOTE_MODE_KILL; ShootFricMode last_fric_mode = SHOOT_FRIC_MODE_STOP; - BoolEdgeDetector* boostEdgeDetector = new BoolEdgeDetector(false); + // BoolEdgeDetector* boostEdgeDetector = new BoolEdgeDetector(false); BoolEdgeDetector* turboShootDetector = new BoolEdgeDetector(false); BoolEdgeDetector* c_edge = new BoolEdgeDetector(false); BoolEdgeDetector* v_edge = new BoolEdgeDetector(false); @@ -131,23 +171,25 @@ void uiTask(void* arg) { BoolEdgeDetector* steer_motor_check_edge = new BoolEdgeDetector(false); BoolEdgeDetector* dbus_edge = new BoolEdgeDetector(false); BoolEdgeDetector* imu_cali_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* imu_temp_edge = new BoolEdgeDetector(false); - BoolEdgeDetector* shoot_jam_edge = new BoolEdgeDetector(false); + // BoolEdgeDetector* imu_temp_edge = new BoolEdgeDetector(false); + // BoolEdgeDetector* shoot_jam_edge = new BoolEdgeDetector(false); while (true) { // Update chassis GUI relative_angle = yaw_motor->GetThetaDelta(gimbal_param->yaw_offset_); - pitch_angle = pitch_motor->GetThetaDelta(gimbal_param->pitch_offset_); - chassisGUI->Update(chassis_vx / chassis_vx_max, chassis_vy / chassis_vy_max, relative_angle); + // pitch_angle = pitch_motor->GetThetaDelta(gimbal_param->pitch_offset_); + chassisGUI->Update(chassis_vx / chassis_vx_max, chassis_vy / chassis_vy_max, + relative_angle); osDelay(UI_OS_DELAY); - power_percent = battery_vol->GetBatteryPercentage(); + // power_percent = battery_vol->GetBatteryPercentage(); - batteryGUI->Update(power_percent); - osDelay(UI_OS_DELAY); + // batteryGUI->Update(power_percent); + // osDelay(UI_OS_DELAY); // Update Gimbal GUI - gimbalGUI->Update(pitch_diff * 200, -yaw_diff * 200, pitch_angle, relative_angle, imu->CaliDone()); - osDelay(UI_OS_DELAY); + // gimbalGUI->Update(pitch_diff * 200, -yaw_diff * 200, pitch_angle, relative_angle, + // imu->CaliDone()); + // osDelay(UI_OS_DELAY); // Update current mode GUI if (remote_mode != last_mode) { @@ -157,6 +199,10 @@ void uiTask(void* arg) { strcpy(modeStr, "STOP MODE "); modeColor = UI_Color_Purplish_red; break; + case REMOTE_MODE_KILL: + strcpy(modeStr, "STOP MODE "); + modeColor = UI_Color_Purplish_red; + break; case REMOTE_MODE_FOLLOW: strcpy(modeStr, "FOLLOW MODE "); modeColor = UI_Color_Green; @@ -181,17 +227,19 @@ void uiTask(void* arg) { // Update wheel status GUI if (last_fric_mode != shoot_flywheel_mode) { - char* wheelStr = shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? wheelOnStr : wheelOffStr; - uint32_t wheelColor = shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? UI_Color_Pink : UI_Color_Green; + char* wheelStr = + shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? wheelOnStr : wheelOffStr; + uint32_t wheelColor = + shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? UI_Color_Pink : UI_Color_Green; wheelGUI->Update(wheelStr, wheelColor); osDelay(UI_OS_DELAY); } - boostEdgeDetector->input(chassis_boost_flag); - if (boostEdgeDetector->edge()) { - char* boostStr = chassis_boost_flag ? boostModeStr : boostOffStr; - boostGUI->Update(boostStr, UI_Color_Pink); - osDelay(UI_OS_DELAY); - } + // boostEdgeDetector->input(chassis_boost_flag); + // if (boostEdgeDetector->edge()) { + // char* boostStr = chassis_boost_flag ? boostModeStr : boostOffStr; + // boostGUI->Update(boostStr, UI_Color_Pink); + // osDelay(UI_OS_DELAY); + // } turboShootDetector->input(turbo_shoot); if (turboShootDetector->edge()) { char* turboshootstr = turbo_shoot ? shootUnlockStr : shootLockStr; @@ -212,8 +260,8 @@ void uiTask(void* arg) { steer_motor_check_edge->input(steering_motor->IsOnline()); dbus_edge->input(dbus->IsOnline()); imu_cali_edge->input(imu->CaliDone()); - imu_temp_edge->input(true); - shoot_jam_edge->input(jam_notify_flags); + // imu_temp_edge->input(true); + // shoot_jam_edge->input(jam_notify_flags); if (fl_motor_check_edge->negEdge()) { strcpy(diagStr, "FL MOTOR OFFLINE "); diagGUI->Update(diagStr, UI_Delay, UI_Color_Pink); @@ -246,26 +294,26 @@ void uiTask(void* arg) { strcpy(diagStr, "DBUS OFFLINE "); diagGUI->Update(diagStr, UI_Delay, UI_Color_Pink); } - if (imu_cali_edge->posEdge()) { - strcpy(diagStr, "IMU CALIBRATION DONE"); - diagGUI->Update(diagStr, UI_Delay, UI_Color_Green); - } - if (imu_temp_edge->posEdge()) { - strcpy(diagStr, "IMU TEMP NOT SAFE "); - diagGUI->Update(diagStr, UI_Delay, UI_Color_Pink); - } - if (shoot_jam_edge->posEdge()) { - jam_notify_flags = false; - strcpy(diagStr, "STEER JAM"); - diagGUI->Update(diagStr, UI_Delay, UI_Color_Pink); - } + // if (imu_cali_edge->posEdge()) { + // strcpy(diagStr, "IMU CALIBRATION DONE"); + // diagGUI->Update(diagStr, UI_Delay, UI_Color_Green); + // } + // if (imu_temp_edge->posEdge()) { + // strcpy(diagStr, "IMU TEMP NOT SAFE "); + // diagGUI->Update(diagStr, UI_Delay, UI_Color_Pink); + // } + // if (shoot_jam_edge->posEdge()) { + // // jam_notify_flags = false; + // strcpy(diagStr, "STEER JAM"); + // diagGUI->Update(diagStr, UI_Delay, UI_Color_Pink); + // } } // clear self-diagnosis messages if (dbus->IsOnline()) { v_edge->input(dbus->keyboard.bit.V); } else { - v_edge->input(refereerc->remote_control.keyboard.bit.V); + v_edge->input(refereerc->vt13_packet.keyboard.bit.V); } if (v_edge->posEdge()) { @@ -274,72 +322,90 @@ void uiTask(void* arg) { osDelay(110); chassisGUI->Delete(); osDelay(110); - gimbalGUI->Delete2(); - osDelay(110); - gimbalGUI->Delete(); - osDelay(110); + // gimbalGUI->Delete2(); + // osDelay(110); + // gimbalGUI->Delete(); + // osDelay(110); crossairGui->Delete(); osDelay(110); - batteryGUI->DeleteName(); - osDelay(110); - batteryGUI->Delete(); + // Delete guidelines + UI->LineDraw(&guidelineLeft, "gl", UI_Graph_Del, 1, UI_Color_Yellow, 2, 0, 0, 0, 0); + UI->LineDraw(&guidelineRight, "gr", UI_Graph_Del, 1, UI_Color_Yellow, 2, 0, 0, 0, 0); + UI->GraphRefresh(2, guidelineLeft, guidelineRight); osDelay(110); + // batteryGUI->DeleteName(); + // osDelay(110); + // batteryGUI->Delete(); + // osDelay(110); modeGUI->Init(); osDelay(110); wheelGUI->Init(); osDelay(110); - boostGUI->Init(); - osDelay(110); + // boostGUI->Init(); + // osDelay(110); shootUnlockGUI->Init(); osDelay(110); modeGUI->InitString(); osDelay(110); wheelGUI->InitString(); osDelay(110); - boostGUI->InitString(); - osDelay(110); + // boostGUI->InitString(); + // osDelay(110); shootUnlockGUI->Init(); osDelay(110); diagGUI->Clear(UI_Delay); osDelay(110); chassisGUI->Init(); osDelay(110); - chassisGUI->Init2(); - osDelay(110); - gimbalGUI->Init(); - osDelay(110); - gimbalGUI->Init2(); - osDelay(110); + // chassisGUI->Init2(); + // osDelay(110); + // gimbalGUI->Init(); + // osDelay(110); + // gimbalGUI->Init2(); + // osDelay(110); crossairGui->Init(); osDelay(110); - batteryGUI->Init(); - osDelay(110); - batteryGUI->InitName(); - osDelay(110); + // Re-init guidelines + UI->LineDraw(&guidelineLeft, "gl", UI_Graph_Add, 1, UI_Color_Yellow, 2, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_END_Y_OFFSET); + UI->LineDraw(&guidelineRight, "gr", UI_Graph_Add, 1, UI_Color_Yellow, 2, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_END_Y_OFFSET); + UI->GraphRefresh(2, guidelineLeft, guidelineRight); + osDelay(110); + // batteryGUI->Init(); + // osDelay(110); + // osDelay(110); modeGUI->Init(); - osDelay(110); + // batteryGUI->InitName(); + // osDelay(110); wheelGUI->Init(); osDelay(110); - boostGUI->Init(); - osDelay(110); + // boostGUI->Init(); + // osDelay(110); modeGUI->InitString(); osDelay(110); wheelGUI->InitString(); osDelay(110); - boostGUI->InitString(); - osDelay(110); + // boostGUI->InitString(); + // osDelay(110); modeGUI->InitString(); osDelay(110); wheelGUI->InitString(); osDelay(110); - boostGUI->InitString(); - osDelay(110); + // boostGUI->InitString(); + // osDelay(110); continue; } if (dbus->IsOnline()) { c_edge->input(dbus->keyboard.bit.C); } else { - c_edge->input(refereerc->remote_control.keyboard.bit.C); + c_edge->input(refereerc->vt13_packet.keyboard.bit.C); } if (c_edge->posEdge()) { diagGUI->Clear(UI_Delay); From a20b9c462491d6b5a285135ceb7dfc207ea2ca13 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 15:59:50 +0800 Subject: [PATCH 7/8] =?UTF-8?q?feat(Hero/gimbal):=20=E9=81=A5=E6=8E=A7?= =?UTF-8?q?=E4=BB=BB=E5=8A=A1=E6=94=AF=E6=8C=81=20VT13=20=E5=B9=B6?= =?UTF-8?q?=E6=96=B0=E5=A2=9E=E9=80=80=E5=BC=B9=E6=A8=A1=E5=BC=8F=E4=B8=8E?= =?UTF-8?q?=2042mm=20=E7=83=AD=E9=87=8F=E9=99=90=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 调试输出改用 RTT，并使用模式字符串辅助函数打印状态 --- programs/Hero/gimbal/include/remote_task.h | 74 ++++++++- programs/Hero/gimbal/include/shoot_task.h | 2 +- programs/Hero/gimbal/src/main.cpp | 170 ++++++++++----------- programs/Hero/gimbal/src/remote_task.cpp | 100 +++++++----- programs/Hero/gimbal/src/shoot_task.cpp | 58 +++++-- 5 files changed, 260 insertions(+), 144 deletions(-) diff --git a/programs/Hero/gimbal/include/remote_task.h b/programs/Hero/gimbal/include/remote_task.h index c265bc4e..2c88e2b0 100644 --- a/programs/Hero/gimbal/include/remote_task.h +++ b/programs/Hero/gimbal/include/remote_task.h @@ -57,6 +57,7 @@ enum ShootMode { SHOOT_MODE_PREPARED = 2, SHOOT_MODE_SINGLE = 3, SHOOT_MODE_BURST = 4, + SHOOT_MODE_UNLOAD = 5, // 退弹 }; extern ShootMode shoot_load_mode; extern osThreadId_t remoteTaskHandle; @@ -71,4 +72,75 @@ const osThreadAttr_t remoteTaskAttribute = .tz_module = 0, .reserved = 0}; void remoteTask(void* arg); -void init_remote(); \ No newline at end of file +void init_remote(); + +inline const char* remote_mode_str(RemoteMode mode) { + switch (mode) { + case REMOTE_MODE_PREPARE: + return "PREPARE"; + break; + case REMOTE_MODE_KILL: + return "KILL"; + break; + case REMOTE_MODE_STOP: + return "STOP"; + break; + case REMOTE_MODE_FOLLOW: + return "FOLLOW"; + break; + case REMOTE_MODE_SPIN: + return "SPIN"; + break; + case REMOTE_MODE_ADVANCED: + return "ADVANCED"; + break; + default: + return "\033[31mUNKNOWN\033[0m"; + break; + } +} + + inline const char* shoot_fric_mode_str(ShootFricMode mode) { + switch (mode) { + case SHOOT_FRIC_MODE_DISABLE: + return "\033[31mDISABLE\033[0m"; + break; + case SHOOT_FRIC_MODE_STOP: + return "STOP"; + break; + case SHOOT_FRIC_MODE_PREPARING: + return "PREPARING"; + break; + case SHOOT_FRIC_MODE_PREPARED: + return "PREPARED"; + break; + default: + return "UNKNOWN"; + break; + } + } + +inline const char* shoot_load_mode_str(ShootMode mode) { + switch (mode) { + case SHOOT_MODE_DISABLE: + return "\033[31mDISABLE\033[0m"; + break; + case SHOOT_MODE_STOP: + return "STOP"; + break; + case SHOOT_MODE_SINGLE: + return "SINGLE"; + break; + case SHOOT_MODE_BURST: + return "BURST"; + break; + case SHOOT_MODE_PREPARED: + return "\033[31mREADY SHOOT!\033[0m"; + default: + return "UNKNOWN"; + break; + case SHOOT_MODE_UNLOAD: + return "UNLOAD"; + break; + } +} \ No newline at end of file diff --git a/programs/Hero/gimbal/include/shoot_task.h b/programs/Hero/gimbal/include/shoot_task.h index b7ee547b..027a0673 100644 --- a/programs/Hero/gimbal/include/shoot_task.h +++ b/programs/Hero/gimbal/include/shoot_task.h @@ -44,7 +44,7 @@ const osThreadAttr_t shootTaskAttribute = extern driver::Motor3508* flywheel_left; extern driver::Motor3508* flywheel_right; extern driver::Motor3508* steering_motor; -extern bool jam_notify_flags; +// extern bool jam_notify_flags; void shootTask(void* arg); void init_shoot(); void kill_shoot(); \ No newline at end of file diff --git a/programs/Hero/gimbal/src/main.cpp b/programs/Hero/gimbal/src/main.cpp index d109dc74..67bb1a07 100644 --- a/programs/Hero/gimbal/src/main.cpp +++ b/programs/Hero/gimbal/src/main.cpp @@ -36,24 +36,52 @@ #include "user_define.h" void RM_RTOS_Init(void) { bsp::SetHighresClockTimer(&BOARD_TIM_SYS); - print_use_uart(&huart1, true, 921600); + print_use_rtt(); + // print_use_uart(&huart1, true, 921600); init_can(); + clear_screen(); + print("CAN init success!"); + init_batt(); + clear_screen(); + print("BATTERTY init success!"); + init_imu(); + clear_screen(); + print("IMU init success!"); + init_buzzer(); + clear_screen(); + print("BUZZER init success!"); + init_referee(); + clear_screen(); + print("REFREE init success!"); + init_remote(); + clear_screen(); + print("REMOTE init success!"); + init_shoot(); + clear_screen(); + print("SCREEN init success!"); + init_gimbal(); + clear_screen(); + print("GIMBAL init success!"); + init_chassis(); + clear_screen(); + print("CHASSIS init success!"); + init_ui(); + clear_screen(); + print("UI init success!"); } void RM_RTOS_Threads_Init(void) { imuTaskHandle = osThreadNew(imuTask, nullptr, &imuTaskAttribute); buzzerTaskHandle = osThreadNew(buzzerTask, nullptr, &buzzerTaskAttribute); - // refereeTaskHandle = osThreadNew(refereeTask, nullptr, &refereeTaskAttribute); - // refereercTaskHandle = osThreadNew(refereercTask, nullptr, &refereercTaskAttribute); remoteTaskHandle = osThreadNew(remoteTask, nullptr, &remoteTaskAttribute); gimbalTaskHandle = osThreadNew(gimbalTask, nullptr, &gimbalTaskAttribute); chassisTaskHandle = osThreadNew(chassisTask, nullptr, &chassisTaskAttribute); @@ -65,102 +93,66 @@ void RM_RTOS_Threads_Init(void) { void RM_RTOS_Default_Task(const void* arg) { UNUSED(arg); osDelay(3000); + Buzzer_Sing(DJI); - // while (true) { - // clear_screen(); - // set_cursor(0, 0); - // flywheel_left->PrintData(); - // flywheel_right->PrintData(); - // osDelay(10); - // } - char s[50]; while (true) { set_cursor(0, 0); clear_screen(); - switch (remote_mode) { - case REMOTE_MODE_PREPARE: - strcpy(s, "PREPARE"); - break; - case REMOTE_MODE_STOP: - strcpy(s, "STOP"); - break; - case REMOTE_MODE_KILL: - strcpy(s, "KILL"); - break; - case REMOTE_MODE_FOLLOW: - strcpy(s, "MANUAL"); - break; - case REMOTE_MODE_SPIN: - strcpy(s, "SPIN"); - break; - default: - strcpy(s, "UNKNOWN"); - break; - } - print("Mode:%s\r\n", s); - // switch (shoot_fric_mode) { - // case SHOOT_FRIC_MODE_PREPARING: - // strcpy(s, "PREPARE"); - // break; - // case SHOOT_FRIC_MODE_STOP: - // strcpy(s, "STOP"); - // break; - // case SHOOT_FRIC_MODE_PREPARED: - // strcpy(s, "PREPARED"); - // break; - // case SHOOT_FRIC_MODE_DISABLE: - // strcpy(s, "DISABLE"); - // break; - // } - // print("Shoot Fric Mode:%s\r\n", s); - switch (shoot_load_mode) { - case SHOOT_MODE_PREPARING: - strcpy(s, "PREPARE"); - break; - case SHOOT_MODE_STOP: - strcpy(s, "STOP"); - break; - case SHOOT_MODE_PREPARED: - strcpy(s, "PREPARED"); - break; - case SHOOT_MODE_DISABLE: - strcpy(s, "DISABLE"); - break; - case SHOOT_MODE_SINGLE: - strcpy(s, "SINGLE"); - break; - case SHOOT_MODE_BURST: - strcpy(s, "BURST"); - break; - } - print("Shoot Mode:%s\r\n", s); + print("Mode:%s\r\n", remote_mode_str(remote_mode)); + print("Shoot Fric Mode:%s\r\n", shoot_fric_mode_str(shoot_flywheel_mode)); + print("Shoot Mode:%s\r\n", shoot_load_mode_str(shoot_load_mode)); + print("\r\n"); + + // Movement info print( - "# %.2f s, IMU %s\r\n", - HAL_GetTick() / 1000.0, - imu->DataReady() ? "\033[1;42mReady\033[0m" : "\033[1;41mNot Ready\033[0m" - ); - print("Temp: %.2f\r\n", imu->Temp); - print("Heater: %.2f\r\n", imu->TempPWM); + "DBUS [CH0: %-4d] [CH1: %-4d] [CH2: %-4d] [CH3: %-4d] [TWL: %d] [SWL: %d] [SWR: %d]" + "@ %d " + "ms\r\n", + dbus->ch0, dbus->ch1, dbus->ch2, dbus->ch3, dbus->ch4, dbus->swl, dbus->swr); + // VT13 info print( - "Euler Angles: %.2f, %.2f, %.2f\r\n", - imu->INS_angle[0] / PI * 180, - imu->INS_angle[1] / PI * 180, - imu->INS_angle[2] / PI * 180 - ); - print("Is Calibrated: %s\r\n", imu->CaliDone() ? "\033[1;42mYes\033[0m" : "\033[1;41mNo\033[0m"); - print("Yaw Motor: %.2f, %.2f\r\n", yaw_motor->GetTheta(), yaw_motor->GetOmega()); - print("Pitch Motor: %.2f, %.2f\r\n", pitch_motor->GetTheta(), pitch_motor->GetOmega()); - print("Chassis Volt: %.3f\r\n", referee->power_heat_data.chassis_volt / 1000.0); - print("Chassis Curr: %.3f\r\n", referee->power_heat_data.chassis_current / 1000.0); - print("Chassis Power: %.3f\r\n", referee->power_heat_data.chassis_power); + "VT13 [CH0: %-4d] [CH1: %-4d] [CH2: %-4d] [CH3: %-4d] [CH4: %-4d] [Mode: %d] [SWL: " + "%d] " + "[SWR: %d] [Trig: %d]\r\n", + refereerc->vt13_packet.remote.ch0, refereerc->vt13_packet.remote.ch1, + refereerc->vt13_packet.remote.ch2, refereerc->vt13_packet.remote.ch3, + refereerc->vt13_packet.remote.ch4, refereerc->vt13_packet.remote.mode_sw, + refereerc->vt13_packet.remote.swl, refereerc->vt13_packet.remote.swr, + refereerc->vt13_packet.remote.trigger); + print("Chassis %.3f %.3f %.3f\r\n", chassis->chassis_vy, chassis->chassis_vy, + chassis->chassis_vt); + print("Power %.3fV %.3fA %.3fW\r\n", referee->power_heat_data.chassis_volt / 1000.0, + referee->power_heat_data.chassis_current / 1000.0, + referee->power_heat_data.chassis_power); print("\r\n"); - print("Shooter Cooling Heat: %hu\r\n", referee->power_heat_data.shooter_id1_42mm_cooling_heat); + + // Shoot info + print("Shooter Cooling Heat: %hu\r\n", + referee->power_heat_data.shooter_id1_17mm_cooling_heat); print("Bullet Frequency: %hhu\r\n", referee->shoot_data.bullet_freq); print("Bullet Speed: %.3f\r\n", referee->shoot_data.bullet_speed); print("\r\n"); - print("Current HP %d/%d\n", referee->game_robot_status.remain_HP, referee->game_robot_status.max_HP); - print("Remain bullet %d\n", referee->bullet_remaining.bullet_remaining_num_42mm); - osDelay(75); + + // Online info + print("[DBUS %s] ", dbus->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("\r\n"); + print("[Referee %s] ", + referee->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("\r\n"); + print("[Referee RC %s] ", + refereerc->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("\r\n"); + print("[Yaw %s] ", + yaw_motor->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("\r\n"); + print("[Pitch %s] ", + pitch_motor->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("\r\n"); + print("[Flywheel %s %s]", + flywheel_left->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m", + flywheel_right->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("\r\n"); + osDelay(50); } } diff --git a/programs/Hero/gimbal/src/remote_task.cpp b/programs/Hero/gimbal/src/remote_task.cpp index 049b99e5..027af4a9 100644 --- a/programs/Hero/gimbal/src/remote_task.cpp +++ b/programs/Hero/gimbal/src/remote_task.cpp @@ -17,6 +17,7 @@ # Public License along with this program. If not, see # # . # ###########################################################*/ +#define DEBUG_MODE #include "remote_task.h" @@ -45,9 +46,7 @@ void remoteTask(void* arg) { bool mode_switch = false; bool shoot_fric_switch = false; bool shoot_switch = false; - // bool shoot_burst_switch = false; bool shoot_stop_switch = false; - // uint32_t shoot_burst_timestamp = 0; remote::switch_t last_state_r = remote::MID; remote::switch_t last_state_l = remote::MID; remote::keyboard_t last_keyboard; @@ -61,6 +60,7 @@ void remoteTask(void* arg) { memset(&last_keyboard, 0, sizeof(last_keyboard)); memset(&last_mouse, 0, sizeof(last_mouse)); bool is_dbus_offline; + bool is_vt13_offline; bool is_robot_dead; bool is_shoot_available; BoolEdgeDetector* z_edge = new BoolEdgeDetector(false); @@ -68,10 +68,17 @@ void remoteTask(void* arg) { BoolEdgeDetector* mouse_left_edge = new BoolEdgeDetector(false); BoolEdgeDetector* mouse_right_edge = new BoolEdgeDetector(false); BoolEdgeDetector* shoot_burst_edge = new BoolEdgeDetector(false); + // VT13 边沿检测器 + BoolEdgeDetector* vt13_pause_edge = new BoolEdgeDetector(false); + BoolEdgeDetector* vt13_left_edge = new BoolEdgeDetector(false); + BoolEdgeDetector* vt13_trigger_edge = new BoolEdgeDetector(false); + while (1) { - // Offline Detection && Security Check + // 检测遥控器是否离线，或者遥控器是否在安全模式下 is_dbus_offline = (!dbus->IsOnline()) || dbus->swr == remote::DOWN; - // Kill Detection + // 检测VT13是否离线或不在C模式 + is_vt13_offline = (!refereerc->IsOnline()) || + refereerc->vt13_packet.remote.mode_sw != remote::vt13_remote_t::MODE_C; #ifdef HAS_REFEREE is_robot_dead = referee->game_robot_status.remain_HP == 0; is_shoot_available = (referee->game_robot_status.shooter_heat_limit - @@ -81,7 +88,8 @@ void remoteTask(void* arg) { is_robot_dead = false; is_shoot_available = true; #endif - if (is_dbus_offline || is_robot_dead) { + // DBUS和VT13都离线，或者机器人死亡，才进入安全模式 + if ((is_dbus_offline && is_vt13_offline) || is_robot_dead) { if (!is_killed) { last_remote_mode = remote_mode; remote_mode = REMOTE_MODE_KILL; @@ -92,7 +100,7 @@ void remoteTask(void* arg) { if (is_killed) { remote_mode = last_remote_mode; shoot_load_mode = SHOOT_MODE_STOP; - shoot_fric_switch = SHOOT_FRIC_MODE_STOP; + shoot_flywheel_mode = SHOOT_FRIC_MODE_STOP; is_killed = false; } } @@ -107,7 +115,7 @@ void remoteTask(void* arg) { last_state_l = state_l; last_keyboard = keyboard; last_mouse = mouse; - // Update State + // Update State：优先使用DBUS，否则使用VT13 if (dbus->IsOnline()) { state_r = dbus->swr; state_l = dbus->swl; @@ -116,11 +124,11 @@ void remoteTask(void* arg) { } else if (refereerc->IsOnline()) { state_r = remote::MID; state_l = remote::MID; - keyboard = refereerc->remote_control.keyboard; - mouse = refereerc->remote_control.mouse; + keyboard = refereerc->vt13_packet.keyboard; + mouse = (remote::mouse_t)refereerc->vt13_packet.mouse; } - // Update Timestamp + // Update Edge Detectors z_edge->input(keyboard.bit.Z); shift_edge->input(keyboard.bit.SHIFT); mouse_left_edge->input(mouse.l); @@ -131,7 +139,13 @@ void remoteTask(void* arg) { turbo_shoot = !turbo_shoot; } - // remote mode switch + // VT13 边沿检测 + vt13_pause_edge->input(refereerc->vt13_packet.remote.pause); + vt13_left_edge->input(refereerc->vt13_packet.remote.swl); + vt13_trigger_edge->input(refereerc->vt13_packet.remote.trigger); + + // ========== 模式切换 ========== + // DT7 右摇杆上拨 switch (state_r) { case remote::UP: if (last_state_r == remote::MID && dbus->IsOnline()) { @@ -139,6 +153,7 @@ void remoteTask(void* arg) { } break; case remote::MID: + // 键盘 SHIFT 切换模式 if (shift_edge->posEdge()) { mode_switch = true; } @@ -146,6 +161,10 @@ void remoteTask(void* arg) { case remote::DOWN: break; } + // VT13 暂停键切换模式 + if (vt13_pause_edge->posEdge()) { + mode_switch = true; + } if (mode_switch) { mode_switch = false; RemoteMode next_mode = (RemoteMode)(remote_mode + 1); @@ -154,7 +173,9 @@ void remoteTask(void* arg) { } remote_mode = next_mode; } - // shoot mode switch + + // ========== 摩擦轮切换 ========== + // DT7 左摇杆上拨 switch (state_l) { case remote::UP: if (last_state_l == remote::MID && dbus->IsOnline()) { @@ -164,14 +185,7 @@ void remoteTask(void* arg) { case remote::DOWN: if (last_state_l == remote::MID && dbus->IsOnline()) { shoot_switch = true; - // shoot_burst_timestamp = 0; } - // else if (last_state_l == remote::DOWN && selftest.dbus) { - // shoot_burst_timestamp++; - // if (shoot_burst_timestamp > 500 * REMOTE_OS_DELAY) { - // shoot_burst_switch = true; - // } - // } break; case remote::MID: switch (last_state_l) { @@ -181,27 +195,34 @@ void remoteTask(void* arg) { case remote::UP: break; case remote::MID: + // 键盘 Z键 切换摩擦轮 if (z_edge->posEdge()) { shoot_fric_switch = true; } + // 鼠标左键 射击 if (mouse_left_edge->posEdge()) { shoot_switch = true; - // shoot_burst_timestamp = 0; - } else if (mouse_left_edge->get()) { - // shoot_burst_timestamp++; - // if (shoot_burst_timestamp > 500 * - // REMOTE_OS_DELAY) { - // shoot_burst_switch = true; - // } } else if (mouse_left_edge->negEdge()) { shoot_stop_switch = true; - } else { + } else if (!mouse_left_edge->get()) { shoot_stop_switch = true; } break; } break; } + // VT13 左键切换摩擦轮 + if (vt13_left_edge->posEdge()) { + shoot_fric_switch = true; + } + // VT13 扳机键射击 + if (vt13_trigger_edge->posEdge()) { + shoot_switch = true; + } + if (vt13_trigger_edge->negEdge()) { + shoot_stop_switch = true; + } + // 切换摩擦轮模式 if (shoot_fric_switch) { shoot_fric_switch = false; @@ -214,6 +235,18 @@ void remoteTask(void* arg) { shoot_load_mode = SHOOT_MODE_STOP; } } + // 退弹 - 不受摩擦轮状态限制 + if (refereerc->vt13_packet.remote.ch4 < 424 || refereerc->vt13_packet.mouse.mid) { + shoot_load_mode = SHOOT_MODE_UNLOAD; + } else if (shoot_load_mode == SHOOT_MODE_UNLOAD) { + // 退弹条件消失，根据摩擦轮状态恢复 + if (shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED) { + shoot_load_mode = SHOOT_MODE_PREPARED; + } else { + shoot_load_mode = SHOOT_MODE_STOP; + } + } + // 射出单颗子弹 if (shoot_switch) { shoot_switch = false; @@ -223,19 +256,6 @@ void remoteTask(void* arg) { shoot_load_mode = SHOOT_MODE_SINGLE; } } - // 射出连发子弹 - // if (shoot_burst_switch) { - // if (shoot_fric_mode == SHOOT_FRIC_MODE_PREPARED) { - // // 必须要在准备就绪或者发出单发子弹的情况下才能发射连发子弹 - // if (shoot_mode == SHOOT_MODE_PREPARED || shoot_mode == - // SHOOT_MODE_SINGLE) { - // shoot_mode = SHOOT_MODE_BURST; - // shoot_burst_switch = false; - // } - // } else { - // shoot_burst_switch = false; - // } - // } // 停止射击 if (shoot_stop_switch) { shoot_stop_switch = false; diff --git a/programs/Hero/gimbal/src/shoot_task.cpp b/programs/Hero/gimbal/src/shoot_task.cpp index f533ead3..1dbcc134 100644 --- a/programs/Hero/gimbal/src/shoot_task.cpp +++ b/programs/Hero/gimbal/src/shoot_task.cpp @@ -25,19 +25,21 @@ driver::Motor3508* flywheel_right = nullptr; driver::Motor3508* steering_motor = nullptr; -bool jam_notify_flags = false; +// bool jam_notify_flags = false; +static bool unload_cmd_latched = false; void jam_callback(void* args) { - driver::Motor3508* motor = static_cast(args); - jam_notify_flags = true; - float target = motor->GetTarget(); - if (target > motor->GetOutputShaftTheta()) { - float prev_target = motor->GetTarget() - 2 * PI / 5; - motor->SetTarget(prev_target); - } else { - float prev_target = motor->GetTarget() + 2 * PI / 5; - motor->SetTarget(prev_target); - } + UNUSED(args); + // driver::Motor3508* motor = static_cast(args); + // jam_notify_flags = true; + // float target = motor->GetTarget(); + // if (target > motor->GetOutputShaftTheta()) { + // float prev_target = motor->GetTarget() - 2 * PI / 5; + // motor->SetTarget(prev_target); + // } else { + // float prev_target = motor->GetTarget() + 2 * PI / 5; + // motor->SetTarget(prev_target); + // } } osThreadId_t shootTaskHandle; @@ -88,8 +90,8 @@ void shootTask(void* arg) { switch (shoot_flywheel_mode) { case SHOOT_FRIC_MODE_PREPARING: - flywheel_left->SetTarget(120.0f * 2 * PI); - flywheel_right->SetTarget(120.0f * 2 * PI); + flywheel_left->SetTarget(67.0f * 2 * PI); + flywheel_right->SetTarget(67.0f * 2 * PI); shoot_flywheel_mode = SHOOT_FRIC_MODE_PREPARED; shoot_load_mode = SHOOT_MODE_PREPARED; break; @@ -105,7 +107,37 @@ void shootTask(void* arg) { // laser->SetOutput(0); break; } + // 退弹处理 - 不受摩擦轮状态限制 + if (shoot_load_mode == SHOOT_MODE_UNLOAD) { + if (!unload_cmd_latched) { + steering_motor->SetTarget(steering_motor->GetTarget() - 2 * PI / 8, true); + unload_cmd_latched = true; // 只触发一次 + } + // 保持UNLOAD期间不覆写电机目标，让电机完成旋转 + } else { + if (unload_cmd_latched) { + // 刚退出UNLOAD，取消未完成的退弹旋转，锁定到当前位置 + steering_motor->SetTarget(steering_motor->GetOutputShaftTheta()); + } + unload_cmd_latched = false; // 退出UNLOAD后解锁，等待下次触发 + + if (shoot_flywheel_mode == SHOOT_FRIC_MODE_STOP) { + steering_motor->SetTarget(steering_motor->GetOutputShaftTheta()); + } + } if (shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED) { +#if SHOOT_REFEREE + // 42mm 枪管过热检测：当前热量接近热量上限时禁止发射 + int heat_limit = referee->game_robot_status.shooter_heat_limit; + int heat_current = referee->power_heat_data.shooter_id1_42mm_cooling_heat; + const int heat_threshold = 100; // 42mm 单发热量约 100，预留一发余量 + if (heat_current > heat_limit - heat_threshold) { + // 过热保护：锁定拨弹电机，跳过本轮发射 + steering_motor->SetTarget(steering_motor->GetOutputShaftTheta()); + osDelay(SHOOT_OS_DELAY); + continue; + } +#endif switch (shoot_load_mode) { case SHOOT_MODE_PREPARING: case SHOOT_MODE_PREPARED: From a7cc4045d689a93f2585ad79d72db4cf24a61991 Mon Sep 17 00:00:00 2001 From: Rinsutoringu Date: Tue, 13 Oct 2026 16:14:02 +0800 Subject: [PATCH 8/8] =?UTF-8?q?style:=20=E4=BD=BF=E7=94=A8=20clang-format?= =?UTF-8?q?=2018.1.8=20=E6=A0=BC=E5=BC=8F=E5=8C=96=20Hero=20=E5=8F=8A?= =?UTF-8?q?=E7=9B=B8=E5=85=B3=E9=A9=B1=E5=8A=A8=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- boards/drivers/DM_MC01/src/bsp_batteryvol.cpp | 1 - boards/drivers/include/dji_remote.h | 36 +++---- programs/Hero/chassis/src/main.cpp | 12 +-- programs/Hero/gimbal/include/chassis_task.h | 1 - programs/Hero/gimbal/include/remote_task.h | 52 +++++------ programs/Hero/gimbal/src/chassis_task.cpp | 25 ++--- programs/Hero/gimbal/src/gimbal_task.cpp | 18 ++-- programs/Hero/gimbal/src/main.cpp | 59 +++++++----- programs/Hero/gimbal/src/remote_task.cpp | 4 +- programs/Hero/gimbal/src/ui_task.cpp | 93 ++++++++++++------- 10 files changed, 163 insertions(+), 138 deletions(-) diff --git a/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp b/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp index b9ebbb56..967ed5a7 100644 --- a/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp +++ b/boards/drivers/DM_MC01/src/bsp_batteryvol.cpp @@ -25,7 +25,6 @@ namespace bsp { BatteryVol::BatteryVol(ADC_HandleTypeDef* hadc, uint32_t channel, uint32_t rank, uint32_t sampling_time) { adc_ = new bADC(hadc, channel, rank, sampling_time); adc_vrefint_ = new bADC(&hadc1, ADC_CHANNEL_VREFINT, 1, ADC_SAMPLETIME_3CYCLES); - } void BatteryVol::InitVREF() { unsigned char i = 0; diff --git a/boards/drivers/include/dji_remote.h b/boards/drivers/include/dji_remote.h index c09b19db..31850f57 100644 --- a/boards/drivers/include/dji_remote.h +++ b/boards/drivers/include/dji_remote.h @@ -36,22 +36,22 @@ namespace remote { typedef union { uint16_t code; struct { - uint16_t W : 1; // 前 - uint16_t S : 1; // 后 - uint16_t A : 1; // 左 - uint16_t D : 1; // 右 - uint16_t SHIFT : 1; // boost - uint16_t CTRL : 1; // 切换 小陀螺/自瞄/坦克/标准 模式 - uint16_t Q : 1; // 小陀螺加速/减速 - uint16_t E : 1; // 小陀螺加速/减速 - uint16_t R : 1; // - uint16_t F : 1; // - uint16_t G : 1; // - uint16_t Z : 1; // 摩擦轮 - uint16_t X : 1; // - uint16_t C : 1; // 清除自定义UI - uint16_t V : 1; // 强制刷新自定义UI - uint16_t B : 1; // + uint16_t W : 1; // 前 + uint16_t S : 1; // 后 + uint16_t A : 1; // 左 + uint16_t D : 1; // 右 + uint16_t SHIFT : 1; // boost + uint16_t CTRL : 1; // 切换 小陀螺/自瞄/坦克/标准 模式 + uint16_t Q : 1; // 小陀螺加速/减速 + uint16_t E : 1; // 小陀螺加速/减速 + uint16_t R : 1; // + uint16_t F : 1; // + uint16_t G : 1; // + uint16_t Z : 1; // 摩擦轮 + uint16_t X : 1; // + uint16_t C : 1; // 清除自定义UI + uint16_t V : 1; // 强制刷新自定义UI + uint16_t B : 1; // } __packed bit; } __packed keyboard_t; @@ -89,8 +89,8 @@ namespace remote { result.x = x; result.y = y; result.z = roll; - result.l = l; // shoot - result.r = r; // unload + result.l = l; // shoot + result.r = r; // unload return result; } } __packed vt13_mouse_t; diff --git a/programs/Hero/chassis/src/main.cpp b/programs/Hero/chassis/src/main.cpp index a91d0aa9..b33a192e 100644 --- a/programs/Hero/chassis/src/main.cpp +++ b/programs/Hero/chassis/src/main.cpp @@ -31,8 +31,6 @@ #include "bsp_batteryvol.h" #include "bsp_os.h" - - // bridge_can 是与顶部大疆c板进行通信的can通道 // chassis_can 是操控底盘四个电机的can通道 // damiao的can相对于大疆是反着的！！！一定要注意！ @@ -71,11 +69,11 @@ void RM_RTOS_Init() { .kd = 0, .max_out = 30000, .max_iout = 10000, - .deadband = 0, // 死区 - .A = 3 * PI, // 变速积分所能达到的最大值为A+B - .B = 2 * PI, // 启动变速积分的死区 - .output_filtering_coefficient = 0.1, // 输出滤波系数 - .derivative_filtering_coefficient = 0, // 微分滤波系数 + .deadband = 0, // 死区 + .A = 3 * PI, // 变速积分所能达到的最大值为A+B + .B = 2 * PI, // 启动变速积分的死区 + .output_filtering_coefficient = 0.1, // 输出滤波系数 + .derivative_filtering_coefficient = 0, // 微分滤波系数 .mode = control::ConstrainedPID::Integral_Limit | // 积分限幅 control::ConstrainedPID::OutputFilter | // 输出滤波 control::ConstrainedPID::Trapezoid_Intergral | // 梯形积分 diff --git a/programs/Hero/gimbal/include/chassis_task.h b/programs/Hero/gimbal/include/chassis_task.h index 153d4b04..79c6f4fa 100644 --- a/programs/Hero/gimbal/include/chassis_task.h +++ b/programs/Hero/gimbal/include/chassis_task.h @@ -54,4 +54,3 @@ extern bool chassis_boost_flag; const float chassis_vx_max = 660.0f; const float chassis_vy_max = 660.0f; const float chassis_vz_max = 660.0f; - diff --git a/programs/Hero/gimbal/include/remote_task.h b/programs/Hero/gimbal/include/remote_task.h index 2c88e2b0..786ec19e 100644 --- a/programs/Hero/gimbal/include/remote_task.h +++ b/programs/Hero/gimbal/include/remote_task.h @@ -57,7 +57,7 @@ enum ShootMode { SHOOT_MODE_PREPARED = 2, SHOOT_MODE_SINGLE = 3, SHOOT_MODE_BURST = 4, - SHOOT_MODE_UNLOAD = 5, // 退弹 + SHOOT_MODE_UNLOAD = 5, // 退弹 }; extern ShootMode shoot_load_mode; extern osThreadId_t remoteTaskHandle; @@ -78,67 +78,67 @@ inline const char* remote_mode_str(RemoteMode mode) { switch (mode) { case REMOTE_MODE_PREPARE: return "PREPARE"; - break; + break; case REMOTE_MODE_KILL: return "KILL"; - break; + break; case REMOTE_MODE_STOP: return "STOP"; - break; + break; case REMOTE_MODE_FOLLOW: return "FOLLOW"; - break; + break; case REMOTE_MODE_SPIN: return "SPIN"; - break; + break; case REMOTE_MODE_ADVANCED: return "ADVANCED"; - break; + break; default: return "\033[31mUNKNOWN\033[0m"; - break; + break; } } - inline const char* shoot_fric_mode_str(ShootFricMode mode) { - switch (mode) { - case SHOOT_FRIC_MODE_DISABLE: - return "\033[31mDISABLE\033[0m"; +inline const char* shoot_fric_mode_str(ShootFricMode mode) { + switch (mode) { + case SHOOT_FRIC_MODE_DISABLE: + return "\033[31mDISABLE\033[0m"; break; - case SHOOT_FRIC_MODE_STOP: - return "STOP"; + case SHOOT_FRIC_MODE_STOP: + return "STOP"; break; - case SHOOT_FRIC_MODE_PREPARING: - return "PREPARING"; + case SHOOT_FRIC_MODE_PREPARING: + return "PREPARING"; break; - case SHOOT_FRIC_MODE_PREPARED: - return "PREPARED"; + case SHOOT_FRIC_MODE_PREPARED: + return "PREPARED"; break; - default: - return "UNKNOWN"; + default: + return "UNKNOWN"; break; - } } +} inline const char* shoot_load_mode_str(ShootMode mode) { switch (mode) { case SHOOT_MODE_DISABLE: return "\033[31mDISABLE\033[0m"; - break; + break; case SHOOT_MODE_STOP: return "STOP"; - break; + break; case SHOOT_MODE_SINGLE: return "SINGLE"; - break; + break; case SHOOT_MODE_BURST: return "BURST"; - break; + break; case SHOOT_MODE_PREPARED: return "\033[31mREADY SHOOT!\033[0m"; default: return "UNKNOWN"; - break; + break; case SHOOT_MODE_UNLOAD: return "UNLOAD"; break; diff --git a/programs/Hero/gimbal/src/chassis_task.cpp b/programs/Hero/gimbal/src/chassis_task.cpp index 4ff6094d..67ad4bf7 100644 --- a/programs/Hero/gimbal/src/chassis_task.cpp +++ b/programs/Hero/gimbal/src/chassis_task.cpp @@ -73,21 +73,17 @@ void chassisTask(void* arg) { car_vx = (float)dbus->ch0 / dbus->ROCKER_MAX * speed_scale; car_vy = (float)dbus->ch1 / dbus->ROCKER_MAX * speed_scale; car_vt = (float)dbus->ch4 / dbus->ROCKER_MAX * speed_scale; - } else if (refereerc->IsOnline() && - (refereerc->vt13_packet.remote.ch0 != remote::vt13_remote_t::ROCKER_MID || - refereerc->vt13_packet.remote.ch1 != remote::vt13_remote_t::ROCKER_MID || - refereerc->vt13_packet.remote.ch4 != remote::vt13_remote_t::ROCKER_MID)) { + } else if (refereerc->IsOnline() && (refereerc->vt13_packet.remote.ch0 != remote::vt13_remote_t::ROCKER_MID || + refereerc->vt13_packet.remote.ch1 != remote::vt13_remote_t::ROCKER_MID || + refereerc->vt13_packet.remote.ch4 != remote::vt13_remote_t::ROCKER_MID)) { // 使用VT13遥控器摇杆 const float speed_scale = 0.5; - car_vx = - (float)(refereerc->vt13_packet.remote.ch0 - remote::vt13_remote_t::ROCKER_MID) / - remote::vt13_remote_t::ROCKER_RANGE * speed_scale; - car_vy = - (float)(refereerc->vt13_packet.remote.ch1 - remote::vt13_remote_t::ROCKER_MID) / - remote::vt13_remote_t::ROCKER_RANGE * speed_scale; - car_vt = - (float)(refereerc->vt13_packet.remote.ch4 - remote::vt13_remote_t::ROCKER_MID) / - remote::vt13_remote_t::ROCKER_RANGE * speed_scale; + car_vx = (float)(refereerc->vt13_packet.remote.ch0 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE * speed_scale; + car_vy = (float)(refereerc->vt13_packet.remote.ch1 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE * speed_scale; + car_vt = (float)(refereerc->vt13_packet.remote.ch4 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE * speed_scale; } else { // 使用键盘 const float keyboard_speed = keyboard.bit.SHIFT ? 1 : 0.5; @@ -129,8 +125,7 @@ void chassisTask(void* arg) { chassis_vt_pid_error = 0; } - static control::ConstrainedPID* chassis_vt_pid = - new control::ConstrainedPID(2 / (2 * PI), 0, 0, 0.5, 1); + static control::ConstrainedPID* chassis_vt_pid = new control::ConstrainedPID(2 / (2 * PI), 0, 0, 0.5, 1); float vt = chassis_vt_pid->ComputeOutput(chassis_vt_pid_error); if (chassis_vt_pid_error != 0) chassis_vt = vt; diff --git a/programs/Hero/gimbal/src/gimbal_task.cpp b/programs/Hero/gimbal/src/gimbal_task.cpp index 2906d047..e44ecfef 100644 --- a/programs/Hero/gimbal/src/gimbal_task.cpp +++ b/programs/Hero/gimbal/src/gimbal_task.cpp @@ -103,8 +103,8 @@ void gimbalTask(void* arg) { if (dbus->IsOnline()) { if (dbus->mouse.y != 0) { pitch_ratio = dbus->mouse.y / 32767.0 * 7.5 / 7.0; - } else if (refereerc->IsOnline() && (refereerc->vt13_packet.mouse.x != 0 || - refereerc->vt13_packet.mouse.y != 0)) { + } else if (refereerc->IsOnline() && + (refereerc->vt13_packet.mouse.x != 0 || refereerc->vt13_packet.mouse.y != 0)) { // DBUS在线但摇杆/鼠标无输入时，使用VT13鼠标 pitch_ratio = refereerc->vt13_packet.mouse.y / 32767.0 * 7.5 / 7.0; } else { @@ -112,8 +112,8 @@ void gimbalTask(void* arg) { } if (dbus->mouse.x != 0) { yaw_ratio = -dbus->mouse.x / 32767.0 * 7.5 / 7.0; - } else if (refereerc->IsOnline() && (refereerc->vt13_packet.mouse.x != 0 || - refereerc->vt13_packet.mouse.y != 0)) { + } else if (refereerc->IsOnline() && + (refereerc->vt13_packet.mouse.x != 0 || refereerc->vt13_packet.mouse.y != 0)) { yaw_ratio = -refereerc->vt13_packet.mouse.x / 32767.0 * 7.5 / 7.0; } else { yaw_ratio = -dbus->ch2 / 18000.0 / 7.0; @@ -122,12 +122,10 @@ void gimbalTask(void* arg) { // DBUS离线时使用VT13：优先摇杆，否则鼠标 if (refereerc->vt13_packet.remote.ch3 != remote::vt13_remote_t::ROCKER_MID || refereerc->vt13_packet.remote.ch2 != remote::vt13_remote_t::ROCKER_MID) { - pitch_ratio = - (float)(refereerc->vt13_packet.remote.ch2 - remote::vt13_remote_t::ROCKER_MID) / - remote::vt13_remote_t::ROCKER_RANGE / 18000.0 * 660.0 / 7.0; - yaw_ratio = - (float)-(refereerc->vt13_packet.remote.ch3 - remote::vt13_remote_t::ROCKER_MID) / - remote::vt13_remote_t::ROCKER_RANGE / 18000.0 * 660.0 / 7.0; + pitch_ratio = (float)(refereerc->vt13_packet.remote.ch2 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE / 18000.0 * 660.0 / 7.0; + yaw_ratio = (float)-(refereerc->vt13_packet.remote.ch3 - remote::vt13_remote_t::ROCKER_MID) / + remote::vt13_remote_t::ROCKER_RANGE / 18000.0 * 660.0 / 7.0; } else { pitch_ratio = -refereerc->vt13_packet.mouse.y / 32767.0 * 7.5 / 7.0; yaw_ratio = -refereerc->vt13_packet.mouse.x / 32767.0 * 7.5 / 7.0; diff --git a/programs/Hero/gimbal/src/main.cpp b/programs/Hero/gimbal/src/main.cpp index 67bb1a07..76d93611 100644 --- a/programs/Hero/gimbal/src/main.cpp +++ b/programs/Hero/gimbal/src/main.cpp @@ -109,27 +109,40 @@ void RM_RTOS_Default_Task(const void* arg) { "DBUS [CH0: %-4d] [CH1: %-4d] [CH2: %-4d] [CH3: %-4d] [TWL: %d] [SWL: %d] [SWR: %d]" "@ %d " "ms\r\n", - dbus->ch0, dbus->ch1, dbus->ch2, dbus->ch3, dbus->ch4, dbus->swl, dbus->swr); + dbus->ch0, + dbus->ch1, + dbus->ch2, + dbus->ch3, + dbus->ch4, + dbus->swl, + dbus->swr + ); // VT13 info print( "VT13 [CH0: %-4d] [CH1: %-4d] [CH2: %-4d] [CH3: %-4d] [CH4: %-4d] [Mode: %d] [SWL: " "%d] " "[SWR: %d] [Trig: %d]\r\n", - refereerc->vt13_packet.remote.ch0, refereerc->vt13_packet.remote.ch1, - refereerc->vt13_packet.remote.ch2, refereerc->vt13_packet.remote.ch3, - refereerc->vt13_packet.remote.ch4, refereerc->vt13_packet.remote.mode_sw, - refereerc->vt13_packet.remote.swl, refereerc->vt13_packet.remote.swr, - refereerc->vt13_packet.remote.trigger); - print("Chassis %.3f %.3f %.3f\r\n", chassis->chassis_vy, chassis->chassis_vy, - chassis->chassis_vt); - print("Power %.3fV %.3fA %.3fW\r\n", referee->power_heat_data.chassis_volt / 1000.0, - referee->power_heat_data.chassis_current / 1000.0, - referee->power_heat_data.chassis_power); + refereerc->vt13_packet.remote.ch0, + refereerc->vt13_packet.remote.ch1, + refereerc->vt13_packet.remote.ch2, + refereerc->vt13_packet.remote.ch3, + refereerc->vt13_packet.remote.ch4, + refereerc->vt13_packet.remote.mode_sw, + refereerc->vt13_packet.remote.swl, + refereerc->vt13_packet.remote.swr, + refereerc->vt13_packet.remote.trigger + ); + print("Chassis %.3f %.3f %.3f\r\n", chassis->chassis_vy, chassis->chassis_vy, chassis->chassis_vt); + print( + "Power %.3fV %.3fA %.3fW\r\n", + referee->power_heat_data.chassis_volt / 1000.0, + referee->power_heat_data.chassis_current / 1000.0, + referee->power_heat_data.chassis_power + ); print("\r\n"); // Shoot info - print("Shooter Cooling Heat: %hu\r\n", - referee->power_heat_data.shooter_id1_17mm_cooling_heat); + print("Shooter Cooling Heat: %hu\r\n", referee->power_heat_data.shooter_id1_17mm_cooling_heat); print("Bullet Frequency: %hhu\r\n", referee->shoot_data.bullet_freq); print("Bullet Speed: %.3f\r\n", referee->shoot_data.bullet_speed); print("\r\n"); @@ -137,21 +150,19 @@ void RM_RTOS_Default_Task(const void* arg) { // Online info print("[DBUS %s] ", dbus->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); print("\r\n"); - print("[Referee %s] ", - referee->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("[Referee %s] ", referee->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); print("\r\n"); - print("[Referee RC %s] ", - refereerc->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("[Referee RC %s] ", refereerc->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); print("\r\n"); - print("[Yaw %s] ", - yaw_motor->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("[Yaw %s] ", yaw_motor->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); print("\r\n"); - print("[Pitch %s] ", - pitch_motor->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print("[Pitch %s] ", pitch_motor->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); print("\r\n"); - print("[Flywheel %s %s]", - flywheel_left->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m", - flywheel_right->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m"); + print( + "[Flywheel %s %s]", + flywheel_left->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m", + flywheel_right->IsOnline() ? "\033[32mOnline\033[0m" : "\033[31mOffline\033[0m" + ); print("\r\n"); osDelay(50); } diff --git a/programs/Hero/gimbal/src/remote_task.cpp b/programs/Hero/gimbal/src/remote_task.cpp index 027af4a9..0bda283a 100644 --- a/programs/Hero/gimbal/src/remote_task.cpp +++ b/programs/Hero/gimbal/src/remote_task.cpp @@ -77,8 +77,8 @@ void remoteTask(void* arg) { // 检测遥控器是否离线，或者遥控器是否在安全模式下 is_dbus_offline = (!dbus->IsOnline()) || dbus->swr == remote::DOWN; // 检测VT13是否离线或不在C模式 - is_vt13_offline = (!refereerc->IsOnline()) || - refereerc->vt13_packet.remote.mode_sw != remote::vt13_remote_t::MODE_C; + is_vt13_offline = + (!refereerc->IsOnline()) || refereerc->vt13_packet.remote.mode_sw != remote::vt13_remote_t::MODE_C; #ifdef HAS_REFEREE is_robot_dead = referee->game_robot_status.remain_HP == 0; is_shoot_available = (referee->game_robot_status.shooter_heat_limit - diff --git a/programs/Hero/gimbal/src/ui_task.cpp b/programs/Hero/gimbal/src/ui_task.cpp index 180d762c..05d9ba4e 100644 --- a/programs/Hero/gimbal/src/ui_task.cpp +++ b/programs/Hero/gimbal/src/ui_task.cpp @@ -42,15 +42,15 @@ communication::DiagGUI* diagGUI = nullptr; // \ / ← end points (lower, near screen bottom) // // Left guideline -const int16_t GUIDELINE_LEFT_START_X_OFFSET = -50-240-140; // Start X (upper, close to center) -const int16_t GUIDELINE_LEFT_START_Y_OFFSET = -100; // Start Y (negative = below center) -const int16_t GUIDELINE_LEFT_END_X_OFFSET = -200-240-140; // End X (lower, spread outward) -const int16_t GUIDELINE_LEFT_END_Y_OFFSET = -440; // End Y (near screen bottom) +const int16_t GUIDELINE_LEFT_START_X_OFFSET = -50 - 240 - 140; // Start X (upper, close to center) +const int16_t GUIDELINE_LEFT_START_Y_OFFSET = -100; // Start Y (negative = below center) +const int16_t GUIDELINE_LEFT_END_X_OFFSET = -200 - 240 - 140; // End X (lower, spread outward) +const int16_t GUIDELINE_LEFT_END_Y_OFFSET = -440; // End Y (near screen bottom) // Right guideline -const int16_t GUIDELINE_RIGHT_START_X_OFFSET = 50+230+180; // Start X (upper, close to center) -const int16_t GUIDELINE_RIGHT_START_Y_OFFSET = -100; // Start Y (negative = below center) -const int16_t GUIDELINE_RIGHT_END_X_OFFSET = 200+230+180; // End X (lower, spread outward) -const int16_t GUIDELINE_RIGHT_END_Y_OFFSET = -440; // End Y (near screen bottom) +const int16_t GUIDELINE_RIGHT_START_X_OFFSET = 50 + 230 + 180; // Start X (upper, close to center) +const int16_t GUIDELINE_RIGHT_START_Y_OFFSET = -100; // Start Y (negative = below center) +const int16_t GUIDELINE_RIGHT_END_X_OFFSET = 200 + 230 + 180; // End X (lower, spread outward) +const int16_t GUIDELINE_RIGHT_END_Y_OFFSET = -440; // End Y (near screen bottom) const int16_t GUIDELINE_CENTER_X = 960; const int16_t GUIDELINE_CENTER_Y = 540; @@ -90,16 +90,30 @@ void uiTask(void* arg) { osDelay(110); // Initialize guideline GUI (reverse-camera style diagonal lines) - UI->LineDraw(&guidelineLeft, "gl", UI_Graph_Add, 1, UI_Color_Yellow, 2, - GUIDELINE_CENTER_X + GUIDELINE_LEFT_START_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_LEFT_START_Y_OFFSET, - GUIDELINE_CENTER_X + GUIDELINE_LEFT_END_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_LEFT_END_Y_OFFSET); - UI->LineDraw(&guidelineRight, "gr", UI_Graph_Add, 1, UI_Color_Yellow, 2, - GUIDELINE_CENTER_X + GUIDELINE_RIGHT_START_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_START_Y_OFFSET, - GUIDELINE_CENTER_X + GUIDELINE_RIGHT_END_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_END_Y_OFFSET); + UI->LineDraw( + &guidelineLeft, + "gl", + UI_Graph_Add, + 1, + UI_Color_Yellow, + 2, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_END_Y_OFFSET + ); + UI->LineDraw( + &guidelineRight, + "gr", + UI_Graph_Add, + 1, + UI_Color_Yellow, + 2, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_END_Y_OFFSET + ); UI->GraphRefresh(2, guidelineLeft, guidelineRight); osDelay(110); @@ -177,8 +191,7 @@ void uiTask(void* arg) { // Update chassis GUI relative_angle = yaw_motor->GetThetaDelta(gimbal_param->yaw_offset_); // pitch_angle = pitch_motor->GetThetaDelta(gimbal_param->pitch_offset_); - chassisGUI->Update(chassis_vx / chassis_vx_max, chassis_vy / chassis_vy_max, - relative_angle); + chassisGUI->Update(chassis_vx / chassis_vx_max, chassis_vy / chassis_vy_max, relative_angle); osDelay(UI_OS_DELAY); // power_percent = battery_vol->GetBatteryPercentage(); @@ -227,10 +240,8 @@ void uiTask(void* arg) { // Update wheel status GUI if (last_fric_mode != shoot_flywheel_mode) { - char* wheelStr = - shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? wheelOnStr : wheelOffStr; - uint32_t wheelColor = - shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? UI_Color_Pink : UI_Color_Green; + char* wheelStr = shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? wheelOnStr : wheelOffStr; + uint32_t wheelColor = shoot_flywheel_mode == SHOOT_FRIC_MODE_PREPARED ? UI_Color_Pink : UI_Color_Green; wheelGUI->Update(wheelStr, wheelColor); osDelay(UI_OS_DELAY); } @@ -366,16 +377,30 @@ void uiTask(void* arg) { crossairGui->Init(); osDelay(110); // Re-init guidelines - UI->LineDraw(&guidelineLeft, "gl", UI_Graph_Add, 1, UI_Color_Yellow, 2, - GUIDELINE_CENTER_X + GUIDELINE_LEFT_START_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_LEFT_START_Y_OFFSET, - GUIDELINE_CENTER_X + GUIDELINE_LEFT_END_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_LEFT_END_Y_OFFSET); - UI->LineDraw(&guidelineRight, "gr", UI_Graph_Add, 1, UI_Color_Yellow, 2, - GUIDELINE_CENTER_X + GUIDELINE_RIGHT_START_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_START_Y_OFFSET, - GUIDELINE_CENTER_X + GUIDELINE_RIGHT_END_X_OFFSET, - GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_END_Y_OFFSET); + UI->LineDraw( + &guidelineLeft, + "gl", + UI_Graph_Add, + 1, + UI_Color_Yellow, + 2, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_LEFT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_LEFT_END_Y_OFFSET + ); + UI->LineDraw( + &guidelineRight, + "gr", + UI_Graph_Add, + 1, + UI_Color_Yellow, + 2, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_START_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_START_Y_OFFSET, + GUIDELINE_CENTER_X + GUIDELINE_RIGHT_END_X_OFFSET, + GUIDELINE_CENTER_Y + GUIDELINE_RIGHT_END_Y_OFFSET + ); UI->GraphRefresh(2, guidelineLeft, guidelineRight); osDelay(110); // batteryGUI->Init();