diff --git a/hw/bsp/c-mini/config/infantry.config b/hw/bsp/c-mini/config/infantry.config
deleted file mode 100644
index 96e18272..00000000
--- a/hw/bsp/c-mini/config/infantry.config
+++ /dev/null
@@ -1,104 +0,0 @@
-# CONFIG_auto_generated_config_prefix_board-esp32-c3 is not set
-# CONFIG_auto_generated_config_prefix_board-node_imu is not set
-# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
-CONFIG_auto_generated_config_prefix_board-rm-c=y
-# CONFIG_auto_generated_config_prefix_board-Webots is not set
-# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
-CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
-# CONFIG_auto_generated_config_prefix_system-Linux is not set
-CONFIG_INIT_TASK_STACK_DEPTH=2048
-
-#
-# FreeRTOS
-#
-CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=512
-CONFIG_FREERTOS_USB_TASK_STACK_DEPTH=256
-CONFIG_FREERTOS_TERM_TASK_STACK_DEPTH=512
-# end of FreeRTOS
-
-# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set
-CONFIG_auto_generated_config_prefix_robot-infantry=y
-# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
-# CONFIG_auto_generated_config_prefix_robot-blink is not set
-# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set
-# CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set
-
-#
-# 设备
-#
-CONFIG_auto_generated_config_prefix_device-servo=y
-# CONFIG_auto_generated_config_prefix_device-simulator is not set
-CONFIG_REF_LAUNCH_SPEED=30
-CONFIG_REF_HEAT_LIMIT_17=100
-CONFIG_REF_HEAT_LIMIT_42=100
-CONFIG_REF_POWER_LIMIT=200
-CONFIG_REF_POWER_BUFF=100
-CONFIG_auto_generated_config_prefix_device-cap=y
-CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256
-
-#
-# 超级电容
-#
-CONFIG_CAP_PERCENT_NO_LIM=80
-CONFIG_CAP_PERCENT_WORK=30
-CONFIG_CAP_MAX_LOAD=100
-# end of 超级电容
-
-CONFIG_auto_generated_config_prefix_device-led_rgb=y
-CONFIG_DEVICE_LED_RGB_TASK_STACK_DEPTH=128
-CONFIG_auto_generated_config_prefix_device-referee=y
-CONFIG_DEVICE_REF_TRANS_TASK_STACK_DEPTH=256
-CONFIG_DEVICE_REF_RECV_TASK_STACK_DEPTH=256
-
-#
-# 裁判系统
-#
-# CONFIG_REF_VIRTUAL is not set
-# end of 裁判系统
-
-#
-# 操作手UI
-#
-CONFIG_UI_DYNAMIC_CYCLE=20
-CONFIG_UI_STATIC_CYCLE=1000
-# end of 操作手UI
-
-# CONFIG_auto_generated_config_prefix_device-laser is not set
-CONFIG_auto_generated_config_prefix_device-can=y
-CONFIG_auto_generated_config_prefix_device-motor=y
-CONFIG_auto_generated_config_prefix_device-bmi088=y
-CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
-# CONFIG_auto_generated_config_prefix_device-buzzer is not set
-# CONFIG_auto_generated_config_prefix_device-tof is not set
-# CONFIG_auto_generated_config_prefix_device-blink_led is not set
-CONFIG_auto_generated_config_prefix_device-dr16=y
-CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
-CONFIG_auto_generated_config_prefix_device-ahrs=y
-CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
-CONFIG_auto_generated_config_prefix_device-ai=y
-CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
-
-#
-# 上位机
-#
-# CONFIG_HOST_CTRL_PRIORITY is not set
-# end of 上位机
-
-# CONFIG_auto_generated_config_prefix_device-wearlab is not set
-CONFIG_auto_generated_config_prefix_device-imu=y
-CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256
-# end of 设备
-
-#
-# 模块
-#
-CONFIG_auto_generated_config_prefix_module-gimbal=y
-CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
-# CONFIG_auto_generated_config_prefix_module-balance is not set
-CONFIG_auto_generated_config_prefix_module-chassis=y
-CONFIG_MODULE_CHASSIS_TASK_STACK_DEPTH=384
-CONFIG_auto_generated_config_prefix_module-launcher=y
-CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
-# CONFIG_auto_generated_config_prefix_module-can_imu is not set
-# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set
-# end of 模块
diff --git a/hw/bsp/rm-c/config/helm_infantry.config b/hw/bsp/rm-c/config/helm_infantry.config
index 1f4348f3..0d5f0a96 100644
--- a/hw/bsp/rm-c/config/helm_infantry.config
+++ b/hw/bsp/rm-c/config/helm_infantry.config
@@ -1,50 +1,42 @@
-# CONFIG_auto_generated_config_prefix_board-c-mini is not set
-# CONFIG_auto_generated_config_prefix_board-dual_canfd is not set
-# CONFIG_auto_generated_config_prefix_board-esp32-c3-arduino is not set
-# CONFIG_auto_generated_config_prefix_board-esp32-c3-idf is not set
-# CONFIG_auto_generated_config_prefix_board-f103_can is not set
-# CONFIG_auto_generated_config_prefix_board-mangopi_r818 is not set
-# CONFIG_auto_generated_config_prefix_board-microswitch is not set
-# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
-# CONFIG_auto_generated_config_prefix_board-MiniPC_with_canfd is not set
-# CONFIG_auto_generated_config_prefix_board-node_imu is not set
CONFIG_auto_generated_config_prefix_board-rm-c=y
+# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
# CONFIG_auto_generated_config_prefix_board-Webots is not set
-# CONFIG_auto_generated_config_prefix_system-Bootloader is not set
-CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
+# CONFIG_auto_generated_config_prefix_board-f103_can is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-c3-idf is not set
+# CONFIG_auto_generated_config_prefix_board-c-mini is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-c3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-s3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-atom is not set
+# CONFIG_auto_generated_config_prefix_board-dual_canfd is not set
+# CONFIG_auto_generated_config_prefix_system-None is not set
# CONFIG_auto_generated_config_prefix_system-Linux is not set
# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
-# CONFIG_auto_generated_config_prefix_system-None is not set
+# CONFIG_auto_generated_config_prefix_system-Bootloader is not set
+CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
+CONFIG_INIT_TASK_STACK_DEPTH=2048
#
# FreeRTOS
#
-CONFIG_INIT_TASK_STACK_DEPTH=2048
CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=512
CONFIG_FREERTOS_USB_TASK_STACK_DEPTH=256
CONFIG_FREERTOS_TERM_TASK_STACK_DEPTH=512
# end of FreeRTOS
-# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-omni_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-arm_engineer is not set
# CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set
-# CONFIG_auto_generated_config_prefix_robot-blink is not set
-# CONFIG_auto_generated_config_prefix_robot-bootloader is not set
-# CONFIG_auto_generated_config_prefix_robot-canfd_to_uart is not set
-# CONFIG_auto_generated_config_prefix_robot-can_to_uart is not set
-# CONFIG_auto_generated_config_prefix_robot-custom_controller is not set
-# CONFIG_auto_generated_config_prefix_robot-dart is not set
# CONFIG_auto_generated_config_prefix_robot-drone_gimbal is not set
-# CONFIG_auto_generated_config_prefix_robot-engineer is not set
-CONFIG_auto_generated_config_prefix_robot-helm_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-sentry is not set
+# CONFIG_auto_generated_config_prefix_robot-dart is not set
# CONFIG_auto_generated_config_prefix_robot-hero is not set
# CONFIG_auto_generated_config_prefix_robot-infantry is not set
-# CONFIG_auto_generated_config_prefix_robot-microswitch is not set
-# CONFIG_auto_generated_config_prefix_robot-sentry is not set
-# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set
-# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
# CONFIG_auto_generated_config_prefix_robot-uart_net_config is not set
-# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
-# CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set
+# CONFIG_auto_generated_config_prefix_robot-canfd_imu is not set
+CONFIG_auto_generated_config_prefix_robot-helm_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
+# CONFIG_auto_generated_config_prefix_robot-blink is not set
#
# 组件
@@ -53,25 +45,58 @@ CONFIG_auto_generated_config_prefix_robot-helm_infantry=y
#
# 设备
#
-CONFIG_auto_generated_config_prefix_device-ahrs=y
-CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-laser is not set
+CONFIG_auto_generated_config_prefix_device-led_rgb=y
+CONFIG_DEVICE_LED_RGB_TASK_STACK_DEPTH=128
+CONFIG_auto_generated_config_prefix_device-motor=y
+CONFIG_auto_generated_config_prefix_device-can=y
+CONFIG_auto_generated_config_prefix_device-bmi088=y
+CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-net_config is not set
# CONFIG_auto_generated_config_prefix_device-ahrs-9 is not set
-CONFIG_auto_generated_config_prefix_device-ai=y
+CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-blink_led is not set
+# CONFIG_auto_generated_config_prefix_device-simulator is not set
+CONFIG_REF_LAUNCH_SPEED=30
+CONFIG_REF_HEAT_LIMIT_17=100
+CONFIG_REF_HEAT_LIMIT_42=100
+CONFIG_REF_POWER_LIMIT=200
+CONFIG_REF_POWER_BUFF=100
+# CONFIG_auto_generated_config_prefix_device-mmc5603 is not set
+CONFIG_auto_generated_config_prefix_device-servo=y
+CONFIG_auto_generated_config_prefix_device-referee=y
+CONFIG_DEVICE_REF_TRANS_TASK_STACK_DEPTH=256
+CONFIG_DEVICE_REF_RECV_TASK_STACK_DEPTH=256
+
+#
+# 裁判系统
+#
+# CONFIG_REF_VIRTUAL is not set
+# end of 裁判系统
+
+#
+# 操作手UI
+#
+CONFIG_UI_DYNAMIC_CYCLE=20
+CONFIG_UI_STATIC_CYCLE=1000
+# end of 操作手UI
+
+# CONFIG_auto_generated_config_prefix_device-ai is not set
CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
+CONFIG_HOST_CTRL_PRIORITY=y
+CONFIG_auto_generated_config_prefix_device-aim=y
#
# 上位机
#
-# CONFIG_HOST_CTRL_PRIORITY is not set
# end of 上位机
-# CONFIG_auto_generated_config_prefix_device-blink_led is not set
-CONFIG_auto_generated_config_prefix_device-bmi088=y
-CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
-# CONFIG_auto_generated_config_prefix_device-bq27220 is not set
+# CONFIG_auto_generated_config_prefix_device-mech is not set
+# CONFIG_auto_generated_config_prefix_device-mt6701 is not set
# CONFIG_auto_generated_config_prefix_device-buzzer is not set
-CONFIG_auto_generated_config_prefix_device-can=y
+# CONFIG_auto_generated_config_prefix_device-ina226 is not set
# CONFIG_auto_generated_config_prefix_device-canfd is not set
+CONFIG_auto_generated_config_prefix_device-ahrs=y
CONFIG_auto_generated_config_prefix_device-cap=y
CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256
@@ -83,73 +108,28 @@ CONFIG_CAP_PERCENT_WORK=30
CONFIG_CAP_MAX_LOAD=100
# end of 超级电容
-# CONFIG_auto_generated_config_prefix_device-custom_controller is not set
CONFIG_auto_generated_config_prefix_device-dr16=y
CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
# CONFIG_auto_generated_config_prefix_device-icm42688 is not set
-CONFIG_auto_generated_config_prefix_device-imu=y
-CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256
-# CONFIG_auto_generated_config_prefix_device-ina226 is not set
-# CONFIG_auto_generated_config_prefix_device-laser is not set
-CONFIG_auto_generated_config_prefix_device-led_rgb=y
-CONFIG_DEVICE_LED_RGB_TASK_STACK_DEPTH=128
-# CONFIG_auto_generated_config_prefix_device-mech is not set
-# CONFIG_auto_generated_config_prefix_device-microswitch is not set
-# CONFIG_auto_generated_config_prefix_device-mmc5603 is not set
-CONFIG_auto_generated_config_prefix_device-motor=y
-# CONFIG_auto_generated_config_prefix_device-net_config is not set
-CONFIG_auto_generated_config_prefix_device-referee=y
-CONFIG_DEVICE_REF_TRANS_TASK_STACK_DEPTH=256
-CONFIG_DEVICE_REF_RECV_TASK_STACK_DEPTH=256
-
-#
-# 裁判系统
-#
-# CONFIG_REF_VIRTUAL is not set
-CONFIG_REF_LAUNCH_SPEED=30
-CONFIG_REF_HEAT_LIMIT_17=100
-CONFIG_REF_HEAT_LIMIT_42=100
-CONFIG_REF_POWER_LIMIT=200
-CONFIG_REF_POWER_BUFF=100
-# end of 裁判系统
-
-#
-# 操作手UI
-#
-CONFIG_UI_DYNAMIC_CYCLE=20
-CONFIG_UI_STATIC_CYCLE=1000
-# end of 操作手UI
-
-CONFIG_auto_generated_config_prefix_device-servo=y
-# CONFIG_auto_generated_config_prefix_device-simulator is not set
-# CONFIG_auto_generated_config_prefix_device-spl06_001 is not set
-# CONFIG_auto_generated_config_prefix_device-tof is not set
# end of 设备
#
# 模块
#
-# CONFIG_auto_generated_config_prefix_module-balance is not set
-# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
-# CONFIG_auto_generated_config_prefix_module-canfd_to_uart is not set
-# CONFIG_auto_generated_config_prefix_module-can_imu is not set
-# CONFIG_auto_generated_config_prefix_module-can_imu_wearlab is not set
-# CONFIG_auto_generated_config_prefix_module-can_usart is not set
-# CONFIG_auto_generated_config_prefix_module-chassis is not set
-# CONFIG_auto_generated_config_prefix_module-custom_controller is not set
# CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set
-# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
-# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
-CONFIG_auto_generated_config_prefix_module-gimbal=y
-CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+# CONFIG_auto_generated_config_prefix_module-chassis is not set
+# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
+# CONFIG_auto_generated_config_prefix_module-omni_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-robot_arm is not set
CONFIG_auto_generated_config_prefix_module-helm_chassis=y
CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
+CONFIG_auto_generated_config_prefix_module-gimbal=y
+CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
+# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
+# CONFIG_auto_generated_config_prefix_module-performance is not set
CONFIG_auto_generated_config_prefix_module-launcher=y
+# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-canfd_imu is not set
# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set
-# CONFIG_auto_generated_config_prefix_module-microswitch is not set
-# CONFIG_auto_generated_config_prefix_module-ore_collect is not set
-# CONFIG_auto_generated_config_prefix_module-performance is not set
-# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
-# CONFIG_auto_generated_config_prefix_module-uart_update is not set
-# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set
# end of 模块
diff --git a/hw/bsp/rm-c/config/omni_infantry.config b/hw/bsp/rm-c/config/omni_infantry.config
new file mode 100644
index 00000000..4704de27
--- /dev/null
+++ b/hw/bsp/rm-c/config/omni_infantry.config
@@ -0,0 +1,135 @@
+CONFIG_auto_generated_config_prefix_board-rm-c=y
+# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
+# CONFIG_auto_generated_config_prefix_board-Webots is not set
+# CONFIG_auto_generated_config_prefix_board-f103_can is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-c3-idf is not set
+# CONFIG_auto_generated_config_prefix_board-c-mini is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-c3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-s3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-atom is not set
+# CONFIG_auto_generated_config_prefix_board-dual_canfd is not set
+# CONFIG_auto_generated_config_prefix_system-None is not set
+# CONFIG_auto_generated_config_prefix_system-Linux is not set
+# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
+# CONFIG_auto_generated_config_prefix_system-Bootloader is not set
+CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
+CONFIG_INIT_TASK_STACK_DEPTH=2048
+
+#
+# FreeRTOS
+#
+CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=512
+CONFIG_FREERTOS_USB_TASK_STACK_DEPTH=256
+CONFIG_FREERTOS_TERM_TASK_STACK_DEPTH=512
+# end of FreeRTOS
+
+CONFIG_auto_generated_config_prefix_robot-omni_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-arm_engineer is not set
+# CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set
+# CONFIG_auto_generated_config_prefix_robot-drone_gimbal is not set
+# CONFIG_auto_generated_config_prefix_robot-sentry is not set
+# CONFIG_auto_generated_config_prefix_robot-dart is not set
+# CONFIG_auto_generated_config_prefix_robot-hero is not set
+# CONFIG_auto_generated_config_prefix_robot-infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-uart_net_config is not set
+# CONFIG_auto_generated_config_prefix_robot-canfd_imu is not set
+# CONFIG_auto_generated_config_prefix_robot-helm_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
+# CONFIG_auto_generated_config_prefix_robot-blink is not set
+
+#
+# 组件
+#
+
+#
+# 设备
+#
+# CONFIG_auto_generated_config_prefix_device-laser is not set
+CONFIG_auto_generated_config_prefix_device-led_rgb=y
+CONFIG_DEVICE_LED_RGB_TASK_STACK_DEPTH=128
+CONFIG_auto_generated_config_prefix_device-motor=y
+CONFIG_auto_generated_config_prefix_device-can=y
+CONFIG_auto_generated_config_prefix_device-bmi088=y
+CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-net_config is not set
+# CONFIG_auto_generated_config_prefix_device-ahrs-9 is not set
+CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-blink_led is not set
+# CONFIG_auto_generated_config_prefix_device-simulator is not set
+CONFIG_REF_LAUNCH_SPEED=30
+CONFIG_REF_HEAT_LIMIT_17=100
+CONFIG_REF_HEAT_LIMIT_42=100
+CONFIG_REF_POWER_LIMIT=200
+CONFIG_REF_POWER_BUFF=100
+# CONFIG_auto_generated_config_prefix_device-mmc5603 is not set
+CONFIG_auto_generated_config_prefix_device-servo=y
+CONFIG_auto_generated_config_prefix_device-referee=y
+CONFIG_DEVICE_REF_TRANS_TASK_STACK_DEPTH=256
+CONFIG_DEVICE_REF_RECV_TASK_STACK_DEPTH=256
+
+#
+# 裁判系统
+#
+# CONFIG_REF_VIRTUAL is not set
+# end of 裁判系统
+
+#
+# 操作手UI
+#
+CONFIG_UI_DYNAMIC_CYCLE=20
+CONFIG_UI_STATIC_CYCLE=1000
+# end of 操作手UI
+
+# CONFIG_auto_generated_config_prefix_device-ai is not set
+CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
+CONFIG_HOST_CTRL_PRIORITY=y
+CONFIG_auto_generated_config_prefix_device-aim=y
+
+#
+# 上位机
+#
+# end of 上位机
+
+# CONFIG_auto_generated_config_prefix_device-mech is not set
+# CONFIG_auto_generated_config_prefix_device-mt6701 is not set
+# CONFIG_auto_generated_config_prefix_device-buzzer is not set
+# CONFIG_auto_generated_config_prefix_device-ina226 is not set
+# CONFIG_auto_generated_config_prefix_device-canfd is not set
+CONFIG_auto_generated_config_prefix_device-ahrs=y
+CONFIG_auto_generated_config_prefix_device-cap=y
+CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256
+
+#
+# 超级电容
+#
+CONFIG_CAP_PERCENT_NO_LIM=80
+CONFIG_CAP_PERCENT_WORK=30
+CONFIG_CAP_MAX_LOAD=100
+# end of 超级电容
+
+CONFIG_auto_generated_config_prefix_device-dr16=y
+CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
+# CONFIG_auto_generated_config_prefix_device-icm42688 is not set
+# end of 设备
+
+#
+# 模块
+#
+# CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set
+# CONFIG_auto_generated_config_prefix_module-chassis is not set
+# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
+CONFIG_auto_generated_config_prefix_module-omni_chassis=y
+# CONFIG_auto_generated_config_prefix_module-robot_arm is not set
+# CONFIG_auto_generated_config_prefix_module-helm_chassis is not set
+CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
+CONFIG_auto_generated_config_prefix_module-gimbal=y
+CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
+# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
+# CONFIG_auto_generated_config_prefix_module-performance is not set
+CONFIG_auto_generated_config_prefix_module-launcher=y
+# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-canfd_imu is not set
+# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set
+# end of 模块
diff --git a/src/component/comp_actuator.hpp b/src/component/comp_actuator.hpp
index 33f7157d..4415befa 100644
--- a/src/component/comp_actuator.hpp
+++ b/src/component/comp_actuator.hpp
@@ -79,6 +79,8 @@ class PosActuator {
float SpeedCalculate(float setpoint, float feedback, float dt);
+ float GetLastError() { return pid_speed_.GetError(); }
+
void Reset();
private:
diff --git a/src/component/comp_pid.hpp b/src/component/comp_pid.hpp
index 5cef0560..49111dde 100644
--- a/src/component/comp_pid.hpp
+++ b/src/component/comp_pid.hpp
@@ -40,6 +40,8 @@ class PID {
float Calculate(float sp, float fb, float fb_dot, float dt);
+ float GetError() { return last_.err; }
+
private:
Param param_;
diff --git a/src/device/aim/Kconfig b/src/device/aim/Kconfig
new file mode 100644
index 00000000..41f203e5
--- /dev/null
+++ b/src/device/aim/Kconfig
@@ -0,0 +1,15 @@
+config DEVICE_AI_TASK_STACK_DEPTH
+
+ int "AI任务堆栈大小"
+
+ range 128 4096
+
+ default 256
+
+ menu "上位机"
+
+ config HOST_CTRL_PRIORITY
+
+ tristate "优先把控制权交给上位机"
+
+ endmenu
diff --git a/src/device/aim/dev_aim.cpp b/src/device/aim/dev_aim.cpp
new file mode 100644
index 00000000..4fcab6d3
--- /dev/null
+++ b/src/device/aim/dev_aim.cpp
@@ -0,0 +1,150 @@
+#include "dev_aim.hpp"
+using namespace Device;
+
+#define AI_LEN_RX_BUFF (sizeof(AIM::RefForAI_t))
+#define AI_LEN_TX_BUFF (sizeof(AIM::TranToAI_t))
+
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
+static uint8_t txbuf[AI_LEN_TX_BUFF];
+
+/* clang-format off */
+AIM::AIM()
+ : event_(Message::Event::FindEvent("cmd_event")),
+ cmd_tp_("cmd_ai"),
+ data_ready_(false)
+{
+
+ auto rx_cplt_callback = [](void *arg) {
+ AIM *aim = static_cast<AIM *>(arg);
+ aim->data_ready_.Post();
+ };
+
+ bsp_uart_register_callback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, rx_cplt_callback,
+ this);
+
+ Component::CMD::RegisterController(this->cmd_tp_);
+ auto ai_thread = [](AIM *aim) {
+
+ auto eulr_sub = Message::Subscriber<Component::Type::Eulr>("imu_eulr");
+
+ while (1)
+ {
+ /* 接收imu和裁判系统数据 */
+ eulr_sub.DumpData(aim->eulr_);
+
+ /* 接收上位机数据 */
+ aim->StartRecv();
+ if (aim->data_ready_.Wait(0))
+ {
+ aim->PraseHost();
+ aim->PackCMD();
+ }
+
+ //发送数据给上位机
+ aim->PackMCU();
+ aim->StartTran();
+
+ System::Thread::Sleep(2);
+ }
+ };
+ this->thread_.Create(ai_thread, this, "aim_thread", DEVICE_AI_TASK_STACK_DEPTH,
+ System::Thread::REALTIME);
+}
+
+double convert_to_0_to_2pi(double theta_prime) {
+ double theta = fmod(theta_prime + 2 * M_PI, 2 * M_PI);
+ return theta;
+}
+
+bool AIM::PackCMD() {
+ /* 确保遥控器开关最高控制权，关遥控器即断控 */
+ if (!Component::CMD::Online()) {
+ return false;
+ }
+
+ if(from_host_.yaw == 0 && from_host_.pitch == 0)
+ {
+ this->to_cmd_.yaw = this->eulr_.yaw;
+ this->to_cmd_.pitch = this->eulr_.pit;
+ this->to_cmd_.roll = this->eulr_.rol;
+ return 0;
+ }
+
+ /* 控制源：AI */
+ if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
+ /* OP控制模式，用于鼠标右键自瞄 */
+ if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) {
+
+ if(from_host_.header == 0xA5)
+ {
+ this->to_cmd_.yaw = from_host_.yaw;
+ this->to_cmd_.pitch = from_host_.pitch;
+ this->to_cmd_.roll = this->eulr_.rol;
+ }
+
+ memcpy(&(this->cmd_.gimbal.eulr), &(this->to_cmd_),
+ sizeof(this->cmd_.gimbal.eulr));
+
+ this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
+
+ this->cmd_tp_.Publish(this->cmd_);
+ this->cmd_tp_.Publish(this->cmd_);
+ this->cmd_tp_.Publish(this->cmd_);
+ }
+ }
+ return true;
+}
+
+bool AIM::PraseHost() {
+ if (Component::CRC16::Verify(rxbuf, sizeof(this->from_host_))) {
+ this->cmd_.online = true;
+ this->last_online_time_ = bsp_time_get_ms();
+ memcpy(&(this->from_host_), rxbuf, sizeof(this->from_host_));
+ memset(rxbuf, 0, AI_LEN_RX_BUFF);
+ return true;
+ }
+ return false;
+}
+
+double convert_to_minus_pi_to_pi(double theta) {
+ while (theta > M_PI) {
+ theta -= 2 * M_PI;
+ }
+ while (theta < -M_PI) {
+ theta += 2 * M_PI;
+ }
+ return theta;
+}
+
+bool AIM::PackMCU()
+{
+ this->to_host_.header = 0x5A;
+ this->to_host_.detect_color = 0;
+ this->to_host_.reset_tracker = 0;
+ this->to_host_.current_v = 24;
+ this->to_host_.yaw = this->eulr_.yaw;
+ this->to_host_.pitch = this->eulr_.pit;
+ this->to_host_.roll = this->eulr_.rol;
+ this->to_host_.aim_x = this->from_host_.x;
+ this->to_host_.aim_y = this->from_host_.y;
+ this->to_host_.aim_z = this->from_host_.z;
+ this->to_host_.checksum = Component::CRC16::Calculate(
+ reinterpret_cast<const uint8_t *>(&(this->to_host_)),
+ sizeof(this->to_host_) - sizeof(uint16_t), CRC16_INIT);
+
+ return true;
+}
+bool AIM::StartTran()
+{
+ size_t len = sizeof(this->to_host_);
+
+ void *src = NULL;
+ src = &(this->to_host_);
+ memcpy(txbuf, src, len);
+ return bsp_uart_transmit(BSP_UART_AI, txbuf, len,false) == BSP_OK;
+}
+
+bool AIM::StartRecv()
+{
+ return bsp_uart_receive(BSP_UART_AI, rxbuf, sizeof(rxbuf), false) == BSP_OK;
+}
diff --git a/src/device/aim/dev_aim.hpp b/src/device/aim/dev_aim.hpp
new file mode 100644
index 00000000..9cd682d5
--- /dev/null
+++ b/src/device/aim/dev_aim.hpp
@@ -0,0 +1,123 @@
+#pragma once
+
+#include "bsp_uart.h"
+#include "comp_cmd.hpp"
+#include "comp_crc16.hpp"
+#include "dev_ahrs.hpp"
+#include "dev_referee.hpp"
+#include "device.hpp"
+
+#ifndef pi
+#define pi 3.1415926535f
+#endif
+
+/* clang-format off */
+namespace Device {
+class AIM {
+ public:
+
+ //MCU需要从上位机获取的数据
+ typedef struct __attribute__((packed)) RevforAI{
+ uint8_t header = 0xA5;
+ bool is_fire : 1;
+ uint8_t reserved : 1;
+ float x;
+ float y;
+ float z;
+ float v_yaw;
+ float pitch;
+ float yaw;
+ uint16_t checksum = 0;
+ }RefForAI_t;
+
+ typedef struct __attribute__ ((packed)) TranToAI{
+ uint8_t header = 0x5A;
+ uint8_t detect_color : 1; // 0-red 1-blue
+ bool reset_tracker : 1;
+ uint8_t reserved : 6;
+ float current_v; // m/s
+ float yaw;
+ float pitch;
+ float roll;
+ float aim_x;
+ float aim_y;
+ float aim_z;
+ uint16_t checksum = 0;
+ }TranToAI_t;
+
+ typedef struct {
+ uint8_t game_type;
+ Device::Referee::Status status;
+ uint8_t team;
+ uint8_t robot_id;
+ uint8_t robot_buff;
+ uint32_t ball_speed;
+ uint32_t max_hp;
+ uint32_t hp;
+
+ uint8_t game_progress;
+ uint16_t base_hp;
+ uint16_t outpost_hp;
+ uint16_t bullet_num;
+ uint16_t coin_num;
+ uint8_t own_virtual_shield_value;
+ float pos_x;
+ float pos_y;
+ float pos_angle;
+ float target_pos_x;
+ float target_pos_y;
+ uint8_t damaged_armor_id;
+ } RefForAI;
+
+ typedef struct
+ {
+ float yaw;
+ float pitch;
+ float roll;
+ }ToCMD_t;
+
+ typedef enum {
+ AI_STOP_FIRE,
+ AI_FIRE_COMMAND,
+ } AIControlData;
+
+ AIControlData aim_status_;
+
+ RefForAI_t from_host_;
+ TranToAI_t to_host_;
+
+/* ----------------------------------------------------------------------------------------------------- */
+ AIM();
+
+ //system
+ Component::Type::Eulr eulr_;
+
+ Component::CMD::Data cmd_{};
+
+ Referee::SentryDecisionData cmd_for_ref_;
+ /* Topic & Event */
+
+ Message::Event event_;
+
+ Message::Topic<Component::CMD::Data> cmd_tp_;
+
+ /* Task Control */
+ System::Thread thread_;
+
+ System::Semaphore data_ready_;
+
+ ToCMD_t to_cmd_;
+
+ bool StartRecv();
+
+ bool StartTran();
+
+ bool PraseHost();
+
+ bool PackCMD();
+
+ bool PackMCU();
+
+uint32_t last_online_time_ = 0;
+};
+} // namespace Device
diff --git a/src/device/aim/info.cmake b/src/device/aim/info.cmake
new file mode 100644
index 00000000..322c4b19
--- /dev/null
+++ b/src/device/aim/info.cmake
@@ -0,0 +1,6 @@
+CHECK_SUB_ENABLE(MODULE_ENABLE device)
+if(${MODULE_ENABLE})
+ file(GLOB CUR_SOURCES "${SUB_DIR}/*.cpp")
+ SUB_ADD_SRC(CUR_SOURCES)
+ SUB_ADD_INC(SUB_DIR)
+endif()
\ No newline at end of file
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index b0b494c8..9374a268 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -15,6 +15,8 @@ Gimbal::Gimbal(Param& param, float control_freq)
st_(param.st),
yaw_actuator_(this->param_.yaw_actr, control_freq),
pit_actuator_(this->param_.pit_actr, control_freq),
+ yaw_ai_actuator_(this->param_.yaw_ai_actr, control_freq),
+ pit_ai_actuator_(this->param_.pit_ai_actr, control_freq),
yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"),
pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"),
ctrl_lock_(true) {
@@ -24,14 +26,13 @@ Gimbal::Gimbal(Param& param, float control_freq)
switch (event) {
case SET_MODE_RELAX:
case SET_MODE_ABSOLUTE:
+ Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC);
gimbal->SetMode(static_cast<Mode>(event));
break;
- case START_AUTO_AIM:
+ case SET_MODE_AUTO_AIM:
Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI);
- break;
- case STOP_AUTO_AIM:
- Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC);
+ gimbal->SetMode((static_cast<Mode>(event)));
break;
}
gimbal->ctrl_lock_.Post();
@@ -61,6 +62,10 @@ Gimbal::Gimbal(Param& param, float control_freq)
gimbal->ctrl_lock_.Post();
gimbal->yaw_tp_.Publish(gimbal->yaw_);
+ gimbal->pit_tp_.Publish(gimbal->pit_);
+ gimbal->alpha_tp_.Publish(gimbal->alpha_);
+ gimbal->eulr_yaw1_tp_.Publish(gimbal->eulr_yaw1_);
+ gimbal->tan_pit_tp_.Publish(gimbal->tan_pit_);
/* 运行结束，等待下一次唤醒 */
gimbal->thread_.SleepUntil(2, last_online_time);
@@ -80,6 +85,91 @@ void Gimbal::UpdateFeedback() {
this->yaw_motor_.Update();
this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
+ this->eulr_yaw1_ = eulr_.yaw;
+ this->pit_ = atan(
+ sqrt(tan(ChangeAngleRange(eulr_.pit) -
+ (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)) *
+ tan(ChangeAngleRange(eulr_.pit) -
+ (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)) +
+ tan(eulr_.rol) * tan(eulr_.rol)));
+ this->alpha_ = this->GetAlpha();
+ this->slope_angle_ = this->pit_ * 180 / M_PI;
+ this->yaw_motor_value_ = this->yaw_motor_.GetAngle().Value();
+ this->pit_motor_value_ = this->pit_motor_.GetAngle().Value();
+
+ this->tan_pit_ =
+ tan(ChangeAngleRange(eulr_.pit) -
+ (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit));
+ this->tan_rol_ = tan(ChangeAngleRange(eulr_.rol));
+ this->test_angle_3_ = ChangeAngleRange(eulr_.pit);
+ // this->tan_yaw_ = -(this->yaw_motor_.GetAngle().Value() -
+ // this->param_.mech_zero.yaw) +
+ // ChangeAngleRange(eulr_.yaw);
+ this->test_angle_4_ = RotateVector3D(static_cast<float>(this->test_angle_3_),
+ static_cast<float>(this->tan_pit_),
+ static_cast<float>(this->tan_rol_));
+}
+
+double Gimbal::GetAlpha() {
+ if (this->tan_rol_ > 0 && this->tan_pit_ > 0) {
+ this->alpha_ = ((this->tan_rol_ * this->tan_rol_) /
+ (tan(this->pit_) * tan(this->pit_))) *
+ M_PI_2;
+ } else if (this->tan_rol_ > 0 && this->tan_pit_ < 0) {
+ this->alpha_ =
+ M_PI -
+ ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2;
+ } else if (this->tan_rol_ < 0 && this->tan_pit_ < 0) {
+ this->alpha_ =
+ M_PI +
+ ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2;
+ } else if (this->tan_rol_ < 0 && this->tan_pit_ > 0) {
+ this->alpha_ =
+ 2 * M_PI -
+ ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2;
+ }
+ return this->alpha_;
+}
+
+float Gimbal::ChangeAngleRange(float angle) {
+ angle = fmod(angle, 2 * M_PI);
+ if (angle < 0) {
+ angle += 2 * M_PI;
+ }
+ // 将角度从 [0, 2π] 转换到 [-π, π]
+ if (angle > M_PI) {
+ angle -= 2 * M_PI;
+ }
+
+ return angle;
+}
+
+float Gimbal::RotateVector3D(float x, float y, float z) {
+ float cos_x = cosf(x);
+ float sin_x = sinf(x);
+ float cos_y = cosf(y);
+ float sin_y = sinf(y);
+ float cos_z = cosf(z);
+ float sin_z = sinf(z);
+ float angle = 0.0f;
+ rotation_mat_[0][0] = cos_x * cos_y;
+ rotation_mat_[0][1] = -sin_x * cos_z + cos_x * sin_y * sin_z;
+ rotation_mat_[0][2] = sin_x * cos_z + cos_x * sin_y * cos_z;
+ rotation_mat_[1][0] = sin_x * cos_y;
+ rotation_mat_[1][1] = cos_x * cos_z + sin_x * sin_y * sin_z;
+ rotation_mat_[1][2] = -cos_x * sin_z + sin_x * sin_y * cos_z;
+ rotation_mat_[2][0] = -sin_y;
+ rotation_mat_[2][1] = cos_y * sin_z;
+ rotation_mat_[2][2] = cos_y * cos_z;
+ angle = atan(-sqrt(rotation_mat_[0][2] * rotation_mat_[0][2] +
+ rotation_mat_[1][2] * rotation_mat_[1][2]) /
+ rotation_mat_[2][2]);
+ return angle;
+ // return {{{cos_x * cos_y, -sin_x * cos_z + cos_x * sin_y * sin_z,
+ // sin_x * cos_z + cos_x * sin_y * cos_z},
+ // {sin_x * cos_y, cos_x * cos_z + sin_x * sin_y * sin_z,
+ // -cos_x * sin_z + sin_x * sin_y * cos_z},
+ // {-sin_y, cos_y * sin_z, cos_y * cos_z}}};
}
void Gimbal::Control() {
@@ -95,7 +185,7 @@ void Gimbal::Control() {
if (this->cmd_.mode == Component::CMD::GIMBAL_RELATIVE_CTRL) {
gimbal_yaw_cmd = this->cmd_.eulr.yaw * this->dt_ * GIMBAL_MAX_SPEED;
gimbal_pit_cmd = this->cmd_.eulr.pit * this->dt_ * GIMBAL_MAX_SPEED;
-
+ pit_ = gimbal_pit_cmd;
} else {
gimbal_yaw_cmd = Component::Type::CycleValue(this->cmd_.eulr.yaw) -
this->setpoint_.eulr_.yaw;
@@ -148,6 +238,16 @@ void Gimbal::Control() {
this->yaw_motor_.Control(yaw_out);
this->pit_motor_.Control(pit_out);
+ break;
+ case AI_CONTROL:
+ yaw_out = this->yaw_ai_actuator_.Calculate(
+ this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_);
+
+ pit_out = this->pit_ai_actuator_.Calculate(
+ this->setpoint_.eulr_.pit, this->gyro_.x, this->eulr_.pit, this->dt_);
+
+ this->yaw_motor_.Control(yaw_out);
+ this->pit_motor_.Control(pit_out);
break;
}
}
@@ -163,11 +263,22 @@ void Gimbal::SetMode(Mode mode) {
memcpy(&(this->setpoint_.eulr_), &(this->eulr_),
sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */
- if (this->mode_ == RELAX) {
- if (mode == ABSOLUTE) {
- this->setpoint_.eulr_.yaw = this->eulr_.yaw;
- }
- }
+ this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+ // if (this->mode_ == RELAX) {
+ // if (mode == ABSOLUTE) {
+ // this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+ // }
+ // }
+ // if (this->mode_ == ABSOLUTE) {
+ // if (mode == AI_CONTROL) {
+ // this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+ // }
+ // }
+ // if (this->mode_ == AI_CONTROL) {
+ // if (mode == ABSOLUTE) {
+ // this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+ // }
+ // }
this->mode_ = mode;
}
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index 378c063d..80a4a20d 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -20,7 +20,9 @@ class Gimbal {
/* 云台运行模式 */
typedef enum {
RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
- ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+ ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+ AI_CONTROL /*Host control mode, based on absolute coordinate system
+ control*/
} Mode;
enum {
@@ -34,8 +36,7 @@ class Gimbal {
typedef enum {
SET_MODE_RELAX,
SET_MODE_ABSOLUTE,
- START_AUTO_AIM,
- STOP_AUTO_AIM,
+ SET_MODE_AUTO_AIM,
} GimbalEvent;
typedef struct {
@@ -44,6 +45,8 @@ class Gimbal {
Component::PosActuator::Param yaw_actr;
Component::PosActuator::Param pit_actr;
+ Component::PosActuator::Param yaw_ai_actr;
+ Component::PosActuator::Param pit_ai_actr;
Device::RMMotor::Param yaw_motor;
Device::RMMotor::Param pit_motor;
@@ -67,12 +70,20 @@ class Gimbal {
void Control();
+ float ChangeAngleRange(float angle);
+
void SetMode(Mode mode);
static void DrawUIStatic(Gimbal *gimbal);
static void DrawUIDynamic(Gimbal *gimbal);
+ void GetSlopeAngle();
+
+ float RotateVector3D(float x, float y, float z);
+
+ double GetAlpha();
+
private:
uint64_t last_wakeup_ = 0;
@@ -80,6 +91,9 @@ class Gimbal {
float dt_ = 0.0f;
+ float yaw_motor_value_;
+ float pit_motor_value_;
+
Param param_;
Gimbal::Mode mode_ = RELAX; /* 云台模式 */
@@ -92,6 +106,8 @@ class Gimbal {
Component::PosActuator yaw_actuator_;
Component::PosActuator pit_actuator_;
+ Component::PosActuator yaw_ai_actuator_;
+ Component::PosActuator pit_ai_actuator_;
Device::RMMotor yaw_motor_;
Device::RMMotor pit_motor_;
@@ -101,8 +117,27 @@ class Gimbal {
System::Semaphore ctrl_lock_;
Message::Topic<float> yaw_tp_ = Message::Topic<float>("chassis_yaw");
+ Message::Topic<float> eulr_tp_ = Message::Topic<float>("ahrs_eulr");
+ Message::Topic<float> quat_tp_ = Message::Topic<float>("ahrs_quat");
+ Message::Topic<float> pit_tp_ = Message::Topic<float>("chassis_pitch");
+ Message::Topic<double> alpha_tp_ = Message::Topic<double>("chassis_alpha");
+ Message::Topic<float> eulr_yaw1_tp_ =
+ Message::Topic<float>("chassis_eulr_yaw1"); /* 首次云台偏航角 */
+ Message::Topic<double> tan_pit_tp_ =
+ Message::Topic<double>("chassis_tan_pit");
float yaw_;
+ float pit_;
+ double alpha_;
+ double slope_angle_;
+ float eulr_yaw1_;
+ double tan_pit_;
+ // Component::Type::CycleValue test_angle_2_;
+ double tan_rol_;
+ double test_angle_3_;
+ float test_angle_4_;
+
+ float rotation_mat_[3][3];
Component::UI::String string_;
diff --git a/src/module/helm_chassis/mod_helm_chassis.cpp b/src/module/helm_chassis/mod_helm_chassis.cpp
index 45c149e0..2b823c50 100644
--- a/src/module/helm_chassis/mod_helm_chassis.cpp
+++ b/src/module/helm_chassis/mod_helm_chassis.cpp
@@ -55,6 +55,8 @@ static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f;
#define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */
+// #define MOTOR_ROTATION_MAX_SPEED 9000; /* 电机的最大转速rpm */
+
using namespace Module;
template <typename Motor, typename MotorParam>
@@ -67,18 +69,18 @@ HelmChassis<Motor, MotorParam>::HelmChassis(Param& param, float control_freq)
for (int i = 0; i < 4; i++) {
this->pos_actr_.at(i) =
new Component::PosActuator(param.pos_param.at(i), control_freq);
- this->pos_motor_.at(i) =
- new Motor(param.pos_motor_param.at(i),
- (std::string("Chassis_pos_") + std::to_string(i)).c_str());
+ this->pos_motor_.at(i) = new Motor(
+ param.pos_motor_param.at(i),
+ (std::string(string_vector1_[i]) + std::to_string(0)).c_str());
}
for (uint8_t i = 0; i < 4; i++) {
this->speed_actr_.at(i) =
new Component::SpeedActuator(param.speed_param.at(i), control_freq);
- this->speed_motor_.at(i) =
- new Motor(param.speed_motor_param.at(i),
- (std::string("Chassis_speed_") + std::to_string(i)).c_str());
+ this->speed_motor_.at(i) = new Motor(
+ param.speed_motor_param.at(i),
+ (std::string(string_vector2_[i]) + std::to_string(0)).c_str());
}
ctrl_lock_.Post();
@@ -117,6 +119,10 @@ HelmChassis<Motor, MotorParam>::HelmChassis(Param& param, float control_freq)
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
auto raw_ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
auto cap_sub = Message::Subscriber<Device::Cap::Info>("cap_info");
+ auto pit_sub = Message::Subscriber<float>("chassis_pitch");
+ auto alpha_sub = Message::Subscriber<double>("chassis_alpha");
+ auto eulr_yaw1_sub = Message::Subscriber<float>("chassis_eulr_yaw1");
+ auto tan_pit_sub = Message::Subscriber<double>("chassis_tan_pit");
uint32_t last_online_time = bsp_time_get_ms();
@@ -126,6 +132,10 @@ HelmChassis<Motor, MotorParam>::HelmChassis(Param& param, float control_freq)
yaw_sub.DumpData(chassis->yaw_);
raw_ref_sub.DumpData(chassis->raw_ref_);
cap_sub.DumpData(chassis->cap_);
+ pit_sub.DumpData(chassis->pit_);
+ alpha_sub.DumpData(chassis->alpha_);
+ eulr_yaw1_sub.DumpData(chassis->eulr_yaw1_);
+ tan_pit_sub.DumpData(chassis->tan_pit_);
chassis->ctrl_lock_.Wait(UINT32_MAX);
/* 更新反馈值 */
@@ -152,7 +162,7 @@ void HelmChassis<Motor, MotorParam>::PraseRef() {
this->ref_.chassis_power_limit =
this->raw_ref_.robot_status.chassis_power_limit;
this->ref_.chassis_pwr_buff = this->raw_ref_.power_heat.chassis_pwr_buff;
- this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
+ // this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
this->ref_.status = this->raw_ref_.status;
}
template <typename Motor, typename MotorParam>
@@ -163,7 +173,17 @@ void HelmChassis<Motor, MotorParam>::UpdateFeedback() {
this->pos_motor_feedback_[i] = this->pos_motor_[i]->GetSpeed();
this->speed_motor_[i]->Update();
this->speed_motor_feedback_[i] = this->speed_motor_[i]->GetSpeed();
+ };
+ for (int i = 0; i < 4; ++i) {
+ this->pos_motor_value_[i] = this->pos_motor_[i]->GetAngle().Value();
+ this->speed_motor_value_[i] =
+ M_2PI - this->speed_motor_[i]->GetAngle().Value();
+ this->pos_err_value_[i] = this->pos_actr_[i]->GetLastError();
}
+ this->wz_test_ = speed_motor_feedback_[0] / 594.04f;
+ this->random_ = Getrandom();
+ this->test_angle01_ = GetRelateAngle();
+ this->test_angle02_ = std::floor(alpha_ / M_PI_4);
}
template <typename Motor, typename MotorParam>
@@ -176,16 +196,17 @@ bool HelmChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
}
float sum_motor_power = 0.0f;
- float motor_3508_power[4];
+ float motor_power[4];
for (size_t i = 0; i < len; i++) {
- motor_3508_power[i] =
+ motor_power[i] =
this->param_.toque_coefficient_3508 * fabsf(motor_out[i]) *
fabsf(speed_rpm[i]) +
this->param_.speed_2_coefficient_3508 * speed_rpm[i] * speed_rpm[i] +
this->param_.out_2_coefficient_3508 * motor_out[i] * motor_out[i];
- sum_motor_power += motor_3508_power[i];
+ sum_motor_power += motor_power[i];
}
sum_motor_power += this->param_.constant_3508;
+ power_ = sum_motor_power;
if (sum_motor_power > power_limit) {
for (size_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / sum_motor_power;
@@ -193,24 +214,104 @@ bool HelmChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
}
return true;
}
+
template <typename Motor, typename MotorParam>
uint16_t HelmChassis<Motor, MotorParam>::MAXSPEEDGET(float power_limit) {
uint16_t speed = 0;
if (power_limit <= 50.0f) {
speed = 5000;
} else if (power_limit <= 60.0f) {
- speed = 5500;
- } else if (power_limit <= 70.0f) {
speed = 6000;
- } else if (power_limit <= 80.0f) {
+ } else if (power_limit <= 70.0f) {
speed = 6500;
- } else if (power_limit <= 100.0f) {
+ } else if (power_limit <= 80.0f) {
speed = 7000;
- } else {
+ } else if (power_limit <= 100.0f) {
speed = 7500;
+ } else {
+ speed = 7000;
}
return speed;
}
+template <typename Motor, typename MotorParam>
+int HelmChassis<Motor, MotorParam>::Getrandom() {
+ static int last_random_value = 1;
+ if (this->mode_ != ROTOR) {
+ this->now_ = bsp_time_get();
+
+ this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+
+ srand(static_cast<unsigned int>(this->now_));
+ int random_value = rand() % 2;
+ if (random_value == 0) {
+ random_value = -1;
+ } else {
+ random_value = 1;
+ };
+
+ last_random_value = random_value;
+ }
+ return last_random_value;
+}
+
+template <typename Motor, typename MotorParam>
+double HelmChassis<Motor, MotorParam>::GetTorqueLength(float angle_a,
+ double angle_b,
+ int selection) {
+ double cofficent = 0, cofficent_1 = 0, cofficent_2 = 0, cofficent_3 = 0,
+ cofficent_4 = 0;
+
+ cofficent_1 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+ 0.081 * tan(angle_a) * cos(3 * M_PI_4 + angle_b));
+ cofficent_2 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+ 0.081 * tan(angle_a) * cos(3 * M_PI_4 - angle_b));
+ cofficent_3 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+ 0.081 * tan(angle_a) * cos(M_PI_4 + angle_b));
+ cofficent_4 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+ 0.081 * tan(angle_a) * cos(M_PI_4 - angle_b));
+ switch (selection) {
+ case 0:
+ cofficent =
+ (1 / ((cofficent_1 / cofficent_2) + (cofficent_1 / cofficent_3) +
+ (cofficent_1 / cofficent_4) + 1)) *
+ 2.8 * sin(pit_);
+ break;
+ case 1:
+ cofficent =
+ (1 / ((cofficent_2 / cofficent_1) + (cofficent_2 / cofficent_3) +
+ (cofficent_2 / cofficent_4) + 1)) *
+ 2.8 * sin(pit_);
+ break;
+ case 2:
+ cofficent =
+ (1 / ((cofficent_3 / cofficent_1) + (cofficent_3 / cofficent_2) +
+ (cofficent_3 / cofficent_4) + 1)) *
+ 2.8 * sin(pit_);
+ break;
+ case 3:
+ cofficent =
+ (1 / ((cofficent_4 / cofficent_1) + (cofficent_4 / cofficent_2) +
+ (cofficent_4 / cofficent_3) + 1)) *
+ 2.8 * sin(pit_);
+ break;
+ default:
+ break;
+ }
+ return cofficent;
+}
+
+template <typename Motor, typename MotorParam>
+float HelmChassis<Motor, MotorParam>::GetRelateAngle() {
+ static float last_relate_angle = 0;
+ if (pit_ < 0.1) {
+ float relate_angle = 0;
+ relate_angle = this->eulr_yaw1_;
+
+ last_relate_angle = relate_angle;
+ }
+ return last_relate_angle;
+}
+
template <typename Motor, typename MotorParam>
void HelmChassis<Motor, MotorParam>::Control() {
this->now_ = bsp_time_get();
@@ -218,9 +319,8 @@ void HelmChassis<Motor, MotorParam>::Control() {
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
this->last_wakeup_ = this->now_;
- // max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit);
- max_motor_rotational_speed_ =
- this->MAXSPEEDGET(this->ref_.chassis_power_limit);
+ max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit);
+
/* 计算vx、vy */
switch (this->mode_) {
case HelmChassis::BREAK: /* 刹车/放松模式电机停止 */
@@ -231,6 +331,7 @@ void HelmChassis<Motor, MotorParam>::Control() {
/* 独立模式控制向量与运动向量相等 */
case HelmChassis::INDENPENDENT:
case HelmChassis::CHASSIS_6020_FOLLOW_GIMBAL: {
+ // float tmp = 0;
float tmp = sqrtf(cmd_.x * cmd_.x + cmd_.y * cmd_.y) * 1.41421f / 2.0f;
clampf(&tmp, -1.0f, 1.0f);
@@ -244,8 +345,7 @@ void HelmChassis<Motor, MotorParam>::Control() {
}
break;
}
- case HelmChassis::CHASSIS_FOLLOW_GIMBAL:
- case HelmChassis::ROTOR: {
+ case HelmChassis::CHASSIS_FOLLOW_GIMBAL: {
float beta = this->yaw_;
float cos_beta = cosf(beta);
float sin_beta = sinf(beta);
@@ -253,6 +353,21 @@ void HelmChassis<Motor, MotorParam>::Control() {
this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
break;
}
+ case HelmChassis::ROTOR: {
+ if (random_ == 1) {
+ deviation_angle_value_ =
+ this->yaw_ + (0.05 * (speed_motor_feedback_[0] / 594.04f));
+ } else {
+ deviation_angle_value_ =
+ this->yaw_ - (0.054 * (speed_motor_feedback_[0] / 594.04f));
+ }
+ float beta = deviation_angle_value_;
+ float cos_beta = cosf(beta);
+ float sin_beta = sinf(beta);
+ this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
+ this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
+ break;
+ }
}
/* 计算wz */
@@ -278,8 +393,8 @@ void HelmChassis<Motor, MotorParam>::Control() {
break;
case HelmChassis::ROTOR:
/* 陀螺模式底盘以一定速度旋转 */
- this->move_vec_.wz =
- this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
+ this->move_vec_.wz = Getrandom();
+ // this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
break;
default:
XB_ASSERT(false);
@@ -310,19 +425,28 @@ void HelmChassis<Motor, MotorParam>::Control() {
}
break;
case HelmChassis::CHASSIS_FOLLOW_GIMBAL:
- case HelmChassis::ROTOR: {
+ case HelmChassis::ROTOR:
float x = 0, y = 0, wheel_pos = 0;
for (int i = 0; i < 4; i++) {
wheel_pos = -i * M_PI / 2.0f + M_PI / 4.0f * 3.0f;
x = sinf(wheel_pos) * move_vec_.wz + move_vec_.vx;
y = cosf(wheel_pos) * move_vec_.wz + move_vec_.vy;
setpoint_.wheel_pos[i] = -(atan2(y, x) - M_PI / 2.0f);
- setpoint_.motor_rotational_speed[i] = max_motor_rotational_speed_ *
- sqrtf(x * x + y * y) * 1.41421f /
- 2.0f;
+
+ if ((move_vec_.vx < 0.05 && move_vec_.vx > -0.05) ||
+ (move_vec_.vy < 0.05 && move_vec_.vy > -0.05)) {
+ setpoint_.motor_rotational_speed[i] =
+ max_motor_rotational_speed_ * sqrt(x * x + y * y);
+ } else {
+ setpoint_.motor_rotational_speed[i] = max_motor_rotational_speed_ *
+ sqrt(x * x + y * y) *
+ (1.41421f / 2.0f);
+ }
+ // float tmp = setpoint_.motor_rotational_speed[i];
+ // clampf(&tmp, -max_motor_rotational_speed_,
+ // max_motor_rotational_speed_);
}
break;
- }
}
/* 寻找电机最近角度 */
@@ -336,19 +460,100 @@ void HelmChassis<Motor, MotorParam>::Control() {
}
}
+ // //底盘前馈2.0
+ // if (pit_ > 0.01) {
+ // forward_coefficient_[0] =
+ // -0.35865 * sin(pit_) * tan(pit_) + 0.913125 * sin(pit_);
+ // forward_coefficient_[1] =
+ // -0.35865 * sin(pit_) * tan(pit_) + 0.913125 * sin(pit_);
+ // forward_coefficient_[2] =
+ // 0.35865 * sin(pit_) * tan(pit_) + 0.95542 * sin(pit_);
+ // forward_coefficient_[3] =
+ // 0.35865 * sin(pit_) * tan(pit_) + 0.95542 * sin(pit_);
+ // } else {
+ // for (float& i : forward_coefficient_) {
+ // i = 0;
+ // }
+ // }
+
+ /* 底盘前馈3.0 */
+
+ if (pit_ > 0.04 && mode_ != ROTOR && abs(cmd_.y) > 0.01) {
+ constexpr int TERM[8][4] = {{1, 0, 2, 3}, {2, 0, 1, 3}, {3, 1, 0, 2},
+ {3, 2, 0, 1}, {2, 3, 1, 0}, {1, 3, 2, 0},
+ {0, 2, 3, 1}, {0, 1, 3, 2}};
+ int state = static_cast<int>(std::floor(alpha_ / M_PI_4));
+ switch (state) {
+ case 0: // 1023
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[0][i]);
+ }
+ break;
+ case 1: // 2013
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[1][i]);
+ }
+ break;
+ case 2: // 3102
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[2][i]);
+ }
+ break;
+ case 3: // 3201
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[3][i]);
+ }
+ break;
+ case 4: // 2310
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[4][i]);
+ }
+ break;
+ case 5: // 1320
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[5][i]);
+ }
+ break;
+ case 6: // 0231
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[6][i]);
+ }
+ break;
+ case 7: // 0132
+ for (int i = 0; i < 4; i++) {
+ forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[7][i]);
+ }
+ break;
+ default:
+ break;
+ }
+ } else {
+ for (float& i : forward_coefficient_) {
+ i = 0;
+ }
+ }
+
+ if ((tan_pit_ < 0 && cmd_.y > 0) || (tan_pit_ > 0 && cmd_.y < 0)) {
+ for (float& i : forward_coefficient_) {
+ i = -i;
+ }
+ }
+
/* 输出计算 */
for (int i = 0; i < 4; i++) {
if (motor_reverse_[i]) {
out_.speed_motor_out[i] =
speed_actr_[i]->Calculate(-setpoint_.motor_rotational_speed[i],
- speed_motor_[i]->GetSpeed(), dt_);
+ speed_motor_[i]->GetSpeed(), dt_) -
+ forward_coefficient_[i];
out_.motor6020_out[i] = pos_actr_[i]->Calculate(
setpoint_.wheel_pos[i] + M_PI + this->param_.mech_zero[i],
pos_motor_[i]->GetSpeed(), pos_motor_[i]->GetAngle(), dt_);
} else {
out_.speed_motor_out[i] =
speed_actr_[i]->Calculate(setpoint_.motor_rotational_speed[i],
- speed_motor_[i]->GetSpeed(), dt_);
+ speed_motor_[i]->GetSpeed(), dt_) +
+ forward_coefficient_[i];
out_.motor6020_out[i] = pos_actr_[i]->Calculate(
setpoint_.wheel_pos[i] + this->param_.mech_zero[i],
pos_motor_[i]->GetSpeed(), pos_motor_[i]->GetAngle(), dt_);
@@ -368,20 +573,24 @@ void HelmChassis<Motor, MotorParam>::Control() {
clampf(&percentage, 0.0f, 1.0f);
/* 控制 */
- float max_power_limit = ref_.chassis_power_limit + ref_.chassis_power_limit *
- 0.2 *
- this->cap_.percentage_;
+ // float max_power_limit = ref_.chassis_power_limit + ref_.chassis_power_limit
+ // *
+ // 1.8 *
+ // this->cap_.percentage_;
+
+ // float max_power_limit = ref_.chassis_power_limit;
+
// sum_6020_out_ =
// Calculate6020Power(out_.motor6020_out, motor_feedback_.pos_speed, 4);
for (int i = 0; i < 4; i++) {
if (cap_.online_) {
- LimitChassisOutPower(max_power_limit, out_.speed_motor_out,
- speed_motor_feedback_, 4);
+ LimitChassisOutPower(180.0f, out_.speed_motor_out, speed_motor_feedback_,
+ 4);
this->speed_motor_[i]->Control(out_.speed_motor_out[i]);
this->pos_motor_[i]->Control(out_.motor6020_out[i]);
} else {
- LimitChassisOutPower(max_power_limit, out_.speed_motor_out,
- speed_motor_feedback_, 4);
+ LimitChassisOutPower(500.f, out_.speed_motor_out, speed_motor_feedback_,
+ 4);
this->speed_motor_[i]->Control(out_.speed_motor_out[i]);
this->pos_motor_[i]->Control(out_.motor6020_out[i]);
}
diff --git a/src/module/helm_chassis/mod_helm_chassis.hpp b/src/module/helm_chassis/mod_helm_chassis.hpp
index b120d573..cf494311 100644
--- a/src/module/helm_chassis/mod_helm_chassis.hpp
+++ b/src/module/helm_chassis/mod_helm_chassis.hpp
@@ -16,7 +16,9 @@
#include <array>
#include <cmath>
#include <comp_type.hpp>
+#include <cstdlib>
#include <module.hpp>
+#include <vector>
#include "comp_actuator.hpp"
#include "comp_cmd.hpp"
@@ -27,7 +29,7 @@
#include "dev_motor.hpp"
#include "dev_referee.hpp"
#include "dev_rm_motor.hpp"
-#define PI (M_2PI)
+#define PI_ (M_2PI)
namespace Module {
template <typename Motor, typename MotorParam>
@@ -99,6 +101,12 @@ class HelmChassis {
float CalcWz(const float LO, const float HI);
+ int Getrandom(); //随机数
+
+ double GetTorqueLength(float angle_a, double angle_b, int selection);
+
+ float GetRelateAngle(); //相对偏航角
+
private:
Param param_;
@@ -108,17 +116,38 @@ class HelmChassis {
uint64_t now_ = 0;
float yaw_;
-
+ float pit_;
+ double alpha_;
+ float pos_motor_value_[4];
+ float speed_motor_value_[4];
+ float pos_err_value_[4];
+ float wz_test_;
float pos_motor_sum_out_;
+ float eulr_yaw1_;
+ int random_;
+ float deviation_angle_value_;
+ float test_angle01_;
+ float test_angle02_;
+ double tan_pit_;
+ std::vector<std::string> string_vector1_{
+ "Chassis_pos_RightFront", "Chassis_pos_LeftFront", "Chassis_pos_LeftBack",
+ "Chassis_pos_RightBack"};
+
+ std::vector<std::string> string_vector2_{
+ "Chassis_speed_RightFront", "Chassis_speed_LeftFront",
+ "Chassis_speed_LeftBack", "Chassis_speed_RightBack"};
float max_motor_rotational_speed_;
+
Device::Cap::Info cap_;
Device::Referee::Data raw_ref_;
float speed_motor_feedback_[4];
float pos_motor_feedback_[4];
-
+ float forward_coefficient_[4];
+ float l_data_[4];
+ float power_;
RefForChassis ref_;
Mode mode_ = RELAX;
@@ -150,6 +179,8 @@ class HelmChassis {
float direct_offset_ = 0.0f;
+ float state_num_;
+
Component::PID follow_pid_;
System::Thread thread_;
@@ -167,6 +198,10 @@ class HelmChassis {
std::array<float, 4> speed_motor_out_;
std::array<float, 4> pos_motor_out_;
+
+ Component::Type::Eulr eulr_;
+ Component::Type::Quaternion quat_;
+ Component::Type::Vector3 gyro_;
};
typedef HelmChassis<Device::RMMotor, Device::RMMotor::Param> RMHelmChassis;
diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp
index 7df68f5d..c38544a4 100644
--- a/src/module/launcher/mod_launcher.cpp
+++ b/src/module/launcher/mod_launcher.cpp
@@ -3,28 +3,28 @@
#include "bsp_pwm.h"
#include "bsp_time.h"
-#define LAUNCHER_TRIG_SPEED_MAX (8191)
+#define LAUNCHER_TRIG_SPEED_MAX (16000)
using namespace Module;
-Launcher::Launcher(Param& param, float control_freq)
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+Launcher<Motor, Motorparam, Fric_num, Trig_num>::Launcher(Param& param,
+ float control_freq)
: param_(param), ctrl_lock_(true) {
- for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+ for (size_t i = 0; i < Trig_num; i++) {
this->trig_actuator_.at(i) =
new Component::PosActuator(param.trig_actr.at(i), control_freq);
- this->trig_motor_.at(i) =
- new Device::RMMotor(this->param_.trig_motor.at(i),
- ("Launcher_Trig" + std::to_string(i)).c_str());
+ this->trig_motor_.at(i) = new Motor(
+ param.trig_param.at(i), ("Launcher_Trig" + std::to_string(i)).c_str());
}
- for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+ for (size_t i = 0; i < Fric_num; i++) {
this->fric_actuator_.at(i) =
new Component::SpeedActuator(param.fric_actr.at(i), control_freq);
- this->fric_motor_.at(i) =
- new Device::RMMotor(this->param_.fric_motor.at(i),
- ("Launcher_Fric" + std::to_string(i)).c_str());
+ this->fric_motor_.at(i) = new Motor(
+ param.fric_param.at(i), ("Launcher_Fric" + std::to_string(i)).c_str());
}
auto event_callback = [](LauncherEvent event, Launcher* launcher) {
@@ -76,8 +76,8 @@ Launcher::Launcher(Param& param, float control_freq)
Component::CMD::RegisterEvent<Launcher*, LauncherEvent>(
event_callback, this, this->param_.EVENT_MAP);
- bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
- bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty);
+ // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
+ // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty);
auto launcher_thread = [](Launcher* launcher) {
auto ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
@@ -109,15 +109,16 @@ Launcher::Launcher(Param& param, float control_freq)
System::Timer::Create(this->DrawUIDynamic, this, 100);
}
-void Launcher::UpdateFeedback() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::UpdateFeedback() {
const float LAST_TRIG_MOTOR_ANGLE = this->trig_motor_[0]->GetAngle();
- for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+ for (size_t i = 0; i < Fric_num; i++) {
this->fric_motor_[i]->Update();
speed[i] = fric_motor_[i]->GetSpeed();
}
- for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+ for (size_t i = 0; i < Trig_num; i++) {
this->trig_motor_[i]->Update();
}
@@ -126,7 +127,8 @@ void Launcher::UpdateFeedback() {
this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio;
}
-void Launcher::Control() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::Control() {
this->now_ = bsp_time_get();
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
@@ -236,19 +238,21 @@ void Launcher::Control() {
/* 计算摩擦轮和拨弹盘并输出 */
switch (this->fire_ctrl_.fire_mode_) {
case RELAX:
- for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+ for (size_t i = 0; i < Trig_num; i++) {
this->trig_motor_[i]->Relax();
}
- for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+ for (size_t i = 0; i < Fric_num; i++) {
this->fric_motor_[i]->Relax();
}
- bsp_pwm_stop(BSP_PWM_LAUNCHER_SERVO);
+ // sp_pwm_stop(BSP_PWM_LAUNCHER_SERVO);
break;
case SAFE:
case LOADED:
- for (int i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+ for (size_t i = 0; i < Trig_num; i++) {
/* 控制拨弹电机 */
+ float trig_speed = this->trig_motor_[i]->GetSpeed();
+ clampf(&trig_speed, 0.f, LAUNCHER_TRIG_SPEED_MAX);
float trig_out = this->trig_actuator_[i]->Calculate(
this->setpoint_.trig_angle_,
this->trig_motor_[i]->GetSpeed() / LAUNCHER_TRIG_SPEED_MAX,
@@ -257,7 +261,7 @@ void Launcher::Control() {
this->trig_motor_[i]->Control(trig_out);
}
- for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+ for (size_t i = 0; i < Fric_num; i++) {
/* 控制摩擦轮 */
float fric_out = this->fric_actuator_[i]->Calculate(
this->setpoint_.fric_rpm_[i], this->fric_motor_[i]->GetSpeed(),
@@ -267,19 +271,23 @@ void Launcher::Control() {
}
/* 根据弹仓盖开关状态更新弹舱盖打开时舵机PWM占空比 */
- if (this->cover_mode_ == OPEN) {
- bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
- bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_open_duty);
- } else {
- bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
- bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty);
- }
+ // if (this->cover_mode_ == OPEN) {
+ // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
+ // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO,
+ // this->param_.cover_open_duty);
+ // } else {
+ // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
+ // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO,
+ // this->param_.cover_close_duty);
+ // }
break;
}
}
/* 拨弹盘模式 */
/* SINGLE,BURST,CONTINUED, */
-void Launcher::SetTrigMode(TrigMode mode) {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::SetTrigMode(
+ TrigMode mode) {
if (mode == this->fire_ctrl_.trig_mode_) {
return;
}
@@ -288,14 +296,16 @@ void Launcher::SetTrigMode(TrigMode mode) {
}
/* 设置摩擦轮模式 */
/* RELEX SAFE LOADED三种模式可以选择 */
-void Launcher::SetFireMode(FireMode mode) {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::SetFireMode(
+ FireMode mode) {
if (mode == this->fire_ctrl_.fire_mode_) { /* 未更改，return */
return;
}
fire_ctrl_.fire = false;
- for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+ for (size_t i = 0; i < Fric_num; i++) {
this->fric_actuator_[i]->Reset();
} /* reset 所有电机执行器PID等参数 */
@@ -306,7 +316,8 @@ void Launcher::SetFireMode(FireMode mode) {
this->fire_ctrl_.fire_mode_ = mode;
}
-void Launcher::HeatLimit() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::HeatLimit() {
if (this->ref_.status == Device::Referee::RUNNING) {
/* 根据机器人型号获得对应数据 */
if (this->param_.model == LAUNCHER_MODEL_42MM) {
@@ -340,7 +351,8 @@ void Launcher::HeatLimit() {
}
}
-void Launcher::PraseRef() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::PraseRef() {
memcpy(&(this->ref_.power_heat), &(this->raw_ref_.power_heat),
sizeof(this->ref_.power_heat));
memcpy(&(this->ref_.robot_status), &(this->raw_ref_.robot_status),
@@ -350,29 +362,51 @@ void Launcher::PraseRef() {
this->ref_.status = this->raw_ref_.status;
}
-float Launcher::LimitLauncherFreq() { /* 热量限制计算 */
+// float Launcher::LimitLauncherFreq() { /* 热量限制计算 */
+// float heat_percent = this->heat_ctrl_.heat / this->heat_ctrl_.heat_limit;
+// float stable_freq = this->heat_ctrl_.cooling_rate /
+// this->heat_ctrl_.heat_increase; /* 每秒可发弹量 */
+// if (this->param_.model == LAUNCHER_MODEL_42MM) {
+// return stable_freq;
+// } else {
+// if (heat_percent > 0.9f) {
+// return 0.5f;
+// } else if (heat_percent > 0.8f) {
+// return 1.0f;
+// } else if (heat_percent > 0.6f) {
+// return 2.0f * stable_freq;
+// } else if (heat_percent > 0.2f) {
+// return 3.0f * stable_freq;
+// } else if (heat_percent > 0.1f) {
+// return 4.0f * stable_freq;
+// } else {
+// return 5.0f;
+// }
+// }
+// }
+
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+float Launcher<Motor, Motorparam, Fric_num,
+ Trig_num>::LimitLauncherFreq() { /* 热量限制计算 */
float heat_percent = this->heat_ctrl_.heat / this->heat_ctrl_.heat_limit;
float stable_freq = this->heat_ctrl_.cooling_rate /
this->heat_ctrl_.heat_increase; /* 每秒可发弹量 */
- if (this->param_.model == LAUNCHER_MODEL_42MM) {
- return stable_freq;
+ if (heat_percent > 0.9f) {
+ return 0.5f;
+ } else if (heat_percent > 0.85f) {
+ return stable_freq * 0.6f;
+ } else if (heat_percent > 0.8f) {
+ return 0.6f * 1000 / this->param_.min_launch_delay;
+ } else if (heat_percent > 0.6f) {
+ return 0.8f * 1000 / this->param_.min_launch_delay;
} else {
- if (heat_percent > 0.9f) {
- return 0.5f;
- } else if (heat_percent > 0.8f) {
- return 1.0f;
- } else if (heat_percent > 0.6f) {
- return 2.0f * stable_freq;
- } else if (heat_percent > 0.2f) {
- return 3.0f * stable_freq;
- } else if (heat_percent > 0.1f) {
- return 4.0f * stable_freq;
- } else {
- return 5.0f;
- }
+ return 1000 / this->param_.min_launch_delay;
}
}
-void Launcher::DrawUIStatic(Launcher* launcher) {
+
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::DrawUIStatic(
+ Launcher* launcher) {
launcher->string_.Draw("SM", Component::UI::UI_GRAPHIC_OP_ADD,
Component::UI::UI_GRAPHIC_LAYER_CONST,
Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80,
@@ -511,7 +545,9 @@ void Launcher::DrawUIStatic(Launcher* launcher) {
Device::Referee::AddUI(launcher->arc_);
}
-void Launcher::DrawUIDynamic(Launcher* launcher) {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::DrawUIDynamic(
+ Launcher* launcher) {
float box_pos_left = 0.0f, box_pos_right = 0.0f;
/* 更新发射器模式选择框 */
@@ -655,3 +691,5 @@ void Launcher::DrawUIDynamic(Launcher* launcher) {
40, 40);
Device::Referee::AddUI(launcher->arc_);
}
+
+template class Module::Launcher<Device::RMMotor, Device::RMMotor::Param>;
diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp
index b569c249..138528ac 100644
--- a/src/module/launcher/mod_launcher.hpp
+++ b/src/module/launcher/mod_launcher.hpp
@@ -10,6 +10,8 @@
#include "dev_rm_motor.hpp"
namespace Module {
+template <typename Motor, typename Motorparam, int Fric_num = 2,
+ int Trig_num = 1>
class Launcher {
public:
/* 发射器运行模式 */
@@ -44,24 +46,6 @@ class Launcher {
CLOSE_COVER,
} LauncherEvent;
- enum {
- LAUNCHER_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
- LAUNCHER_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
- LAUNCHER_ACTR_FRIC_NUM, /* 总共的动作器数量 */
- };
-
- enum {
- LAUNCHER_ACTR_TRIG_IDX, /* 拨弹电机相关的索引值 */
- LAUNCHER_ACTR_TRIG_NUM, /* 总共的动作器数量 */
- };
-
- enum {
- LAUNCHER_CTRL_FRIC1_SPEED_IDX = 0, /* 摩擦轮1控制的速度环控制器的索引值 */
- LAUNCHER_CTRL_FRIC2_SPEED_IDX, /* 摩擦轮2控制的速度环控制器的索引值 */
- LAUNCHER_CTRL_TRIG_SPEED_IDX, /* 拨弹电机控制的速度环控制器的索引值 */
- LAUNCHER_CTRL_NUM, /* 总共的控制器数量 */
- };
-
typedef enum {
LAUNCHER_MODEL_17MM = 0, /* 17mm发射机构 */
LAUNCHER_MODEL_42MM, /* 42mm发射机构 */
@@ -77,11 +61,10 @@ class Launcher {
float default_bullet_speed; /* 默认弹丸初速度 */
uint32_t min_launch_delay; /* 最小发射间隔(1s/最大射频) */
- std::array<Component::PosActuator::Param, LAUNCHER_ACTR_TRIG_NUM> trig_actr;
- std::array<Component::SpeedActuator::Param, LAUNCHER_ACTR_FRIC_NUM>
- fric_actr;
- std::array<Device::RMMotor::Param, LAUNCHER_ACTR_TRIG_NUM> trig_motor;
- std::array<Device::RMMotor::Param, LAUNCHER_ACTR_FRIC_NUM> fric_motor;
+ std::array<Component::PosActuator::Param, Trig_num> trig_actr;
+ std::array<Component::SpeedActuator::Param, Fric_num> fric_actr;
+ std::array<Motorparam, Trig_num> trig_param;
+ std::array<Motorparam, Fric_num> fric_param;
const std::vector<Component::CMD::EventMapItem> EVENT_MAP;
} Param;
@@ -165,11 +148,11 @@ class Launcher {
HeatControl heat_ctrl_;
FireControl fire_ctrl_;
- std::array<Component::PosActuator *, LAUNCHER_ACTR_TRIG_NUM> trig_actuator_;
- std::array<Component::SpeedActuator *, LAUNCHER_ACTR_FRIC_NUM> fric_actuator_;
+ std::array<Component::PosActuator *, Trig_num> trig_actuator_;
+ std::array<Component::SpeedActuator *, Fric_num> fric_actuator_;
- std::array<Device::RMMotor *, LAUNCHER_ACTR_TRIG_NUM> trig_motor_;
- std::array<Device::RMMotor *, LAUNCHER_ACTR_FRIC_NUM> fric_motor_;
+ std::array<Device::RMMotor *, Trig_num> trig_motor_;
+ std::array<Device::RMMotor *, Fric_num> fric_motor_;
RefForLauncher ref_;
@@ -185,4 +168,5 @@ class Launcher {
Component::UI::Arc arc_;
};
+typedef class Launcher<Device::RMMotor, Device::RMMotor::Param> RMLauncher;
} // namespace Module
diff --git a/src/module/omni_chassis/mod_omni_chassis.cpp b/src/module/omni_chassis/mod_omni_chassis.cpp
index 6f11c638..1fd94de4 100644
--- a/src/module/omni_chassis/mod_omni_chassis.cpp
+++ b/src/module/omni_chassis/mod_omni_chassis.cpp
@@ -17,12 +17,13 @@
#include "bsp_time.h"
-#define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */
-#define ROTOR_WZ_MAX 1.0f /* 小陀螺旋转位移上界 */
+#define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
+#define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
#define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */
#define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */
+#define MOTOR_MAX_ROTATIONAL_SPEED 9600 /* 电机最大转速 */
#if POWER_LIMIT_WITH_CAP
/* 保证电容电量宏定义在正确范围内 */
@@ -44,11 +45,13 @@ static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f;
using namespace Module;
template <typename Motor, typename MotorParam>
-OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
+nOmniChassis<Motor, MotorParam>::nOmniChassis(Param& param, float control_freq)
: param_(param),
- mode_(OmniChassis::RELAX),
+ mode_(nOmniChassis::RELAX),
mixer_(param.type),
follow_pid_(param.follow_pid_param, control_freq),
+ xaccl_pid_(param.xaccl_pid_param, control_freq),
+ yaccl_pid_(param.yaccl_pid_param, control_freq),
ctrl_lock_(true) {
memset(&(this->cmd_), 0, sizeof(this->cmd_));
@@ -61,12 +64,9 @@ OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
(std::string("Chassis_") + std::to_string(i)).c_str());
}
- this->setpoint_.motor_rotational_speed =
- reinterpret_cast<float*>(System::Memory::Malloc(
- this->mixer_.len_ * sizeof(*this->setpoint_.motor_rotational_speed)));
XB_ASSERT(this->setpoint_.motor_rotational_speed);
- auto event_callback = [](ChassisEvent event, OmniChassis* chassis) {
+ auto event_callback = [](ChassisEvent event, nOmniChassis* chassis) {
chassis->ctrl_lock_.Wait(UINT32_MAX);
switch (event) {
@@ -94,10 +94,10 @@ OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
chassis->ctrl_lock_.Post();
};
- Component::CMD::RegisterEvent<OmniChassis*, ChassisEvent>(
+ Component::CMD::RegisterEvent<nOmniChassis*, ChassisEvent>(
event_callback, this, this->param_.EVENT_MAP);
- auto chassis_thread = [](OmniChassis* chassis) {
+ auto chassis_thread = [](nOmniChassis* chassis) {
auto raw_ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
auto cmd_sub =
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
@@ -137,36 +137,26 @@ OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
}
template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::UpdateFeedback() {
+void nOmniChassis<Motor, MotorParam>::UpdateFeedback() {
/* 将CAN中的反馈数据写入到feedback中 */
for (size_t i = 0; i < this->mixer_.len_; i++) {
this->motor_[i]->Update();
- this->motor_feedback_.motor_speed[i] = this->motor_[i]->GetSpeed();
+ this->motor_speed_[i] = this->motor_[i]->GetSpeed();
}
}
template <typename Motor, typename MotorParam>
-uint16_t OmniChassis<Motor, MotorParam>::MAXSPEEDGET(float power_limit) {
- uint16_t speed = 0.0f;
- if (power_limit <= 50.0f) {
- speed = 5000;
- } else if (power_limit <= 60.0f) {
- speed = 5500;
- } else if (power_limit <= 70.0f) {
- speed = 5500;
- } else if (power_limit <= 80.0f) {
- speed = 6500;
- } else if (power_limit <= 100.0f) {
- speed = 7000;
+uint16_t nOmniChassis<Motor, MotorParam>::MAXSPEEDGET(float power_limit) {
+ if (param_.get_speed) {
+ return param_.get_speed(power_limit);
} else {
- speed = 7500;
+ return 5500;
}
- return speed;
}
template <typename Motor, typename MotorParam>
-bool OmniChassis<Motor, MotorParam>::LimitChassisOutput(float power_limit,
- float* motor_out,
- float* speed_rpm,
- uint32_t len) {
+bool nOmniChassis<Motor, MotorParam>::LimitChassisOutput(float power_limit,
+ float* motor_out,
+ float* speed_rpm,
+ uint32_t len) {
if (power_limit < 0.0f) {
return 0;
}
@@ -209,34 +199,22 @@ bool OmniChassis<Motor, MotorParam>::LimitChassisOutput(float power_limit,
return 0;
}
template <typename Motor, typename MotorParam>
-bool OmniChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
- float* motor_out,
- float* speed_rpm,
- uint32_t len) {
+bool nOmniChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
+ float* motor_out,
+ float* speed_rpm,
+ uint32_t len) {
if (power_limit < 0.0f) {
return 0;
}
- // float sum_motor_out = 0.0f;
- // for (size_t i = 0; i < len; i++) {
- // sum_motor_out += fabsf(motor_out[i]) * fabsf(speed[i]) * 0.06f;
- // }
- // sum_motor_out += 9.2326f;
- // power_1_ = sum_motor_out;
- // if (sum_motor_out > power_limit) {
- // for (size_t i = 0; i < len; i++) {
- // motor_out[i] *= power_limit / sum_motor_out;
- // }
- // }
- // return true;
float sum_motor_power = 0.0f;
- float motor_3508_power[len];
+ float motor_power[4];
for (size_t i = 0; i < len; i++) {
- motor_3508_power[i] =
+ motor_power[i] =
this->param_.toque_coefficient_ * fabsf(motor_out[i]) *
fabsf(speed_rpm[i]) +
this->param_.speed_2_coefficient_ * speed_rpm[i] * speed_rpm[i] +
this->param_.out_2_coefficient_ * motor_out[i] * motor_out[i];
- sum_motor_power += motor_3508_power[i];
+ sum_motor_power += motor_power[i];
}
sum_motor_power += this->param_.constant_;
power_ = sum_motor_power;
@@ -249,7 +227,7 @@ bool OmniChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
return true;
}
template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::Control() {
+void nOmniChassis<Motor, MotorParam>::Control() {
this->now_ = bsp_time_get();
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
@@ -260,27 +238,58 @@ void OmniChassis<Motor, MotorParam>::Control() {
/* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */
/* 计算vx、vy */
switch (this->mode_) {
- case OmniChassis::BREAK: /* 刹车模式电机停止 */
+ case nOmniChassis::BREAK: /* 刹车模式电机停止 */
this->move_vec_.vx = 0.0f;
this->move_vec_.vy = 0.0f;
break;
- case OmniChassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等
- */
+ case nOmniChassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等
+ */
this->move_vec_.vx = this->cmd_.x;
this->move_vec_.vy = this->cmd_.y;
break;
- case OmniChassis::RELAX:
- case OmniChassis::FOLLOW_GIMBAL_INTERSECT: /* 按照云台方向换算运动向量
- */
- case OmniChassis::FOLLOW_GIMBAL_CROSS:
- case OmniChassis::ROTOR: {
+ case nOmniChassis::RELAX:
+ case nOmniChassis::FOLLOW_GIMBAL_INTERSECT: /* 按照云台方向换算运动向量 */
+ case nOmniChassis::FOLLOW_GIMBAL_CROSS: {
float beta = this->yaw_;
float cos_beta = cosf(beta);
float sin_beta = sinf(beta);
+ /*控制加速度*/
+ this->move_vec_.vx = this->xaccl_pid_.Calculate(
+ (cos_beta * this->cmd_.x - sin_beta * this->cmd_.y),
+ this->move_vec_.vx, dt_);
+ if (!cmd_.x) {
+ this->xaccl_pid_.Reset();
+ }
+ this->move_vec_.vy = this->yaccl_pid_.Calculate(
+ (sin_beta * this->cmd_.x + cos_beta * this->cmd_.y),
+ this->move_vec_.vy, dt_);
+ if (!cmd_.y) {
+ this->yaccl_pid_.Reset();
+ }
+ float scalar_sum = fabs(this->move_vec_.vx) + fabs(this->move_vec_.vy);
+ if (scalar_sum > 1.01f) {
+ this->move_vec_.vx = this->move_vec_.vx / scalar_sum;
+ this->move_vec_.vy = this->move_vec_.vy / scalar_sum;
+ }
+ } break;
+ case nOmniChassis::ROTOR: {
+ float beta = this->yaw_ - M_PI / 8.0f;
+ // if (this->move_vec_.wz == 1) {
+ // beta = this->yaw_ - M_PI_4;
+ // } else {
+ // beta = this->yaw_;
+ // }
+ float cos_beta = cosf(beta);
+ float sin_beta = sinf(beta);
this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
+ float scalar_sum = fabs(this->move_vec_.vx) + fabs(this->move_vec_.vy);
+ if (scalar_sum > 1.01f) {
+ this->move_vec_.vx = this->move_vec_.vx / scalar_sum;
+ this->move_vec_.vy = this->move_vec_.vy / scalar_sum;
+ }
break;
}
default:
@@ -289,25 +298,74 @@ void OmniChassis<Motor, MotorParam>::Control() {
/* 计算wz */
switch (this->mode_) {
- case OmniChassis::RELAX:
- case OmniChassis::BREAK:
- case OmniChassis::INDENPENDENT: /* 独立模式wz为0 */
+ case nOmniChassis::RELAX:
+ case nOmniChassis::BREAK:
+ case nOmniChassis::INDENPENDENT: /* 独立模式wz为0 */
this->move_vec_.wz = this->cmd_.z;
break;
- case OmniChassis::FOLLOW_GIMBAL_INTERSECT:
+ case nOmniChassis::FOLLOW_GIMBAL_INTERSECT:
+ //{
+ // float direction = 0.0f;
+ // /*双零点*/
+ // if (this->yaw_ > M_PI_2) {
+ // direction = 3.14158f;
+ // }
+ // if (this->yaw_ < -M_PI_2) {
+ // direction = 3.14158f;
+ // }
+ // this->move_vec_.wz =
+ // this->follow_pid_.Calculate(direction, this->yaw_, this->dt_);
+ // clampf(&this->move_vec_.wz, -1.0f, 1.0f);
+ // float move_scal_sum = fabs(this->move_vec_.vx) +
+ // fabs(this->move_vec_.vy) +
+ // fabs(this->move_vec_.wz);
+ // if (move_scal_sum > 1.01f) {
+ // this->move_vec_.vx =
+ // this->move_vec_.vx * (1 - fabs(this->move_vec_.wz));
+ // this->move_vec_.vy =
+ // this->move_vec_.vy * (1 - fabs(this->move_vec_.wz));
+ // }
+ // } break;
this->move_vec_.wz =
this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_);
break;
- case OmniChassis::FOLLOW_GIMBAL_CROSS:
+ case nOmniChassis::FOLLOW_GIMBAL_CROSS: {
+ // float direction = 0.0f;
+ // /*双零点*/
+ // if (this->yaw_ > M_PI_2) {
+ // direction = 3.14158f;
+ // }
+ // if (this->yaw_ < -M_PI_2) {
+ // direction = 3.14158f;
+ // }
+ // this->move_vec_.wz = this->follow_pid_.Calculate(
+ // direction, this->yaw_ - M_PI / 4.0f, this->dt_);
+ // clampf(&this->move_vec_.wz, -1.0f, 1.0f);
+ // float move_scal_sum = fabs(this->move_vec_.vx) +
+ // fabs(this->move_vec_.vy) +
+ // fabs(this->move_vec_.wz);
+ // if (move_scal_sum > 1.01f) {
+ // this->move_vec_.vx =
+ // this->move_vec_.vx * (1 - fabs(this->move_vec_.wz));
+ // this->move_vec_.vy =
+ // this->move_vec_.vy * (1 - fabs(this->move_vec_.wz));
+ // }
this->move_vec_.wz = this->follow_pid_.Calculate(
0.0f, this->yaw_ - M_PI / 4.0f, this->dt_);
- break;
-
- case OmniChassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
- */
- this->move_vec_.wz =
- this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
+ } break;
+
+ case nOmniChassis::ROTOR: /* 小陀螺模式使底盘以一定速度旋转 */
+ { /* TODO 改成实际底盘速度 */
+ this->move_vec_.wz = 1;
+ // this->move_vec_.wz = this->wz_dir_mult_;
+ float move_scal_sum = fabs(this->move_vec_.vx) +
+ fabs(this->move_vec_.vy) + fabs(this->move_vec_.wz);
+ if (move_scal_sum > 1.01f) {
+ this->move_vec_.wz = this->move_vec_.wz / move_scal_sum;
+ this->move_vec_.vx = this->move_vec_.vx / move_scal_sum;
+ this->move_vec_.vy = this->move_vec_.vy / move_scal_sum;
+ }
break;
}
default:
@@ -322,11 +380,11 @@ void OmniChassis<Motor, MotorParam>::Control() {
/* 根据底盘模式计算输出值 */
switch (this->mode_) {
- case OmniChassis::BREAK:
- case OmniChassis::FOLLOW_GIMBAL_INTERSECT:
- case OmniChassis::FOLLOW_GIMBAL_CROSS:
- case OmniChassis::ROTOR:
- case OmniChassis::INDENPENDENT: /* 独立模式,受PID控制 */ {
+ case nOmniChassis::BREAK:
+ case nOmniChassis::FOLLOW_GIMBAL_INTERSECT:
+ case nOmniChassis::FOLLOW_GIMBAL_CROSS:
+ case nOmniChassis::ROTOR:
+ case nOmniChassis::INDENPENDENT: /* 独立模式,受PID控制 */ {
float percentage = 0.0f;
if (ref_.status == Device::Referee::RUNNING) {
if (ref_.chassis_pwr_buff > 30) {
@@ -337,31 +395,40 @@ void OmniChassis<Motor, MotorParam>::Control() {
} else {
percentage = 1.0f;
}
+
clampf(&percentage, 0.0f, 1.0f);
- float max_power_limit =
- ref_.chassis_power_limit +
- ref_.chassis_power_limit * 0.2 * this->cap_.percentage_;
+
for (unsigned i = 0; i < this->mixer_.len_; i++) {
out_.motor3508_out[i] = this->actuator_[i]->Calculate(
this->setpoint_.motor_rotational_speed[i] *
- max_motor_rotational_speed_,
+ MOTOR_MAX_ROTATIONAL_SPEED,
this->motor_[i]->GetSpeed(), this->dt_);
}
+
+ if (this->cmd_.z > 0.5) {
+ this->max_power_limit = 120;
+ } else {
+ this->max_power_limit = ref_.chassis_power_limit;
+ }
+
for (unsigned i = 0; i < this->mixer_.len_; i++) {
if (cap_.online_) {
- LimitChassisOutPower(max_power_limit, out_.motor3508_out,
- motor_feedback_.motor_speed, this->mixer_.len_);
+ // LimitChassisOutPower(this->max_power_limit, out_.motor3508_out,
+ // motor_speed_, this->mixer_.len_);
+ LimitChassisOutPower(180, out_.motor3508_out, motor_speed_,
+ this->mixer_.len_);
this->motor_[i]->Control(out_.motor3508_out[i]);
} else {
+ /* 不限功率的兵种使用该底盘时
+ * 注意更改chassis_power_limit至电池安全功率 */
LimitChassisOutPower(ref_.chassis_power_limit, out_.motor3508_out,
- motor_feedback_.motor_speed, this->mixer_.len_);
-
+ motor_speed_, this->mixer_.len_);
this->motor_[i]->Control(out_.motor3508_out[i]);
}
}
break;
}
- case OmniChassis::RELAX: /* 放松模式,不输出 */
+ case nOmniChassis::RELAX: /* 放松模式,不输出 */
for (size_t i = 0; i < this->mixer_.len_; i++) {
this->motor_[i]->Relax();
}
@@ -373,37 +440,38 @@ void OmniChassis<Motor, MotorParam>::Control() {
}
template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::PraseRef() {
+void nOmniChassis<Motor, MotorParam>::PraseRef() {
this->ref_.chassis_power_limit =
this->raw_ref_.robot_status.chassis_power_limit;
this->ref_.chassis_pwr_buff = this->raw_ref_.power_heat.chassis_pwr_buff;
- this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
+ // this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
this->ref_.status = this->raw_ref_.status;
}
+/* 随机转速小陀螺 结合自身超电与敌方视觉水平使用 */
template <typename Motor, typename MotorParam>
-float OmniChassis<Motor, MotorParam>::CalcWz(const float LO, const float HI) {
+float nOmniChassis<Motor, MotorParam>::CalcWz(const float LO, const float HI) {
float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO;
clampf(&wz_vary, LO, HI);
return wz_vary;
}
template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::SetMode(OmniChassis::Mode mode) {
+void nOmniChassis<Motor, MotorParam>::SetMode(nOmniChassis::Mode mode) {
if (mode == this->mode_) {
return; /* 模式未改变直接返回 */
}
- if (mode == OmniChassis::ROTOR && this->mode_ != OmniChassis::ROTOR) {
+ if (mode == nOmniChassis::ROTOR && this->mode_ != nOmniChassis::ROTOR) {
std::srand(this->now_);
this->wz_dir_mult_ = (std::rand() % 2) ? -1 : 1;
}
- if (mode == OmniChassis::FOLLOW_GIMBAL_CROSS &&
- this->mode_ != OmniChassis::FOLLOW_GIMBAL_CROSS) {
+ if (mode == nOmniChassis::FOLLOW_GIMBAL_CROSS &&
+ this->mode_ != nOmniChassis::FOLLOW_GIMBAL_CROSS) {
this->param_.type = Component::Mixer::OMNICROSS;
}
- if (mode == OmniChassis::FOLLOW_GIMBAL_INTERSECT &&
- this->mode_ != OmniChassis::FOLLOW_GIMBAL_INTERSECT) {
+ if (mode == nOmniChassis::FOLLOW_GIMBAL_INTERSECT &&
+ this->mode_ != nOmniChassis::FOLLOW_GIMBAL_INTERSECT) {
this->param_.type = Component::Mixer::OMNIPLUS;
}
/* 切换模式后重置PID和滤波器 */
@@ -413,9 +481,10 @@ void OmniChassis<Motor, MotorParam>::SetMode(OmniChassis::Mode mode) {
this->mode_ = mode;
}
+/*慢速刷新*/
template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::DrawUIStatic(
- OmniChassis<Motor, MotorParam>* chassis) {
+void nOmniChassis<Motor, MotorParam>::DrawUIStatic(
+ nOmniChassis<Motor, MotorParam>* chassis) {
chassis->string_.Draw("CM", Component::UI::UI_GRAPHIC_OP_ADD,
Component::UI::UI_GRAPHIC_LAYER_CONST,
Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80,
@@ -474,8 +543,8 @@ void OmniChassis<Motor, MotorParam>::DrawUIStatic(
}
template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::DrawUIDynamic(
- OmniChassis<Motor, MotorParam>* chassis) {
+void nOmniChassis<Motor, MotorParam>::DrawUIDynamic(
+ nOmniChassis<Motor, MotorParam>* chassis) {
float box_pos_left = 0.0f, box_pos_right = 0.0f;
/* 更新底盘模式选择框 */
@@ -519,4 +588,4 @@ void OmniChassis<Motor, MotorParam>::DrawUIDynamic(
Device::Referee::AddUI(chassis->rectange_);
}
}
-template class Module::OmniChassis<Device::RMMotor, Device::RMMotor::Param>;
+template class Module::nOmniChassis<Device::RMMotor, Device::RMMotor::Param>;
diff --git a/src/module/omni_chassis/mod_omni_chassis.hpp b/src/module/omni_chassis/mod_omni_chassis.hpp
index 38ded65e..81f7a87d 100644
--- a/src/module/omni_chassis/mod_omni_chassis.hpp
+++ b/src/module/omni_chassis/mod_omni_chassis.hpp
@@ -24,7 +24,7 @@
#include "dev_rm_motor.hpp"
namespace Module {
template <typename Motor, typename MotorParam>
-class OmniChassis {
+class nOmniChassis {
public:
/* 底盘运行模式 */
typedef enum {
@@ -36,12 +36,19 @@ class OmniChassis {
INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
} Mode;
+ typedef enum {
+ COMMON,
+ BEAST,
+ } Power_Mode;
+
typedef enum {
SET_MODE_RELAX,
SET_MODE_INTERSECT,
SET_MODE_CROSS,
SET_MODE_ROTOR,
SET_MODE_INDENPENDENT,
+ CHANGE_POWER_UP,
+ CHANGE_POWER_DOWN,
} ChassisEvent;
/* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组
*/
@@ -54,13 +61,15 @@ class OmniChassis {
Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */
Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */
+ Component::PID::Param xaccl_pid_param{}; /* 加速跟随PID的参数 */
+ Component::PID::Param yaccl_pid_param{}; /* y方向加速跟随PID */
const std::vector<Component::CMD::EventMapItem> EVENT_MAP;
std::array<Component::SpeedActuator::Param, 4> actuator_param{};
std::array<MotorParam, 4> motor_param;
-
+ float (*get_speed)(float);
} Param;
typedef struct {
@@ -70,7 +79,7 @@ class OmniChassis {
float chassis_watt;
} RefForChassis;
- OmniChassis(Param ¶m, float control_freq);
+ nOmniChassis(Param ¶m, float control_freq);
void UpdateFeedback();
@@ -78,6 +87,8 @@ class OmniChassis {
void SetMode(Mode mode);
+ void ChangePowerlim(Power_Mode power_mode_);
+
bool LimitChassisOutPower(float power_limit, float *motor_out, float *speed,
uint32_t len);
bool LimitChassisOutput(float power_limit, float *motor_out, float *speed,
@@ -85,9 +96,9 @@ class OmniChassis {
void PraseRef();
uint16_t MAXSPEEDGET(float power_limit);
- static void DrawUIStatic(OmniChassis<Motor, MotorParam> *chassis);
+ static void DrawUIStatic(nOmniChassis<Motor, MotorParam> *chassis);
- static void DrawUIDynamic(OmniChassis<Motor, MotorParam> *chassis);
+ static void DrawUIDynamic(nOmniChassis<Motor, MotorParam> *chassis);
float CalcWz(const float LO, const float HI);
@@ -109,6 +120,8 @@ class OmniChassis {
RefForChassis ref_;
Mode mode_ = RELAX;
+ Mode last_mode_ = mode_;
+ Power_Mode power_mode_ = COMMON;
Device::Cap::Info cap_;
@@ -125,14 +138,19 @@ class OmniChassis {
/* PID计算的目标值 */
struct {
- float *motor_rotational_speed; /* 电机转速的动态数组，单位：RPM */
+ float motor_rotational_speed[4]; /* 电机转速的动态数组，单位：RPM */
} setpoint_;
+ float motor_speed_[4];
struct {
float motor3508_out[4]; /*转矩电流范围-1--1*/
} out_;
/* 反馈控制用的PID */
Component::PID follow_pid_; /* 跟随云台用的PID */
+ Component::PID xaccl_pid_; /* x方向加速跟随PID */
+ Component::PID yaccl_pid_; /* y方向加速跟随PID */
+
+ float max_power_limit;
System::Thread thread_;
@@ -141,16 +159,15 @@ class OmniChassis {
float yaw_;
Device::Referee::Data raw_ref_;
- Device::BaseMotor::Feedback motor_feedback_;
-
Component::CMD::ChassisCMD cmd_;
Component::UI::String string_;
+ Component::UI::Cycle cycle_;
Component::UI::Line line_;
Component::UI::Rectangle rectange_;
};
-typedef OmniChassis<Device::RMMotor, Device::RMMotor::Param> RMChassis;
+typedef nOmniChassis<Device::RMMotor, Device::RMMotor::Param> RMChassis;
} // namespace Module
diff --git a/src/robot/helm_infantry/.idea/.gitignore b/src/robot/helm_infantry/.idea/.gitignore
new file mode 100644
index 00000000..35410cac
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/.gitignore
@@ -0,0 +1,8 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
+# 基于编辑器的 HTTP 客户端请求
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/src/robot/helm_infantry/.idea/helm_infantry.iml b/src/robot/helm_infantry/.idea/helm_infantry.iml
new file mode 100644
index 00000000..bc2cd874
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/helm_infantry.iml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="CPP_MODULE" version="4">
+ <component name="NewModuleRootManager">
+ <content url="file://$MODULE_DIR$" />
+ <orderEntry type="inheritedJdk" />
+ <orderEntry type="sourceFolder" forTests="false" />
+ </component>
+</module>
\ No newline at end of file
diff --git a/src/robot/helm_infantry/.idea/modules.xml b/src/robot/helm_infantry/.idea/modules.xml
new file mode 100644
index 00000000..106af59b
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/modules.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="ProjectModuleManager">
+ <modules>
+ <module fileurl="file://$PROJECT_DIR$/.idea/helm_infantry.iml" filepath="$PROJECT_DIR$/.idea/helm_infantry.iml" />
+ </modules>
+ </component>
+</project>
\ No newline at end of file
diff --git a/src/robot/helm_infantry/.idea/vcs.xml b/src/robot/helm_infantry/.idea/vcs.xml
new file mode 100644
index 00000000..c2365ab1
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/vcs.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="VcsDirectoryMappings">
+ <mapping directory="$PROJECT_DIR$/../../.." vcs="Git" />
+ </component>
+</project>
\ No newline at end of file
diff --git a/src/robot/helm_infantry/robot.cpp b/src/robot/helm_infantry/robot.cpp
index f33f88f2..55ff9efa 100644
--- a/src/robot/helm_infantry/robot.cpp
+++ b/src/robot/helm_infantry/robot.cpp
@@ -9,21 +9,36 @@
/* clang-format off */
Robot::HelmInfantry::Param param =
{
+
+ // .robot_st =
+ // {
+ // .k=0.038,//0.092
+ // .bullet_type=Device::NewAi::BULLET_17,
+ // .bias_time=60,
+ // .s_bias=0.21500,
+ // .z_bias=0.12500,
+ // },
+
.chassis =
{
- .toque_coefficient_3508 = 0.044876589f,
- .speed_2_coefficient_3508 = 1.563578147703433e-19f,
- .out_2_coefficient_3508 = 54.67448580571495f,
- .constant_3508 = 6.564590719535691f,
+ // .toque_coefficient_3508 = 0.044876589f,
+ // .speed_2_coefficient_3508 = 1.563578147703433e-19f,
+ // .out_2_coefficient_3508 = 54.67448580571495f,
+ // .constant_3508 = 6.564590719535691f,
+ .toque_coefficient_3508 = 0.04585928841670807f,
+ .speed_2_coefficient_3508 = 1.7539151934403435e-18f,
+ .out_2_coefficient_3508 = 26.953338626324133f,
+ .constant_3508 = 4.220175002051428e-09f,
+
.follow_pid_param =
{
- .k = 5.0f,
- .p = 0.5f,
- .i = 0.5f,
- .d = 0.0f,
- .i_limit = 1.0f,
- .out_limit = 5.0f,
+ .k = 5.f,
+ .p = 1.3f,
+ .i = 0.8f,
+ .d = 0.05f,
+ .i_limit = 1.2f,
+ .out_limit = 6.0f,
.d_cutoff_freq = -1.0f,
.cycle = true,
},
@@ -52,25 +67,26 @@ Robot::HelmInfantry::Param param =
Module::RMHelmChassis::SET_MODE_6020_FOLLOW}
},
+
.pos_param =
{
Component::PosActuator::Param{
.speed =
{
.k = 0.005f,
- .p = 1.f,
- .i = 0.3f,
- .d = 0.0f,
+ .p = 1.0f,//1.0f,
+ .i = 0.4f,//0.3f
+ .d = 0.001f,
.i_limit = 0.25f,
- .out_limit = 0.4f,
+ .out_limit = 0.6f,
.d_cutoff_freq = -1.0f,
.cycle = false,
},
.position =
{
.k = 150.0f,
- .p = 1.0f,
- .i = 0.3f,
+ .p = 1.5f,
+ .i = 0.3f,//0.3f
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 200.0f,
@@ -86,9 +102,9 @@ Robot::HelmInfantry::Param param =
.speed =
{
.k = 0.005f,
- .p = 1.f,
- .i = 0.3f,
- .d = 0.0f,
+ .p = 1.0f,//1.0f,
+ .i = 0.4f,
+ .d = 0.001f,
.i_limit = 0.25f,
.out_limit = 0.4f,
.d_cutoff_freq = -1.0f,
@@ -98,7 +114,7 @@ Robot::HelmInfantry::Param param =
{
.k = 150.0f,
.p = 1.0f,
- .i = 0.3f,
+ .i = 0.4f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 200.0f,
@@ -114,9 +130,9 @@ Robot::HelmInfantry::Param param =
.speed =
{
.k = 0.005f,
- .p = 1.f,
- .i = 0.3f,
- .d = 0.0f,
+ .p = 1.0,//1.0f,
+ .i = 0.4f,
+ .d = 0.001f,
.i_limit = 0.25f,
.out_limit = 0.4f,
.d_cutoff_freq = -1.0f,
@@ -126,7 +142,7 @@ Robot::HelmInfantry::Param param =
{
.k = 150.0f,
.p = 1.0f,
- .i = 0.3f,
+ .i = 0.4f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 200.0f,
@@ -142,9 +158,9 @@ Robot::HelmInfantry::Param param =
.speed =
{
.k = 0.005f,
- .p = 1.f,
- .i = 0.3f,
- .d = 0.0f,
+ .p = 1.0f,//1.0f,
+ .i = 0.4f,//0.3f
+ .d = 0.001f,
.i_limit = 0.25f,
.out_limit = 0.4f,
.d_cutoff_freq = -1.0f,
@@ -154,7 +170,7 @@ Robot::HelmInfantry::Param param =
{
.k = 150.0f,
.p = 1.0f,
- .i = 0.3f,
+ .i = 0.4f,//0.3f
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 200.0f,
@@ -172,7 +188,7 @@ Robot::HelmInfantry::Param param =
Component::SpeedActuator::Param{
.speed =
{
- .k = 0.0001f,
+ .k = 0.0001f,//0.0001
.p = 2.0f,
.i = 0.0f,
.d = 0.0f,
@@ -190,7 +206,7 @@ Robot::HelmInfantry::Param param =
Component::SpeedActuator::Param{
.speed =
{
- .k = 0.0001f,
+ .k = 0.0001f,//0.0001
.p = 2.0f,
.i = 0.0f,
.d = 0.0f,
@@ -239,58 +255,57 @@ Robot::HelmInfantry::Param param =
.out_cutoff_freq = -1.0f,
},
},
-
- .mech_zero = { 1.15201962, 3.88327241, 3.41387439, 0.99171859},
-
+ .mech_zero = {0.448689401,3.6010201,3.14389372,3.13315582},
+ //.mech_zero = {2.10860233,5.35004959,4.736166, 4.7754863},
.pos_motor_param =
{
Device::RMMotor::Param{
- .id_feedback = 0x205,
+ .id_feedback = 0x206,
.id_control = GM6020_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
- .reverse = false
+ .reverse = false,
},
Device::RMMotor::Param{
- .id_feedback = 0x206,
- .id_control = GM6020_CTRL_ID_BASE,
+ .id_feedback = 0x20A,
+ .id_control = GM6020_CTRL_ID_EXTAND,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
- .reverse = false
+ .reverse = false,
},
Device::RMMotor::Param{
.id_feedback = 0x208,
.id_control = GM6020_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
- .reverse = false
+ .reverse = false,
},
Device::RMMotor::Param{
- .id_feedback = 0x207,
+ .id_feedback = 0x205,
.id_control = GM6020_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
- .reverse = false
+ .reverse = false,
},
},
.speed_motor_param =
{
Device::RMMotor::Param{
- .id_feedback = 0x201,
+ .id_feedback = 0x203,
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
.reverse = true,
},
Device::RMMotor::Param{
- .id_feedback = 0x202,
+ .id_feedback = 0x201,
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
.reverse = true,
},
Device::RMMotor::Param{
- .id_feedback = 0x203,
+ .id_feedback = 0x202,
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
@@ -364,11 +379,11 @@ Robot::HelmInfantry::Param param =
.speed =
{
/* GIMBAL_CTRL_PIT_OMEGA_IDX */
- .k = 0.4,
- .p = 1.0f,
- .i = 0.6f,
- .d = 0.f,
- .i_limit = 0.5f,
+ .k = 0.4f,
+ .p = 0.9f,//1.0f,
+ .i = 0.6f,//0.6f
+ .d = 0.0f,
+ .i_limit = 0.7f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.cycle = false,
@@ -377,6 +392,39 @@ Robot::HelmInfantry::Param param =
.position =
{
/* GIMBAL_CTRL_PIT_ANGLE_IDX */
+ .k = 10.0f,
+ .p = 1.1f,//1.0f,
+ .i = 0.0f,
+ .d = 0.0f,
+ .i_limit = 0.0f,
+ .out_limit = 10.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = true,
+ },
+
+ .in_cutoff_freq = -1.0f,
+
+ .out_cutoff_freq = -1.0f,
+ },
+
+ .yaw_ai_actr =
+ {
+ .speed =
+ {
+ /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+ .k = 0.4f,
+ .p = 1.f,
+ .i = 0.5f,
+ .d = 0.f,
+ .i_limit = 0.2f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = false,
+ },
+
+ .position =
+ {
+ /* GIMBAL_CTRL_YAW_ANGLE_IDX */
.k = 20.0f,
.p = 1.0f,
.i = 0.0f,
@@ -389,21 +437,53 @@ Robot::HelmInfantry::Param param =
.in_cutoff_freq = -1.0f,
+ .out_cutoff_freq = -1.0f,
+ },
+ .pit_ai_actr =
+ {
+ .speed =
+ {
+ /* GIMBAL_CTRL_PIT_OMEGA_IDX */
+ .k = 0.4f,
+ .p = 0.9f,//1.0f,
+ .i = 0.6f,//0.6f
+ .d = 0.0f,
+ .i_limit = 0.7f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = false,
+ },
+
+ .position =
+ {
+ /* GIMBAL_CTRL_PIT_ANGLE_IDX */
+ .k = 10.0f,
+ .p = 1.1f,//1.0f,
+ .i = 0.0f,
+ .d = 0.0f,
+ .i_limit = 0.0f,
+ .out_limit = 10.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = true,
+ },
+
+ .in_cutoff_freq = -1.0f,
+
.out_cutoff_freq = -1.0f,
},
.yaw_motor =
{
- .id_feedback = 0x20A,
+ .id_feedback = 0x209,
.id_control = GM6020_CTRL_ID_EXTAND,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
- .reverse = false
+ .reverse=false,
},
.pit_motor =
{
- .id_feedback = 0x209,
+ .id_feedback = 0x20B,
.id_control = GM6020_CTRL_ID_EXTAND,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
@@ -412,15 +492,16 @@ Robot::HelmInfantry::Param param =
.mech_zero =
{
- .yaw = 6.26477766f,
- .pit = M_2PI - 1.0f,
+ //.yaw = 2.08314586f,
+ .yaw = 3.66237926f,
+ .pit = M_2PI - 5.4602046f,
.rol = 0.0f,
},
.limit =
{
- .pitch_max = M_2PI - 0.60f,
- .pitch_min = M_2PI - 1.4f,
+ .pitch_max = M_2PI - 5.1f,
+ .pitch_min = M_2PI - 5.8f,
.yaw_max = 0.0f,
.yaw_min = 0.0f,
},
@@ -428,32 +509,37 @@ Robot::HelmInfantry::Param param =
.EVENT_MAP = {Component::CMD::EventMapItem{
Component::CMD::CMD_EVENT_LOST_CTRL,
Module::Gimbal::SET_MODE_RELAX},
+ Component::CMD::EventMapItem{
+ Device::DR16::DR16_SW_R_POS_TOP,
+ Module::Gimbal::ABSOLUTE},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_TOP,
Module::Gimbal::SET_MODE_ABSOLUTE},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_MID,
- Module::Gimbal::START_AUTO_AIM},
+ Module::Gimbal::SET_MODE_ABSOLUTE},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_BOT,
- Module::Gimbal::START_AUTO_AIM},
+ Module::Gimbal::SET_MODE_ABSOLUTE},
Component::CMD::EventMapItem{
Device::DR16::KEY_R_PRESS,
- Module::Gimbal::START_AUTO_AIM},
+ Module::Gimbal::SET_MODE_AUTO_AIM},
Component::CMD::EventMapItem{
Device::DR16::KEY_R_RELEASE,
- Module::Gimbal::STOP_AUTO_AIM}},
+ Module::Gimbal::SET_MODE_ABSOLUTE}},
},
.launcher = {
- .num_trig_tooth = 8.0f,
- .trig_gear_ratio = 36.0f,
- .fric_radius = 0.03f,
- .cover_open_duty = 0.125f,
- .cover_close_duty = 0.075f,
- .model = Module::Launcher::LAUNCHER_MODEL_17MM,
- .default_bullet_speed = 30.f,
+ .num_trig_tooth = 10.0f,
+ .trig_gear_ratio = 36.0f * 2.5f,
+ .fric_radius = 0.03f,
+ .cover_open_duty = 0.125f,
+ .cover_close_duty = 0.075f,
+ .model = Module::RMLauncher::LAUNCHER_MODEL_17MM,
+ .default_bullet_speed = 25.0f,
.min_launch_delay = static_cast<uint32_t>(1000.0f / 20.0f),
+ // .allow_reverse = false,
+ // .fric_speed = 3000.0f,
.trig_actr = {
Component::PosActuator::Param{
@@ -488,8 +574,8 @@ Robot::HelmInfantry::Param param =
.fric_actr = {
Component::SpeedActuator::Param{
.speed = {
- .k = 0.00025f,
- .p = 1.0f,
+ .k = 0.0003f,
+ .p = 1.8f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.2f,
@@ -504,8 +590,8 @@ Robot::HelmInfantry::Param param =
},
Component::SpeedActuator::Param{
.speed = {
- .k = 0.00025f,
- .p = 1.0f,
+ .k = 0.0003f,
+ .p = 1.8f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.2f,
@@ -520,7 +606,7 @@ Robot::HelmInfantry::Param param =
},
},
- .trig_motor = {
+ .trig_param = {
Device::RMMotor::Param{
.id_feedback = 0x204,
.id_control = M3508_M2006_CTRL_ID_BASE,
@@ -530,60 +616,65 @@ Robot::HelmInfantry::Param param =
}
},
- .fric_motor = {
+ .fric_param = {
Device::RMMotor::Param{
.id_feedback = 0x206,
.id_control = M3508_M2006_CTRL_ID_EXTAND,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
- .reverse = false
+ .reverse=false,
},
Device::RMMotor::Param{
.id_feedback = 0x207,
.id_control = M3508_M2006_CTRL_ID_EXTAND,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
- .reverse = false
+ .reverse=false,
},
},
.EVENT_MAP = {
Component::CMD::EventMapItem{
Component::CMD::CMD_EVENT_LOST_CTRL,
- Module::Launcher::CHANGE_FIRE_MODE_RELAX
+ Module::RMLauncher::CHANGE_FIRE_MODE_RELAX
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_TOP,
- Module::Launcher::CHANGE_FIRE_MODE_SAFE
+ Module::RMLauncher::CHANGE_FIRE_MODE_SAFE
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_MID,
- Module::Launcher::CHANGE_FIRE_MODE_LOADED
+ Module::RMLauncher::CHANGE_FIRE_MODE_LOADED
},
+ // Component::CMD::EventMapItem{
+ // Device::DR16::DR16_SW_R_POS_MID,
+ // Module::Launcher::LAUNCHER_STOP_TRIG
+ // },
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_BOT,
- Module::Launcher::CHANGE_FIRE_MODE_LOADED
+ Module::RMLauncher::CHANGE_FIRE_MODE_LOADED,
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_BOT,
- Module::Launcher::LAUNCHER_START_FIRE
+ Module::RMLauncher::LAUNCHER_START_FIRE,
},
+ // Component::CMD::EventMapItem{
+ // Device::DR16::DR16_SW_R_POS_BOT,
+ // Module::Launcher::CHANGE_TRIG_MODE_BURST,
+ // },
Component::CMD::EventMapItem{
Device::DR16::KEY_L_PRESS,
- Module::Launcher::LAUNCHER_START_FIRE
+ Module::RMLauncher::CHANGE_TRIG_MODE_BURST
},
Component::CMD::EventMapItem{
- Device::DR16::KEY_G,
- Module::Launcher::CHANGE_TRIG_MODE
+ Device::DR16::KEY_L_RELEASE,
+ Module::RMLauncher::LAUNCHER_STOP_TRIG
},
Component::CMD::EventMapItem{
- Device::DR16::KEY_R,
- Module::Launcher::OPEN_COVER
+ Device::DR16::KEY_G,
+ Module::RMLauncher::CHANGE_TRIG_MODE
},
- Component::CMD::EventMapItem{
- Device::DR16::KEY_F,
- Module::Launcher::CLOSE_COVER
- }
+
},
}, /* launcher */
.bmi088_rot =
@@ -596,7 +687,7 @@ Robot::HelmInfantry::Param param =
},
},
.cap = {
- .can = BSP_CAN_1,
+ .can = BSP_CAN_2,
},
};
/* clang-format on */
diff --git a/src/robot/helm_infantry/robot.hpp b/src/robot/helm_infantry/robot.hpp
index 201b620c..af06f7bb 100644
--- a/src/robot/helm_infantry/robot.hpp
+++ b/src/robot/helm_infantry/robot.hpp
@@ -1,7 +1,7 @@
/* #include "dev_xxx.hpp" */
#include "comp_cmd.hpp"
#include "dev_ahrs.hpp"
-#include "dev_ai.hpp"
+#include "dev_aim.hpp"
#include "dev_bmi088.hpp"
#include "dev_can.hpp"
#include "dev_cap.hpp"
@@ -20,7 +20,7 @@ class HelmInfantry {
typedef struct Param {
Module::RMHelmChassis::Param chassis;
Module::Gimbal::Param gimbal;
- Module::Launcher::Param launcher;
+ Module::RMLauncher::Param launcher;
Device::BMI088::Rotation bmi088_rot{};
Device::Cap::Param cap{};
} Param;
@@ -29,7 +29,7 @@ class HelmInfantry {
Device::Referee referee_;
Device::Can can_;
- Device::AI ai_;
+ Device::AIM aim_;
Device::AHRS ahrs_;
Device::BMI088 bmi088_;
Device::Cap cap_;
@@ -38,7 +38,7 @@ class HelmInfantry {
Module::RMHelmChassis chassis_;
Module::Gimbal gimbal_;
- Module::Launcher launcher_;
+ Module::RMLauncher launcher_;
HelmInfantry(Param& param, float control_freq)
: bmi088_(param.bmi088_rot),
diff --git a/src/robot/omni_infantry/robot.cpp b/src/robot/omni_infantry/robot.cpp
index 976cd2a9..06abdab0 100644
--- a/src/robot/omni_infantry/robot.cpp
+++ b/src/robot/omni_infantry/robot.cpp
@@ -3,36 +3,64 @@
#include <comp_actuator.hpp>
#include "dev_rm_motor.hpp"
-#include "mod_omni_chassis.hpp"
+// #include "mod_new_omni_chassis.hpp"
#include "system.hpp"
/* clang-format off */
Robot::OmniInfantry::Param param = {
.chassis={
- .toque_coefficient_ = 0.0327120418848f,
- .speed_2_coefficient_ = 1.227822928729637e-07,
- .out_2_coefficient_ = 1.1108430132455055e-24,
- .constant_ = 1.8135014050213443,
+ .toque_coefficient_ = 0.03985309056810189f,
+ .speed_2_coefficient_ = 5.297566618742181e-08f,
+ .out_2_coefficient_ = 28.851126052234243f,
+ .constant_ = 7.479591458247737f,
.follow_pid_param = {
- .k = 0.5f,
- .p = 1.0f,
- .i = 0.0f,
- .d = 0.0f,
- .i_limit = 1.0f,
+ .k = -1.5f,
+ //.p = 0.45f,
+ .p = 0.70f,
+ .i = 0.05f,
+ .d = 0.01f,
+ .i_limit = 0.5f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.cycle = true,
},
+ .xaccl_pid_param =
+ {
+ .k = 1.0f,
+ .p = 0.6f,
+ .i = 1.6f,
+ .d = 0.00f,
+ .i_limit = 1.0f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -0.001f,
+ .cycle = false,
+ },
+ .yaccl_pid_param =
+ {
+ .k = 1.0f,
+ .p = 0.6f,
+ .i = 1.6f,
+ .d = 0.00f,
+ .i_limit = 1.0f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -0.001f,
+ .cycle = false,
+ },
+
.EVENT_MAP = {
Component::CMD::EventMapItem{
Component::CMD::CMD_EVENT_LOST_CTRL,
Module::RMChassis::SET_MODE_RELAX
},
+ // Component::CMD::EventMapItem{
+ // Device::DR16::DR16_SW_L_POS_TOP,
+ // Module::RMChassis::SET_MODE_RELAX
+ // },
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_L_POS_TOP,
- Module::RMChassis::SET_MODE_RELAX
+ Module::RMChassis::RELAX
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_L_POS_MID,
@@ -59,6 +87,7 @@ Robot::OmniInfantry::Param param = {
.actuator_param = {
Component::SpeedActuator::Param{
.speed = {
+ //.k = 0.00050f,
.k = 0.00015f,
.p = 1.0f,
.i = 0.0f,
@@ -76,7 +105,7 @@ Robot::OmniInfantry::Param param = {
},
Component::SpeedActuator::Param{
.speed = {
- .k = 0.00018f,
+ .k = 0.00015f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
@@ -130,26 +159,47 @@ Robot::OmniInfantry::Param param = {
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
+ .reverse=false,
},
Device::RMMotor::Param{
.id_feedback = 0x204,
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
+ .reverse=false,
},
Device::RMMotor::Param{
.id_feedback = 0x201,
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
+ .reverse=false,
},
Device::RMMotor::Param{
.id_feedback = 0x202,
.id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_1,
+ .reverse=false,
},
},
+ .get_speed = [](float power_limit){
+ float speed = 0.0f;
+ if (power_limit <= 50.0f) {
+ speed = 5500;
+ } else if (power_limit <= 60.0f) {
+ speed = 6000;
+ } else if (power_limit <= 70.0f) {
+ speed = 6500;
+ } else if (power_limit <= 80.0f) {
+ speed = 7000;
+ } else if (power_limit <= 100.0f) {
+ speed = 7500;
+ } else {
+ speed = 8000;
+ }
+ return speed;
+ },
},
.gimbal= {
@@ -174,9 +224,9 @@ Robot::OmniInfantry::Param param = {
.yaw_actr = {
.speed = {
/* GIMBAL_CTRL_YAW_OMEGA_IDX */
- .k = 0.28f,
+ .k = 0.4f,
.p = 1.f,
- .i = 1.f,
+ .i = 0.5f,
.d = 0.f,
.i_limit = 0.2f,
.out_limit = 1.0f,
@@ -201,21 +251,50 @@ Robot::OmniInfantry::Param param = {
.out_cutoff_freq = -1.0f,
},
.pit_actr = {
- .speed = {
- /* GIMBAL_CTRL_PIT_OMEGA_IDX */
- .k = 0.0f,
- .p = 0.0f,
- .i = 0.f,
+ .speed = {
+ /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+ .k = 0.4f,
+ .p = 0.8f,
+ .i = 0.1f,
+ .d = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = false,
+ },
+
+ .position = {
+ /* GIMBAL_CTRL_YAW_ANGLE_IDX */
+ .k = 20.0f,
+ .p = 0.75f,
+ .i = 0.1f,
+ .d = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 10.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = true,
+ },
+
+ .in_cutoff_freq = -1.0f,
+
+ .out_cutoff_freq = -1.0f,
+ },
+ .yaw_ai_actr = {
+ .speed = {
+ /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+ .k = 0.4f,
+ .p = 1.f,
+ .i = 0.5f,
.d = 0.f,
- .i_limit = 0.f,
- .out_limit = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.cycle = false,
},
.position = {
- /* GIMBAL_CTRL_PIT_ANGLE_IDX */
- .k = 30.0f,
+ /* GIMBAL_CTRL_YAW_ANGLE_IDX */
+ .k = 20.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
@@ -229,29 +308,60 @@ Robot::OmniInfantry::Param param = {
.out_cutoff_freq = -1.0f,
},
+ .pit_ai_actr = {
+ .speed = {
+ /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+ .k = 0.4f,
+ .p = 0.8f,
+ .i = 0.1f,
+ .d = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = false,
+ },
+
+ .position = {
+ /* GIMBAL_CTRL_YAW_ANGLE_IDX */
+ .k = 20.0f,
+ .p = 0.75f,
+ .i = 0.1f,
+ .d = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 10.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = true,
+ },
+
+ .in_cutoff_freq = -1.0f,
+
+ .out_cutoff_freq = -1.0f,
+ },
.yaw_motor = {
.id_feedback = 0x205,
.id_control = GM6020_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_1,
+ .reverse = true,
},
.pit_motor ={
- .kp = 10.0f,
- .kd = 1.0f,
- .feedback_id = 0,
- .id = 1,
+ .id_feedback = 0x207,
+ .id_control = GM6020_CTRL_ID_BASE,
+ .model = Device::RMMotor::MOTOR_GM6020,
.can = BSP_CAN_2,
.reverse = true,
},
.mech_zero = {
- .yaw = 1.3f + M_PI / 2.0f,
- .pit = 0.0f,
+ .yaw = M_2PI - 3.69229198f,
+ .pit = 5.82529211,
.rol = 0.0f,
},
.limit = {
- .pitch_max = M_2PI - 0.38f,
- .pitch_min = M_2PI - 2.27f,
+ .pitch_max = M_2PI - 5.49569994f,
+ .pitch_min = M_2PI - 6.15663195f,
+ .yaw_max = 0.0f,
+ .yaw_min = 0.0f,
},
.EVENT_MAP = {
@@ -259,6 +369,10 @@ Robot::OmniInfantry::Param param = {
Component::CMD::CMD_EVENT_LOST_CTRL,
Module::Gimbal::SET_MODE_RELAX
},
+ Component::CMD::EventMapItem{
+ Device::DR16::DR16_SW_R_POS_TOP,
+ Module::Gimbal::ABSOLUTE
+ },
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_TOP,
Module::Gimbal::SET_MODE_ABSOLUTE
@@ -269,50 +383,57 @@ Robot::OmniInfantry::Param param = {
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_BOT,
- Module::Gimbal::SET_MODE_ABSOLUTE
- },
+ Module::Gimbal::SET_MODE_AUTO_AIM
+ },
+ // Component::CMD::EventMapItem{
+ // Device::DR16::DR16_SW_R_POS_MID,
+ // Module::Gimbal::ABSOLUTE
+ // },
+ // Component::CMD::EventMapItem{
+ // Device::DR16::DR16_SW_R_POS_BOT,
+ // Module::Gimbal::ABSOLUTE
+ // },
Component::CMD::EventMapItem{
Device::DR16::KEY_R_PRESS,
- Module::Gimbal::START_AUTO_AIM
+ Module::Gimbal::SET_MODE_AUTO_AIM
},
Component::CMD::EventMapItem{
Device::DR16::KEY_R_RELEASE,
- Module::Gimbal::STOP_AUTO_AIM
+ Module::Gimbal::SET_MODE_ABSOLUTE
}
},
-
},
.launcher = {
- .num_trig_tooth = 8.0f,
- .trig_gear_ratio = 36.0f,
+ .num_trig_tooth = 10.0f,
+ .trig_gear_ratio = 36.0f * 2.5f,
.fric_radius = 0.03f,
.cover_open_duty = 0.125f,
.cover_close_duty = 0.075f,
- .model = Module::Launcher::LAUNCHER_MODEL_17MM,
- .default_bullet_speed = 15.f,
- .min_launch_delay = static_cast<uint32_t>(1000.0f / 20.0f),
+ .model = Module::RMLauncher::LAUNCHER_MODEL_17MM,
+ .default_bullet_speed = 25.0f,
+ .min_launch_delay = static_cast<uint32_t>(1000.0f / 12.0f),
.trig_actr = {
Component::PosActuator::Param{
.speed = {
- .k = 5.0f,
- .p = 1.0f,
- .i = 0.15f,
+ .k = 3.0f,
+ .p = 2.0f,
+ .i = 0.5f,
.d = 0.0f,
- .i_limit = 0.1f,
- .out_limit = 1.5f,
+ .i_limit = 0.5f,
+ .out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.cycle = false,
},
.position = {
- .k = 5.0f,
+ .k = 1.8f,
.p = 1.0f,
.i = 0.0f,
- .d = 0.010f,
+ .d = 0.0f,
.i_limit = 0.0f,
- .out_limit = 0.70f,
+ .out_limit = 0.55f,
.d_cutoff_freq = -1.0f,
.cycle = true,
},
@@ -323,109 +444,128 @@ Robot::OmniInfantry::Param param = {
},
},
- .fric_actr = {
- Component::SpeedActuator::Param{
- .speed = {
- .k = 0.00025f,
- .p = 1.0f,
- .i = 0.4f,
- .d = 0.01f,
- .i_limit = 0.5f,
- .out_limit = 1.0f,
- .d_cutoff_freq = -1.0f,
- .cycle = false,
+ .fric_actr = {
+ Component::SpeedActuator::Param{
+ .speed = {
+ .k = 0.00025f,
+ .p = 1.0f,
+ .i = 0.0f,
+ .d = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = false,
+ },
+
+ .in_cutoff_freq = -1.0f,
+
+ .out_cutoff_freq = -1.0f,
+ },
+ Component::SpeedActuator::Param{
+ .speed = {
+ .k = 0.00025f,
+ .p = 1.0f,
+ .i = 0.0f,
+ .d = 0.0f,
+ .i_limit = 0.2f,
+ .out_limit = 1.0f,
+ .d_cutoff_freq = -1.0f,
+ .cycle = false,
+ },
+
+ .in_cutoff_freq = -1.0f,
+
+ .out_cutoff_freq = -1.0f,
},
-
- .in_cutoff_freq = -1.0f,
-
- .out_cutoff_freq = -1.0f,
- },
- Component::SpeedActuator::Param{
- .speed = {
- .k = 0.00025f,
- .p = 1.0f,
- .i = 0.4f,
- .d = 0.01f,
- .i_limit = 0.5f,
- .out_limit = 1.0f,
- .d_cutoff_freq = -1.0f,
- .cycle = false,
},
- .in_cutoff_freq = -1.0f,
-
- .out_cutoff_freq = -1.0f,
- },
- },
-
- .trig_motor = {
+ .trig_param = {
Device::RMMotor::Param{
- .id_feedback = 0x208,
+ .id_feedback = 0x207,
.id_control = M3508_M2006_CTRL_ID_EXTAND,
.model = Device::RMMotor::MOTOR_M2006,
- .can = BSP_CAN_2,
+ .can = BSP_CAN_1,
+ .reverse = true,
}
},
- .fric_motor = {
+ .fric_param = {
Device::RMMotor::Param{
- .id_feedback = 0x206,
- .id_control = M3508_M2006_CTRL_ID_EXTAND,
+ .id_feedback = 0x201,
+ .id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_2,
+ .reverse=false,
},
Device::RMMotor::Param{
- .id_feedback = 0x205,
- .id_control = M3508_M2006_CTRL_ID_EXTAND,
+ .id_feedback = 0x203,
+ .id_control = M3508_M2006_CTRL_ID_BASE,
.model = Device::RMMotor::MOTOR_M3508,
.can = BSP_CAN_2,
+ .reverse=false,
},
},
.EVENT_MAP = {
Component::CMD::EventMapItem{
Component::CMD::CMD_EVENT_LOST_CTRL,
- Module::Launcher::CHANGE_FIRE_MODE_RELAX
+ Module::RMLauncher::CHANGE_FIRE_MODE_RELAX
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_TOP,
- Module::Launcher::CHANGE_FIRE_MODE_SAFE
+ Module::RMLauncher::CHANGE_FIRE_MODE_SAFE
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_MID,
- Module::Launcher::CHANGE_FIRE_MODE_LOADED
+ Module::RMLauncher::CHANGE_FIRE_MODE_LOADED
+ },
+ Component::CMD::EventMapItem{
+ Device::DR16::DR16_SW_R_POS_MID,
+ Module::RMLauncher::LAUNCHER_STOP_TRIG
},
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_BOT,
- Module::Launcher::CHANGE_FIRE_MODE_LOADED
+ Module::RMLauncher::CHANGE_FIRE_MODE_LOADED
},
+ // Component::CMD::EventMapItem{
+ // Device::DR16::DR16_SW_R_POS_BOT,
+ // Module::RMLauncher::LAUNCHER_START_FIRE
+ // },
Component::CMD::EventMapItem{
Device::DR16::DR16_SW_R_POS_BOT,
- Module::Launcher::LAUNCHER_START_FIRE
+ Module::RMLauncher::CHANGE_TRIG_MODE_BURST
},
Component::CMD::EventMapItem{
Device::DR16::KEY_L_PRESS,
- Module::Launcher::LAUNCHER_START_FIRE
+ Module::RMLauncher::CHANGE_TRIG_MODE_BURST
+ },
+ Component::CMD::EventMapItem{
+ Device::DR16::KEY_L_RELEASE,
+ Module::RMLauncher::LAUNCHER_STOP_TRIG
},
Component::CMD::EventMapItem{
Device::DR16::KEY_G,
- Module::Launcher::CHANGE_TRIG_MODE
+ Module::RMLauncher::CHANGE_TRIG_MODE
},
Component::CMD::EventMapItem{
Device::DR16::KEY_R,
- Module::Launcher::OPEN_COVER
+ Module::RMLauncher::OPEN_COVER
},
Component::CMD::EventMapItem{
Device::DR16::KEY_F,
- Module::Launcher::CLOSE_COVER
- }
+ Module::RMLauncher::CLOSE_COVER
+ },
+ // Component::CMD::EventMapItem{
+ // Device::DR16::KEY_V,
+ // }
+ // Module::RMLauncher::CHANGE_AI_MODE
},
}, /* launcher */
.bmi088_rot = {
.rot_mat = {
- { +1, +0, +0},
{ +0, +1, +0},
+ { -1, +0, +0},
{ +0, +0, +1},
},
},
diff --git a/src/robot/omni_infantry/robot.hpp b/src/robot/omni_infantry/robot.hpp
index 97f0f8df..0064a3ea 100644
--- a/src/robot/omni_infantry/robot.hpp
+++ b/src/robot/omni_infantry/robot.hpp
@@ -1,15 +1,15 @@
#include "comp_cmd.hpp"
#include "dev_ahrs.hpp"
-#include "dev_ai.hpp"
+#include "dev_aim.hpp"
#include "dev_bmi088.hpp"
#include "dev_can.hpp"
#include "dev_cap.hpp"
#include "dev_dr16.hpp"
#include "dev_led_rgb.hpp"
#include "dev_referee.hpp"
+#include "mod_gimbal.hpp"
#include "mod_launcher.hpp"
#include "mod_omni_chassis.hpp"
-#include "mod_omni_gimbal.hpp"
void robot_init();
namespace Robot {
@@ -18,14 +18,14 @@ class OmniInfantry {
typedef struct Param {
Module::RMChassis::Param chassis;
Module::Gimbal::Param gimbal;
- Module::Launcher::Param launcher;
+ Module::RMLauncher::Param launcher;
Device::BMI088::Rotation bmi088_rot{};
Device::Cap::Param cap{};
} Param;
Component::CMD cmd_;
- Device::AI ai_;
+ Device::AIM ai_;
Device::AHRS ahrs_;
Device::BMI088 bmi088_;
Device::Can can_;
@@ -36,7 +36,7 @@ class OmniInfantry {
Module::RMChassis chassis_;
Module::Gimbal gimbal_;
- Module::Launcher launcher_;
+ Module::RMLauncher launcher_;
OmniInfantry(Param& param, float control_freq)
: bmi088_(param.bmi088_rot),