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),