diff --git a/.vscode/extensions.json b/.vscode/extensions.json index d1761aa2..9897dec8 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -16,7 +16,9 @@ "davidanson.vscode-markdownlint", "marus25.cortex-debug", "xyz.local-history", - "usernamehw.errorlens" + "usernamehw.errorlens", + "wayou.vscode-todo-highlight", + "gruntfuggly.todo-tree" ], // List of extensions recommended by VS Code that should not be recommended for users of this workspace. "unwantedRecommendations": [] diff --git a/hw/bsp/rm-c/config/helm_infantry.config b/hw/bsp/rm-c/config/helm_infantry.config index 1f4348f3..d0528863 100644 --- a/hw/bsp/rm-c/config/helm_infantry.config +++ b/hw/bsp/rm-c/config/helm_infantry.config @@ -1,50 +1,63 @@ +# CONFIG_auto_generated_config_prefix_board-custom_ctrl 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-mangopi_r818 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-custom_ctrl_spi is not set +# CONFIG_auto_generated_config_prefix_board-mc02 is not set +# CONFIG_auto_generated_config_prefix_board-node_imu is not set # 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-MiniPC_with_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-esp32-s3-arduino is not set +# CONFIG_auto_generated_config_prefix_board-atom 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-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-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-custom_ctrl 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-motor_control is not set +# CONFIG_auto_generated_config_prefix_robot-mit_control 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-Radar_car is not set +# CONFIG_auto_generated_config_prefix_robot-custom_ctrl_spi 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-custom_controller is not set +# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set # CONFIG_auto_generated_config_prefix_robot-hero is not set +# CONFIG_auto_generated_config_prefix_robot-bootloader is not set +# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set +# CONFIG_auto_generated_config_prefix_robot-dm_robot is not set +# CONFIG_auto_generated_config_prefix_robot-wearlab_imu 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-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-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-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-blink is not set +# CONFIG_auto_generated_config_prefix_robot-canfd_to_uart is not set # # 组件 @@ -53,25 +66,68 @@ 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-damiaomotor 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-MA600 is not set +# CONFIG_auto_generated_config_prefix_device-tof is not set +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-bq27220 is not set +# 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-imu=y +CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256 +# CONFIG_auto_generated_config_prefix_device-ai is not set CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384 +# CONFIG_HOST_CTRL_PRIORITY is not set +# CONFIG_auto_generated_config_prefix_device-ahrs-k is not set +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-custom_controller 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-spl06_001 is not set +# CONFIG_auto_generated_config_prefix_device-cap_parallel is not set CONFIG_auto_generated_config_prefix_device-cap=y CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256 @@ -83,73 +139,44 @@ 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-microswitch 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-speed_control is not set +CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384 +# 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-Radar is not set +# CONFIG_auto_generated_config_prefix_module-can_usart is not set +# CONFIG_auto_generated_config_prefix_module-hero_launcher is not set # CONFIG_auto_generated_config_prefix_module-balance is not set +# CONFIG_auto_generated_config_prefix_module-mit_control 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-omni_chassis is not set +# CONFIG_auto_generated_config_prefix_module-uart_update is not set +# CONFIG_auto_generated_config_prefix_module-robot_arm 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-helm_chassis=y # 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-helm_chassis=y -CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384 +# 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-dm_balance is not set +# CONFIG_auto_generated_config_prefix_module-can_imu_wearlab 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-launcher_drone is not set +# CONFIG_auto_generated_config_prefix_module-engineer_chassis 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-canfd_imu is not set +# CONFIG_auto_generated_config_prefix_module-omni_gimbal is not set # CONFIG_auto_generated_config_prefix_module-wheel_leg is not set +# CONFIG_auto_generated_config_prefix_module-canfd_to_uart is not set +# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set # end of 模块 diff --git a/hw/bsp/rm-c/config/hero.config b/hw/bsp/rm-c/config/hero.config index 9a5ca453..a31cfbe2 100644 --- a/hw/bsp/rm-c/config/hero.config +++ b/hw/bsp/rm-c/config/hero.config @@ -1,37 +1,63 @@ +# CONFIG_auto_generated_config_prefix_board-Webots is not set +# CONFIG_auto_generated_config_prefix_board-node_imu is not set +# CONFIG_auto_generated_config_prefix_board-esp32-c3-arduino is not set # CONFIG_auto_generated_config_prefix_board-f103_can is not set +# CONFIG_auto_generated_config_prefix_board-custom_ctrl_spi is not set +# CONFIG_auto_generated_config_prefix_board-esp32-c3-idf is not set # CONFIG_auto_generated_config_prefix_board-MiniPC is not set -# CONFIG_auto_generated_config_prefix_board-node_imu is not set -# CONFIG_auto_generated_config_prefix_board-microswitch 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_board-esp32-c3 is not set # CONFIG_auto_generated_config_prefix_board-c-mini is not set +# CONFIG_auto_generated_config_prefix_board-microswitch is not set +# CONFIG_auto_generated_config_prefix_board-mangopi_r818 is not set +# CONFIG_auto_generated_config_prefix_board-atom is not set +# CONFIG_auto_generated_config_prefix_board-custom_ctrl is not set +# CONFIG_auto_generated_config_prefix_board-esp32-s3-arduino is not set +# CONFIG_auto_generated_config_prefix_board-mc02 is not set +# CONFIG_auto_generated_config_prefix_board-MiniPC_with_canfd 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_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_system-Linux_Webots is not set # CONFIG_auto_generated_config_prefix_system-Linux is not set -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-blink is not set -# CONFIG_auto_generated_config_prefix_robot-dart is not set -# CONFIG_auto_generated_config_prefix_robot-infantry is not set -# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set -# CONFIG_auto_generated_config_prefix_robot-microswitch is not set # CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set +# CONFIG_auto_generated_config_prefix_robot-motor_control is not set CONFIG_auto_generated_config_prefix_robot-hero=y +# CONFIG_auto_generated_config_prefix_robot-helm_infantry is not set +# CONFIG_auto_generated_config_prefix_robot-bootloader is not set +# CONFIG_auto_generated_config_prefix_robot-omni_infantry is not set +# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set +# CONFIG_auto_generated_config_prefix_robot-custom_ctrl_spi is not set +# CONFIG_auto_generated_config_prefix_robot-canfd_to_uart is not set +# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set # CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set # CONFIG_auto_generated_config_prefix_robot-sentry is not set # CONFIG_auto_generated_config_prefix_robot-can_to_uart is not set -# CONFIG_auto_generated_config_prefix_robot-engineer is not set +# CONFIG_auto_generated_config_prefix_robot-dart is not set +# CONFIG_auto_generated_config_prefix_robot-uart_net_config is not set +# CONFIG_auto_generated_config_prefix_robot-custom_controller is not set +# CONFIG_auto_generated_config_prefix_robot-microswitch is not set +# CONFIG_auto_generated_config_prefix_robot-custom_ctrl is not set +# CONFIG_auto_generated_config_prefix_robot-Radar_car is not set +# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set +# CONFIG_auto_generated_config_prefix_robot-arm_engineer is not set +# CONFIG_auto_generated_config_prefix_robot-canfd_imu is not set +# CONFIG_auto_generated_config_prefix_robot-dm_robot is not set +# CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set +# CONFIG_auto_generated_config_prefix_robot-infantry is not set +# CONFIG_auto_generated_config_prefix_robot-blink is not set +# CONFIG_auto_generated_config_prefix_robot-drone_gimbal is not set +# CONFIG_auto_generated_config_prefix_robot-mit_control is not set # # 组件 @@ -40,17 +66,6 @@ CONFIG_auto_generated_config_prefix_robot-hero=y # # 设备 # -CONFIG_auto_generated_config_prefix_device-imu=y -CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256 -# CONFIG_auto_generated_config_prefix_device-wearlab is not set -CONFIG_auto_generated_config_prefix_device-dr16=y -CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384 -# CONFIG_auto_generated_config_prefix_device-mech is not set -# CONFIG_auto_generated_config_prefix_device-tof is not set -CONFIG_auto_generated_config_prefix_device-ahrs=y -CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256 -CONFIG_auto_generated_config_prefix_device-servo=y -CONFIG_auto_generated_config_prefix_device-motor=y CONFIG_auto_generated_config_prefix_device-ai=y CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384 @@ -60,16 +75,9 @@ CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384 # CONFIG_HOST_CTRL_PRIORITY is not set # end of 上位机 -# 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-bmi088=y -CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256 CONFIG_auto_generated_config_prefix_device-can=y -# CONFIG_auto_generated_config_prefix_device-microswitch is not set +# CONFIG_auto_generated_config_prefix_device-net_config is not set +# CONFIG_auto_generated_config_prefix_device-MA600 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 @@ -78,6 +86,11 @@ 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 裁判系统 # @@ -87,10 +100,31 @@ CONFIG_UI_DYNAMIC_CYCLE=20 CONFIG_UI_STATIC_CYCLE=1000 # end of 操作手UI -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-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-ahrs-9 is not set +CONFIG_DEVICE_AHRS_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-servo is not set +# CONFIG_auto_generated_config_prefix_device-custom_controller is not set +# CONFIG_auto_generated_config_prefix_device-microswitch is not set +# CONFIG_auto_generated_config_prefix_device-spl06_001 is not set +# CONFIG_auto_generated_config_prefix_device-simulator is not set +CONFIG_auto_generated_config_prefix_device-bmi088=y +CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256 +CONFIG_auto_generated_config_prefix_device-ahrs=y +# CONFIG_auto_generated_config_prefix_device-mt6701 is not set +# CONFIG_auto_generated_config_prefix_device-cap_parallel is not set +# CONFIG_auto_generated_config_prefix_device-canfd is not set +# CONFIG_auto_generated_config_prefix_device-blink_led is not set +# CONFIG_auto_generated_config_prefix_device-mmc5603 is not set +# CONFIG_auto_generated_config_prefix_device-aim is not set # CONFIG_auto_generated_config_prefix_device-buzzer is not set -# CONFIG_auto_generated_config_prefix_device-laser is not set CONFIG_auto_generated_config_prefix_device-cap=y CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256 @@ -102,24 +136,48 @@ CONFIG_CAP_PERCENT_WORK=30 CONFIG_CAP_MAX_LOAD=100 # end of 超级电容 -# CONFIG_auto_generated_config_prefix_device-blink_led 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-tof is not set +# CONFIG_auto_generated_config_prefix_device-laser is not set +# CONFIG_auto_generated_config_prefix_device-damiaomotor is not set +# CONFIG_auto_generated_config_prefix_device-ina226 is not set +# CONFIG_auto_generated_config_prefix_device-ahrs-k is not set # end of 设备 # # 模块 # -# CONFIG_auto_generated_config_prefix_module-ore_collect is not set +# CONFIG_auto_generated_config_prefix_module-balance is not set +# CONFIG_auto_generated_config_prefix_module-omni_gimbal is not set +# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set +# CONFIG_auto_generated_config_prefix_module-can_usart is not set +# CONFIG_auto_generated_config_prefix_module-robot_arm is not set +# CONFIG_auto_generated_config_prefix_module-canfd_to_uart is not set +# CONFIG_auto_generated_config_prefix_module-performance is not set # CONFIG_auto_generated_config_prefix_module-wheel_leg is not set -# CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set +# CONFIG_auto_generated_config_prefix_module-uart_update is not set +# CONFIG_auto_generated_config_prefix_module-launcher is not set +# CONFIG_auto_generated_config_prefix_module-speed_control is not set +# CONFIG_auto_generated_config_prefix_module-custom_controller is not set +# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set +# CONFIG_auto_generated_config_prefix_module-dm_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-microswitch 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-dart_gimbal is not set +# CONFIG_auto_generated_config_prefix_module-helm_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-microswitch is not set -CONFIG_auto_generated_config_prefix_module-launcher=y -CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384 -# CONFIG_auto_generated_config_prefix_module-can_usart is not set -CONFIG_auto_generated_config_prefix_module-chassis=y -CONFIG_MODULE_CHASSIS_TASK_STACK_DEPTH=384 # CONFIG_auto_generated_config_prefix_module-dart_launcher is not set -# CONFIG_auto_generated_config_prefix_module-balance is not set +CONFIG_auto_generated_config_prefix_module-hero_launcher=y +CONFIG_MODULE_HERO_LAUNCHER_TASK_STACK_DEPTH=512 +# CONFIG_auto_generated_config_prefix_module-canfd_imu is not set +# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set +# CONFIG_auto_generated_config_prefix_module-Radar is not set +# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set +# CONFIG_auto_generated_config_prefix_module-omni_chassis is not set +# CONFIG_auto_generated_config_prefix_module-mit_control 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..4ed288bf --- /dev/null +++ b/hw/bsp/rm-c/config/omni_infantry.config @@ -0,0 +1,182 @@ +# CONFIG_auto_generated_config_prefix_board-custom_ctrl 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-mangopi_r818 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-custom_ctrl_spi is not set +# CONFIG_auto_generated_config_prefix_board-mc02 is not set +# CONFIG_auto_generated_config_prefix_board-node_imu is not set +# CONFIG_auto_generated_config_prefix_board-c-mini is not set +# CONFIG_auto_generated_config_prefix_board-MiniPC_with_canfd 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-microswitch 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-custom_ctrl 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-motor_control is not set +# CONFIG_auto_generated_config_prefix_robot-mit_control is not set +# CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set +# CONFIG_auto_generated_config_prefix_robot-can_to_uart is not set +# CONFIG_auto_generated_config_prefix_robot-Radar_car is not set +# CONFIG_auto_generated_config_prefix_robot-custom_ctrl_spi 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-custom_controller is not set +# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set +# CONFIG_auto_generated_config_prefix_robot-hero is not set +# CONFIG_auto_generated_config_prefix_robot-bootloader is not set +# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set +# CONFIG_auto_generated_config_prefix_robot-dm_robot is not set +# CONFIG_auto_generated_config_prefix_robot-wearlab_imu 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-microswitch 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_robot-canfd_to_uart is not set + +# +# 组件 +# + +# +# 设备 +# +# CONFIG_auto_generated_config_prefix_device-laser is not set +# CONFIG_auto_generated_config_prefix_device-damiaomotor 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-MA600 is not set +# CONFIG_auto_generated_config_prefix_device-tof is not set +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-bq27220 is not set +# 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-imu=y +CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256 +# CONFIG_auto_generated_config_prefix_device-ai is not set +CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384 +# CONFIG_HOST_CTRL_PRIORITY is not set +# CONFIG_auto_generated_config_prefix_device-ahrs-k is not set +CONFIG_auto_generated_config_prefix_device-aim=y + +# +# 上位机 +# +# end of 上位机 + +# CONFIG_auto_generated_config_prefix_device-custom_controller 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-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-spl06_001 is not set +# CONFIG_auto_generated_config_prefix_device-cap_parallel is not set +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-microswitch 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 +# end of 设备 + +# +# 模块 +# +# CONFIG_auto_generated_config_prefix_module-speed_control is not set +CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384 +# 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-Radar is not set +# CONFIG_auto_generated_config_prefix_module-can_usart is not set +# CONFIG_auto_generated_config_prefix_module-hero_launcher is not set +# CONFIG_auto_generated_config_prefix_module-balance is not set +# CONFIG_auto_generated_config_prefix_module-mit_control 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-uart_update is not set +# CONFIG_auto_generated_config_prefix_module-robot_arm is not set +# CONFIG_auto_generated_config_prefix_module-can_imu is not set +# CONFIG_auto_generated_config_prefix_module-helm_chassis is not set +# CONFIG_auto_generated_config_prefix_module-custom_controller is not set +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-dm_balance is not set +# CONFIG_auto_generated_config_prefix_module-can_imu_wearlab 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-microswitch is not set +# CONFIG_auto_generated_config_prefix_module-canfd_imu is not set +# CONFIG_auto_generated_config_prefix_module-omni_gimbal is not set +# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set +# CONFIG_auto_generated_config_prefix_module-canfd_to_uart is not set +# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set +# end of 模块 diff --git a/lib/host-protocol b/lib/host-protocol index 06f5d56d..5e0a1a9b 160000 --- a/lib/host-protocol +++ b/lib/host-protocol @@ -1 +1 @@ -Subproject commit 06f5d56d62baa44d8265a514cd6e997699289c35 +Subproject commit 5e0a1a9b2c3c3229690a2934d625d33acfa5020b diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index 6acd13f0..91270a91 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -8,8 +8,7 @@ #define AI_CMD_LIMIT (0.08f) #define AI_CTRL_SENSE (1.0f / 90.0f) #define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t)) -#define AI_LEN_TX_BUFF \ - (sizeof(Protocol_UpPackageMCU_t) + sizeof(Protocol_UpPackageReferee_t)) +#define AI_LEN_TX_BUFF (sizeof(Protocol_UpPackage_t)) static uint8_t rxbuf[AI_LEN_RX_BUFF]; static uint8_t txbuf[AI_LEN_TX_BUFF]; @@ -25,18 +24,15 @@ AI::AI(bool autoscan_enable) AI *ai = static_cast(arg); ai->data_ready_.Post(); }; - + // TODO: AUTOAIM和AI串口接收都要加idle回调 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 = [](AI *ai) { - auto quat_sub = - Message::Subscriber("imu_quat"); auto ref_sub = Message::Subscriber("referee"); auto yaw_sub = Message::Subscriber("chassis_yaw"); - auto eulr_sub = Message::Subscriber("imu_eulr"); while (1) { @@ -63,9 +59,7 @@ AI::AI(bool autoscan_enable) ai->ai_tp_.Publish(ai->cmd_for_ref_); /* 发送数据到上位机 */ - quat_sub.DumpData(ai->quat_); ai->PackMCU(); - ai->StartTrans(); System::Thread::Sleep(2); @@ -92,14 +86,12 @@ bool AI::PraseHost() { } bool AI::StartTrans() { - size_t len = sizeof(this->to_host_.mcu); + this->to_host_.crc16 = Component::CRC16::Calculate( + reinterpret_cast(&(this->to_host_)), + sizeof(this->to_host_) - sizeof(uint16_t), CRC16_INIT); + size_t len = sizeof(this->to_host_); void *src = NULL; - if (this->ref_updated_) { - len += sizeof(this->to_host_.ref); - src = &(this->to_host_); - } else { - src = &(this->to_host_.mcu); - } + src = &(to_host_); this->ref_updated_ = false; memcpy(txbuf, src, len); @@ -109,7 +101,6 @@ bool AI::StartTrans() { bool AI::Offline() { /* 离线移交控制权 */ if (bsp_time_get_ms() - this->last_online_time_ > 200) { - memset(&this->cmd_, 0, sizeof(cmd_)); if (!this->autoscan_enable_) { this->cmd_.online = true; } else { @@ -122,30 +113,35 @@ bool AI::Offline() { } bool AI::PackMCU() { - this->to_host_.mcu.id = AI_ID_MCU; - memcpy(&(this->to_host_.mcu.package.data.quat), &(this->quat_), - sizeof(this->quat_)); - this->to_host_.mcu.package.data.notice = this->notice_for_ai_; - this->to_host_.mcu.package.crc16 = Component::CRC16::Calculate( - reinterpret_cast(&(this->to_host_.mcu.package)), - sizeof(this->to_host_.mcu.package) - sizeof(uint16_t), CRC16_INIT); + float temp = 0.0; + + this->to_host_.id = AI_ID_MCU; + + memcpy(&(this->to_host_.data.eulr), &(this->eulr_), sizeof(this->eulr_)); + memcpy(&(this->to_host_.data.yaw), &temp, sizeof(this->to_host_.data.yaw)); + memcpy(&(to_host_.data.pit), &temp, sizeof(this->to_host_.data.pit)); + memcpy(&(to_host_.data.rol), &temp, sizeof(this->to_host_.data.rol)); + + this->to_host_.data.notice = this->notice_for_ai_; + return true; } bool AI::PackRef() { - this->to_host_.ref.id = AI_ID_REF; - this->to_host_.mcu.package.data.ball_speed = - static_cast(this->ref_.ball_speed); - this->to_host_.ref.package.data.arm = this->ref_.robot_id; - this->to_host_.ref.package.data.rfid = this->ref_.robot_buff; - this->to_host_.ref.package.data.team = this->ref_.team; - this->to_host_.ref.package.data.race = this->ref_.game_type; - this->to_host_.ref.package.crc16 = Component::CRC16::Calculate( - reinterpret_cast(&(this->to_host_.ref.package)), - sizeof(this->to_host_.ref.package) - sizeof(uint16_t), CRC16_INIT); - + this->to_host_.data.ball_speed = static_cast(this->ref_.ball_speed); + this->to_host_.data.rfid = this->ref_.rfid; + this->to_host_.data.sentry_hp = this->ref_.hp; + this->to_host_.data.game_progress = this->ref_.game_progress; + this->to_host_.data.ballet_remain = this->ref_.bullet_num; this->ref_updated_ = true; - + this->to_host_.data.hero_x = this->ref_.hero_x; + this->to_host_.data.hero_y = this->ref_.hero_y; + this->to_host_.data.standard_3_x = this->ref_.infantry_3_x; + this->to_host_.data.standard_3_y = this->ref_.infantry_3_y; + this->to_host_.data.standard_4_x = this->ref_.infantry_4_x; + this->to_host_.data.standard_4_y = this->ref_.infantry_4_y; + this->to_host_.data.engineer_x = this->ref_.engineer_x; + this->to_host_.data.engineer_y = this->ref_.engineer_y; return true; } @@ -153,12 +149,9 @@ void AI::DecideAction() { memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); this->notice_ = this->from_host_.data.notice; - /* AI自瞄数据不更新，重置notice_ */ - // TODO:检测更新方式不合理 - if (this->cmd_.gimbal.eulr.yaw == this->last_auto_aim_eulr_.yaw || - this->cmd_.gimbal.eulr.pit == this->last_auto_aim_eulr_.pit) { - this->notice_ = 0; - } else { + + if (this->cmd_.gimbal.eulr.yaw != this->last_auto_aim_eulr_.yaw || + this->cmd_.gimbal.eulr.pit != this->last_auto_aim_eulr_.pit) { this->aim_time_ = bsp_time_get_ms(); /* 自瞄锁定时刻 */ } memcpy(&(this->last_auto_aim_eulr_), &(this->cmd_.gimbal.eulr), @@ -178,13 +171,7 @@ void AI::DecideAction() { this->damage_.is_damaged_ = false; } - /* 判定是否可以开始导航 */ - if (this->from_host_.data.chassis_move_vec.vx == 0 && damage_.is_damaged_) { - navigation_enable_ = false; - } else { - navigation_enable_ = true; - } - + navigation_enable_ = true; /* 裁判系统行为：确认复活、购买发弹量 */ if (this->ref_.hp == 0) { this->action_.ai_to_referee = CONFIRM_RESURRECTION; @@ -195,42 +182,27 @@ void AI::DecideAction() { } /* 底盘行为*/ - if (navigation_enable_) { - if (this->ref_.outpost_hp > 200) { /* 认为哨兵&基地无敌 */ - this->action_.ai_chassis = AI::Action::TO_HIGHWAY; - this->notice_for_ai_ = AI::Action::TO_HIGHWAY; - } else { - if (this->ref_.hp < 100 || - (this->action_.ai_to_referee == EXCHANGE_BULLETS)) { /* max = 400 */ - this->action_.ai_chassis = AI::Action::TO_SUPPLY; - this->notice_for_ai_ = AI::Action::TO_SUPPLY; - } else { - this->action_.ai_chassis = AI::Action::TO_PATROL_AREA; - this->notice_for_ai_ = AI::Action::TO_PATROL_AREA; - } - } - } else { - this->action_.ai_chassis = AI::Action::ROTOR; + if (navigation_enable_ && damage_.is_damaged_ == false) { + this->action_.ai_chassis = AI::Action::START_AUTO_CONTROL; + } else if (damage_.is_damaged_ == true) { + this->action_.ai_chassis = AI::Action::STOP_AUTO_CONTROL; } /* 云台行为 */ - if (this->notice_ == AI_NOTICE_AUTO_AIM || this->notice_ == AI_NOTICE_FIRE || - bsp_time_get_ms() - this->aim_time_ < 1000) { + if (this->notice_ == 2 || bsp_time_get_ms() - aim_time_ < 1300) { /* 自瞄/丢失目标1000ms内，进行视觉暂留 */ this->action_.ai_gimbal = AI::Action::AUTO_AIM; this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - } else if (damage_.is_damaged_) { - this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - this->action_.ai_gimbal = AI::Action::AFFECTED; - } else { + } else if (this->notice_ == 5) { this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; this->action_.ai_gimbal = AI::Action::SCANF; } /* 发射机构行为 */ + // TODO: notice应该按位与，防止命令冲突 if (this->notice_ == AI_NOTICE_FIRE) { /* 开火 */ this->action_.ai_launcher = AI::Action::FIRE; - } else { + } else if (this->notice_ == AI_NOTICE_AUTO_AIM) { this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */ } } @@ -246,15 +218,12 @@ bool AI::PackCMD() { /* AUTO控制模式，用于全自动机器人 */ if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) { switch (this->action_.ai_chassis) { - case TO_PATROL_AREA: - case TO_HIGHWAY: - case TO_OUTPOST: - case TO_SUPPLY: + case START_AUTO_CONTROL: memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); break; - case ROTOR: + case STOP_AUTO_CONTROL: this->event_.Active(AI_ROTOR); break; } @@ -263,25 +232,34 @@ bool AI::PackCMD() { case AUTO_AIM: memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); - break; - case AFFECTED: - this->cmd_.gimbal.eulr.yaw = this->damage_.gimbal_yaw_ + - this->damage_.id_ * (M_2PI / 4.0) - - this->damage_.yaw_offset_; - this->cmd_.gimbal.eulr.pit = 0.0f; - this->gimbal_scan_start_angle_ = - this->eulr_.yaw; /* 更新yaw扫描的起始位置 */ - this->target_scan_angle_ = 0.0; /* 置零yaw扫描的位置增量 */ + if (cmd_.gimbal.eulr.pit == 0 && cmd_.gimbal.eulr.yaw == 0) { + cmd_.gimbal.eulr.pit = eulr_.pit; + cmd_.gimbal.eulr.yaw = eulr_.yaw; + } break; case SCANF: - this->target_scan_angle_ += this->scanf_mode_.scanf_yaw_rate; - this->cmd_.gimbal.eulr.yaw = - this->target_scan_angle_ + this->gimbal_scan_start_angle_; + /*yaw轴旋转加入随机数扰动*/ + static float smoothed_random = + 0.0f; /* 静态变量，用于存储平滑后的随机值 */ + float alpha = 0.1f; /* 平滑系数，控制新随机值的影响程度 */ + float raw_random = + static_cast(rand() % 10 - 5) / 1000.0f; /* 原始随机扰动 */ + smoothed_random = alpha * raw_random + + (1.0f - alpha) * smoothed_random; /* 低通滤波 */ + float yaw_rate = this->scanf_mode_.scanf_yaw_rate; + this->target_scan_angle_ += yaw_rate * (1.0f + smoothed_random); + this->cmd_.gimbal.eulr.yaw = this->eulr_.yaw; + + float t = static_cast(bsp_time_get_ms()) / 1000.0f; + + /* 计算三角波 */ + float phase = + fmodf(t * this->scanf_mode_.scanf_pit_omega, 2.0f * M_PI); + float triangle_wave_mapped = + (fabsf(phase / static_cast(M_PI) - 1.0f) * 2.0f) - 1.0f; this->cmd_.gimbal.eulr.pit = this->scanf_mode_.scanf_pit_center + - this->scanf_mode_.scanf_pit_range * - sinf(this->scanf_mode_.scanf_pit_omega * - static_cast(bsp_time_get_ms()) / 1000.0f); + this->scanf_mode_.scanf_pit_range * triangle_wave_mapped; break; } @@ -319,29 +297,23 @@ bool AI::PackCMD() { } this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num; - /* 比赛开始前不运行 */ - if (this->ref_.game_progress == GAMING || - this->ref_.game_progress == PREPARATION || - this->ref_.game_progress == WAITTING) { - this->cmd_tp_.Publish(this->cmd_); - } + this->cmd_tp_.Publish(this->cmd_); + this->cmd_tp_.Publish(this->cmd_); + this->cmd_tp_.Publish(this->cmd_); } /* OP控制模式，用于鼠标右键自瞄 */ - else if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) { + if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) { memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); - memcpy(&(this->cmd_.ext.extern_channel), - &(this->from_host_.data.extern_channel), - sizeof(this->cmd_.ext.extern_channel)); + if (cmd_.gimbal.eulr.pit == 0 && cmd_.gimbal.eulr.yaw == 0) { + cmd_.gimbal.eulr.pit = eulr_.pit; + cmd_.gimbal.eulr.yaw = eulr_.yaw; + } memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); - memcpy(&(this->cmd_.ext.extern_channel), - &(this->from_host_.data.extern_channel), - sizeof(this->cmd_.ext.extern_channel)); - this->notice_ = this->from_host_.data.notice; this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI; @@ -355,14 +327,14 @@ bool AI::PackCMD() { } void AI::PraseRef() { +// TODO: 错误的射速 #if RB_HERO - this->ref_.ball_speed = BULLET_SPEED_LIMIT_42MM + this->ref_.ball_speed = BULLET_SPEED_LIMIT_42MM; #else this->ref_.ball_speed = BULLET_SPEED_LIMIT_17MM; #endif - this->ref_.max_hp = - this->raw_ref_.robot_status.max_hp; + this->ref_.max_hp = this->raw_ref_.robot_status.max_hp; this->ref_.hp = this->raw_ref_.robot_status.remain_hp; @@ -373,14 +345,6 @@ void AI::PraseRef() { } this->ref_.status = this->raw_ref_.status; - if (this->raw_ref_.rfid.own_highland_annular == 1 || - this->raw_ref_.rfid.enemy_highland_annular == 1) { - this->ref_.robot_buff |= AI_RFID_SNIP; - } else if (this->raw_ref_.rfid.own_energy_mech_activation == 1) { - this->ref_.robot_buff |= AI_RFID_BUFF; - } else { - this->ref_.robot_buff = 0; - } switch (this->raw_ref_.game_status.game_type) { case Referee::REF_GAME_TYPE_RMUC: this->ref_.game_type = AI_RACE_RMUC; @@ -420,20 +384,26 @@ void AI::PraseRef() { this->ref_.game_progress = this->raw_ref_.game_status.game_progress; + // TODO: game_robot_hp只有哨兵？ if (this->raw_ref_.robot_status.robot_id < 100) { this->ref_.base_hp = this->raw_ref_.game_robot_hp.red_base; this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.red_outpose; - this->ref_.own_virtual_shield_value = - this->raw_ref_.field_event.virtual_shield_value; + this->ref_.hp = this->raw_ref_.game_robot_hp.red_7; } else { this->ref_.base_hp = this->raw_ref_.game_robot_hp.blue_base; this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.blue_outpose; - this->ref_.own_virtual_shield_value = - this->raw_ref_.field_event.virtual_shield_value; + this->ref_.hp = this->raw_ref_.game_robot_hp.blue_7; } this->ref_.coin_num = this->raw_ref_.bullet_remain.coin_remain; - this->ref_.pos_x = this->raw_ref_.robot_pos.x; - this->ref_.pos_y = this->raw_ref_.robot_pos.y; + this->ref_.bullet_num = this->raw_ref_.bullet_remain.bullet_17_remain; + this->ref_.hero_x = this->raw_ref_.robot_pos_for_snetry.hero_x; + this->ref_.hero_y = this->raw_ref_.robot_pos_for_snetry.hero_y; + this->ref_.infantry_3_x = this->raw_ref_.robot_pos_for_snetry.standard_3_x; + this->ref_.infantry_3_y = this->raw_ref_.robot_pos_for_snetry.standard_3_y; + this->ref_.infantry_4_x = this->raw_ref_.robot_pos_for_snetry.standard_4_x; + this->ref_.infantry_4_y = this->raw_ref_.robot_pos_for_snetry.standard_4_y; + this->ref_.engineer_x = this->raw_ref_.robot_pos_for_snetry.engineer_x; + this->ref_.engineer_y = this->raw_ref_.robot_pos_for_snetry.engineer_y; this->ref_.pos_angle = this->raw_ref_.robot_pos.angle; this->ref_.target_pos_x = this->raw_ref_.client_map.position_x; @@ -442,4 +412,6 @@ void AI::PraseRef() { if (this->raw_ref_.robot_damage.damage_type == 0) { this->ref_.damaged_armor_id = this->raw_ref_.robot_damage.armor_id; } + + std::memcpy(&this->ref_.rfid, &this->raw_ref_.rfid, sizeof(uint32_t)); } diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp index 8f25773d..13707d97 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -10,16 +10,6 @@ namespace Device { class AI { public: - typedef struct __attribute__((packed)) { - uint8_t id; - Protocol_UpPackageReferee_t package; - } RefereePckage; - - typedef struct __attribute__((packed)) { - uint8_t id; - Protocol_UpPackageMCU_t package; - } MCUPckage; - typedef struct { uint8_t game_type; Device::Referee::Status status; @@ -29,6 +19,7 @@ class AI { uint32_t ball_speed; uint32_t max_hp; uint32_t hp; + uint32_t rfid; uint8_t game_progress; uint16_t base_hp; @@ -36,8 +27,16 @@ class AI { uint16_t bullet_num; uint16_t coin_num; uint8_t own_virtual_shield_value; - float pos_x; - float pos_y; + + float hero_x; + float hero_y; + float infantry_3_x; + float infantry_3_y; + float infantry_4_x; + float infantry_4_y; + float engineer_x; + float engineer_y; + float pos_angle; float target_pos_x; float target_pos_y; @@ -69,22 +68,19 @@ class AI { GAME_END = 5, } GameProgress; typedef enum { - TO_PATROL_AREA = 0, /* 哨兵巡逻区 */ - TO_SUPPLY = 1, /* 补给区 */ - TO_HIGHWAY = 2, /* 公路区 */ - TO_OUTPOST = 3, /* 前哨站 */ - ROTOR = 4, - - SCANF = 5, - AUTO_AIM = 6, - AFFECTED = 7, /* 反击*/ - - CEASEFIRE = 8, /* 停火 */ - FIRE = 9, - - NOTHING = 10, - CONFIRM_RESURRECTION = 11, /* 确认复活 */ - EXCHANGE_BULLETS = 12, /* 兑换弹丸 */ + START_AUTO_CONTROL = 0, /* 哨兵巡逻区 */ + STOP_AUTO_CONTROL = 1, /* 补给区 */ + + SCANF = 2, + AUTO_AIM = 3, + AFFECTED = 4, /* 反击*/ + + CEASEFIRE = 5, /* 停火 */ + FIRE = 6, + + NOTHING = 7, + CONFIRM_RESURRECTION = 8, /* 确认复活 */ + EXCHANGE_BULLETS = 9, /* 兑换弹丸 */ /* 其他行为暂不考虑 */ } Action; @@ -138,9 +134,7 @@ class AI { /* angle record */ float chassis_yaw_offset_ = 0; - float gimbal_scan_start_angle_; Component::Type::Eulr eulr_; - Component::Type::Quaternion quat_; Component::Type::CycleValue target_scan_angle_ = 0.0; struct { float yaw; /* 偏航角（Yaw angle） */ @@ -164,11 +158,7 @@ class AI { /* Data */ Protocol_DownPackage_t from_host_{}; - - struct { - RefereePckage ref{}; - MCUPckage mcu{}; - } to_host_; + Protocol_UpPackage_t to_host_{}; RefForAI ref_; @@ -177,10 +167,10 @@ class AI { AICtrlAction action_; ScanfMode scanf_mode_ = { - .scanf_yaw_rate = 0.0025f, - .scanf_pit_center = 0.04f, - .scanf_pit_range = 0.22f, - .scanf_pit_omega = 5.0f, + .scanf_yaw_rate = 0.0053f, + .scanf_pit_center = 0.1f, + .scanf_pit_range = 0.5f, + .scanf_pit_omega = 13.0f, }; SentryDecisionData cmd_for_ref_; 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..9e93aad8 --- /dev/null +++ b/src/device/aim/dev_aim.cpp @@ -0,0 +1,149 @@ +#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(arg); + aim->data_ready_.Post(); + }; + + auto rx_idle_callback = [](void* arg){ + AIM *aim = static_cast(arg); + aim->data_ready_.Post(); + }; + + bsp_uart_register_callback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, rx_cplt_callback, + this); + bsp_uart_register_callback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB, rx_idle_callback, this); + + Component::CMD::RegisterController(this->cmd_tp_); + auto ai_thread = [](AIM *aim) { + + auto eulr_sub = Message::Subscriber("imu_eulr"); + + while (1) + { + /* 接收imu和裁判系统数据 */ + eulr_sub.DumpData(aim->eulr_); + + /* 接收上位机数据 */ + aim->StartRecv(); + if (aim->data_ready_.Wait(10)) + { + aim->PraseHost(); + aim->PackCMD(); + } + + /* 发送数据给上位机 */ + aim->PackMCU(); + aim->StartTran(); + } + }; + this->thread_.Create(ai_thread, this, "aim_thread", DEVICE_AI_TASK_STACK_DEPTH, + System::Thread::REALTIME); +} + +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; +} + +float convert_to_minus_pi_to_pi(float 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(&(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..4f057064 --- /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: + + // TODO: 移动到protocol里面 + 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 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/device/motor/dev_mit_motor.cpp b/src/device/motor/dev_mit_motor.cpp index 2b7d7baa..2afcd232 100644 --- a/src/device/motor/dev_mit_motor.cpp +++ b/src/device/motor/dev_mit_motor.cpp @@ -1,7 +1,9 @@ + #include "dev_mit_motor.hpp" #include "bsp_time.h" +// TODO: 加到Param里面 #define P_MIN -12.5f #define P_MAX 12.5f #define V_MIN -45.0f @@ -10,14 +12,11 @@ #define KP_MAX 500.0f #define KD_MIN 0.0f #define KD_MAX 5.0f -#define T_MIN -18.0f -#define T_MAX 18.0f +#define T_MIN -10.0f +#define T_MAX 10.0f using namespace Device; -// static const uint8_t RELAX_CMD[8] = {0X7F, 0XFF, 0X7F, 0XF0, -// 0X00, 0X00, 0X07, 0XFF}; - static const uint8_t ENABLE_CMD[8] = {0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFC}; @@ -32,6 +31,7 @@ MitMotor::MitMotor(const Param ¶m, const char *name) : BaseMotor(name, param.reverse), param_(param) { auto rx_callback = [](Can::Pack &rx, MitMotor *motor) { if ((rx.data[0] & 0x0f) == motor->param_.id) { + motor->last_online_time_ = bsp_time_get_ms(); motor->recv_.Overwrite(rx); } @@ -46,14 +46,14 @@ MitMotor::MitMotor(const Param ¶m, const char *name) (std::string("mit_motor_can") + std::to_string(this->param_.can)) .c_str()); - Can::Subscribe(*MitMotor::mit_tp_[this->param_.can], this->param_.can, 0, - 1); - initd[this->param_.can] = true; } Message::Topic motor_tp(name); - + /* 订阅反馈消息 */ + Can::Subscribe(*MitMotor::mit_tp_[this->param_.can], this->param_.can, + this->param_.feedback_id, 1); + /* 注册回调函数 */ motor_tp.RegisterCallback(rx_callback, this); motor_tp.Link(*this->mit_tp_[this->param_.can]); @@ -64,11 +64,11 @@ bool MitMotor::Update() { while (this->recv_.Receive(pack)) { this->Decode(pack); - last_online_time_ = bsp_time_get_ms(); } return true; } + void MitMotor::SetMit(float out) { clampf(&out, -1.0f, 1.0f); if (this->feedback_.temp > 75.0f) { @@ -82,9 +82,9 @@ void MitMotor::SetMit(float out) { int kd_int = float_to_uint(0.0f, KD_MIN, KD_MAX, 12); int t_int = 0; if (reverse_) { - t_int = -float_to_uint(out * T_MAX, T_MIN, T_MAX, 12); + t_int = -float_to_uint(out, T_MIN, T_MAX, 12); } else { - t_int = float_to_uint(out * T_MAX, T_MIN, T_MAX, 12); + t_int = float_to_uint(out, T_MIN, T_MAX, 12); } Can::Pack tx_buff; @@ -100,29 +100,22 @@ void MitMotor::SetMit(float out) { tx_buff.data[7] = t_int & 0xff; Can::SendStdPack(this->param_.can, tx_buff); - this->Update(); } -void MitMotor::Decode(Can::Pack &rx) { - if (this->param_.id != (rx.data[0] & 0x0f)) { - return; - } +void MitMotor::Decode(Can::Pack &rx) { uint16_t raw_position = rx.data[1] << 8 | rx.data[2]; - uint16_t raw_speed = (rx.data[3] << 4) | (rx.data[4] >> 4); - - uint16_t raw_current = (rx.data[4] & 0x0f) << 8 | rx.data[5]; + uint16_t raw_current = (rx.data[4] & 0x0F) << 8 | rx.data[5]; raw_pos_ = uint_to_float(raw_position, P_MIN, P_MAX, 16); float speed = uint_to_float(raw_speed, V_MIN, V_MAX, 12); - float current = uint_to_float(raw_current, -T_MAX, T_MAX, 12); + float current = uint_to_float(raw_current, T_MIN, T_MAX, 12); this->feedback_.rotational_speed = speed; this->feedback_.rotor_abs_angle = raw_pos_; this->feedback_.torque_current = current; } -/* MIT电机协议只提供pd位置控制 */ void MitMotor::Control(float output) { static_cast(output); XB_ASSERT(false); @@ -155,8 +148,6 @@ void MitMotor::SetPos(float pos) { pos_sp = -pos_sp; } - // clampf(&pos_sp, -param_.max_error, param_.max_error); - pos_sp += this->raw_pos_; while (pos_sp > 4 * M_PI) { @@ -187,29 +178,22 @@ void MitMotor::SetPos(float pos) { tx_buff.data[7] = t_int & 0xff; Can::SendStdPack(this->param_.can, tx_buff); - this->Update(); } void MitMotor::Relax() { Can::Pack tx_buff; - tx_buff.index = this->param_.id; - memcpy(tx_buff.data, RELAX_CMD, sizeof(RELAX_CMD)); - Can::SendStdPack(this->param_.can, tx_buff); + System::Thread::Sleep(50); } bool MitMotor::Enable() { Can::Pack tx_buff; - tx_buff.index = this->param_.id; - memcpy(tx_buff.data, ENABLE_CMD, sizeof(ENABLE_CMD)); - if (Can::SendStdPack(this->param_.can, tx_buff)) { - return true; - } else { - return false; - }; + Can::SendStdPack(this->param_.can, tx_buff); + System::Thread::Sleep(5); + return true; } diff --git a/src/device/motor/dev_mit_motor.hpp b/src/device/motor/dev_mit_motor.hpp index 0aa6ffb3..c6998921 100644 --- a/src/device/motor/dev_mit_motor.hpp +++ b/src/device/motor/dev_mit_motor.hpp @@ -32,6 +32,7 @@ class MitMotor : public BaseMotor { void SetPos(float pos); void SetMit(float out); + bool Enable(); private: diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp index 29159e96..32a8a7aa 100644 --- a/src/device/referee/dev_referee.cpp +++ b/src/device/referee/dev_referee.cpp @@ -102,9 +102,7 @@ Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) { auto ref_trans_thread = [](Referee *ref) { uint32_t last_online_time = bsp_time_get_ms(); - auto ai_sub = Message::Subscriber("sentry_cmd_for_ref"); while (1) { - ai_sub.DumpData(ref->data_from_sentry_); ref->Update(); /* 更新UI */ ref->trans_thread_.SleepUntil(40, last_online_time); } @@ -123,10 +121,10 @@ bool Referee::StartRecv() { void Referee::Prase() { this->ref_data_.status = RUNNING; - const size_t data_length = bsp_uart_get_count(BSP_UART_REF); + const size_t DATA_LENGTH = bsp_uart_get_count(BSP_UART_REF); const uint8_t *index = rxbuf; /* const 保护原始rxbuf不被修改 */ - const uint8_t *const RXBUF_END = rxbuf + data_length; + const uint8_t *const RXBUF_END = rxbuf + DATA_LENGTH; while (index < RXBUF_END) { /* 1.处理帧头 */ @@ -310,7 +308,7 @@ void Referee::Prase() { this->ref_data_.power_heat.chassis_pwr_buff = REF_POWER_BUFF; #endif - memset(rxbuf, 0, data_length); + memset(rxbuf, 0, DATA_LENGTH); } bool Referee::Update() { diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp index 304b509f..4f349a66 100644 --- a/src/device/referee/dev_referee.hpp +++ b/src/device/referee/dev_referee.hpp @@ -13,7 +13,7 @@ #define GAME_HEAT_INCREASE_17MM (10.0f) /* 每发射一颗17mm弹丸增加10热量 */ #define BULLET_SPEED_LIMIT_42MM (16.0) -#define BULLET_SPEED_LIMIT_17MM (30.0) +#define BULLET_SPEED_LIMIT_17MM (25.0) #define GAME_CHASSIS_MAX_POWER_WO_REF 40.0f /* 裁判系统离线时底盘最大功率 */ #define REF_UI_BOX_UP_OFFSET (4) @@ -116,7 +116,7 @@ class Referee { uint16_t red_2; uint16_t red_3; uint16_t red_4; - uint16_t red_5; + uint16_t res_1; uint16_t red_7; uint16_t red_outpose; uint16_t red_base; @@ -124,7 +124,7 @@ class Referee { uint16_t blue_2; uint16_t blue_3; uint16_t blue_4; - uint16_t blue_5; + uint16_t res_2; uint16_t blue_7; uint16_t blue_outpose; uint16_t blue_base; @@ -133,17 +133,15 @@ class Referee { uint32_t blood_supply_before_status : 1; uint32_t blood_supply_inner_status : 1; uint32_t blood_supply_status_RMUL : 1; - uint32_t energy_mech_activation_status : 1; uint32_t energy_mech_small_status : 1; uint32_t energy_mech_big_status : 1; - uint32_t highland_annular : 2; + uint32_t highland_center : 2; - uint32_t highland_trapezium_1 : 2; - uint32_t highland_trapezium_2 : 2; - uint32_t virtual_shield_value : 8; + uint32_t highland_trapezium : 2; uint32_t last_hit_time : 9; - uint32_t last_hit_target : 2; - uint32_t res : 1; + uint32_t last_hit_target : 3; + uint32_t highland_center_status : 2; + uint32_t res : 9; } FieldEvents; /* 0x0101 */ typedef struct __attribute__((packed)) { uint8_t res; @@ -161,10 +159,10 @@ class Referee { typedef struct __attribute__((packed)) { uint8_t countdown; - uint16_t dart_last_target : 2; + uint16_t dart_last_target : 3; uint16_t attack_count : 3; uint16_t dart_target : 2; - uint16_t res : 9; + uint16_t res : 8; } DartCountdown; /* 0x0105 */ typedef struct __attribute__((packed)) { @@ -181,9 +179,9 @@ class Referee { } RobotStatus; /* 0x0201 */ typedef struct __attribute__((packed)) { - uint16_t chassis_volt; - uint16_t chassis_amp; - float chassis_watt; + uint16_t res_1; + uint16_t res_2; + float res_3; uint16_t chassis_pwr_buff; uint16_t launcher_id1_17_heat; uint16_t launcher_id2_17_heat; @@ -202,6 +200,11 @@ class Referee { uint8_t defense_buff; uint8_t vulnerability_buff; uint16_t attack_buff; + uint8_t percent_50_remain_energy : 1; + uint8_t percent_30_remain_energy : 1; + uint8_t percent_15_remain_energy : 1; + uint8_t percent_5_remain_energy : 1; + uint8_t percent_1_remain_energy : 1; } RobotBuff; /* 0x0204 */ typedef struct __attribute__((packed)) { @@ -229,25 +232,30 @@ class Referee { typedef struct __attribute__((packed)) { uint32_t own_base : 1; - uint32_t own_highland_annular : 1; - uint32_t enemy_highland_annular : 1; - uint32_t own_trapezium_R3B3 : 1; - uint32_t enemy_trapezium_R3B3 : 1; - uint32_t own_trapezium_R4B4 : 1; - uint32_t enemy_trapezium_R4B4 : 1; - uint32_t own_energy_mech_activation : 1; - uint32_t own_slope_before : 1; - uint32_t own_slope_after : 1; - uint32_t enemy_slope_before : 1; - uint32_t enemy_slope_after : 1; - uint32_t own_outpose : 1; - uint32_t own_blood_supply : 1; - uint32_t own_sentry_area : 1; - uint32_t enemy_sentry_area : 1; + uint32_t own_highland_center : 1; + uint32_t enemy_highland_center : 1; + uint32_t own_trapezium : 1; + uint32_t enemy_trapezium : 1; + uint32_t own_slope_before_R1B1 : 1; + uint32_t own_slope_after_R1B1 : 1; + uint32_t enemy_slope_before_R4B4 : 1; + uint32_t enemy_slope_after_R4B4 : 1; + uint32_t own_terrain_crossing_up_R2B2 : 1; + uint32_t own_terrain_crossing_down_R2B2 : 1; + uint32_t enemy_terrain_corrssing_up_R2B2 : 1; + uint32_t enemy_terrain_corrssing_down_R2B2 : 1; + uint32_t own_terrain_crossing_up_R3B3 : 1; + uint32_t own_terrain_crossing_down_R3B3 : 1; + uint32_t enemy_terrain_corrssing_up_R3B5 : 1; + uint32_t enemy_terrain_corrssing_down_R3B3 : 1; + uint32_t own_fortress : 1; + uint32_t own_outpost : 1; + uint32_t own_blood_supply_unoverlapping : 1; + uint32_t own_blood_supply_overlapping : 1; uint32_t own_resource : 1; uint32_t enemy_resource : 1; - uint32_t own_exchange_area : 1; - uint32_t res : 13; + uint32_t center_resource_RMUL : 1; + uint32_t res : 8; } RFID; /* 0x0209 */ typedef struct __attribute__((packed)) { @@ -265,16 +273,15 @@ class Referee { float standard_3_y; float standard_4_x; float standard_4_y; - float standard_5_x; - float standard_5_y; + float res_1; + float res_2; } RobotPosForSentry; /* 0x020B */ typedef struct __attribute__((packed)) { - uint8_t mark_hero_progress; - uint8_t mark_engineer_progress; - uint8_t mark_standard_3_progress; - uint8_t mark_standard_4_progress; - uint8_t mark_standard_5_progress; - uint8_t mark_sentry_progress; + uint8_t mark_hero_progress : 1; + uint8_t mark_engineer_progress : 1; + uint8_t mark_standard_3_progress : 1; + uint8_t mark_standard_4_progress : 1; + uint8_t mark_sentry_progress : 1; } RadarMarkProgress; /* 0x020C */ typedef struct __attribute__((packed)) { uint32_t exchanged_bullet_num : 11; @@ -494,7 +501,8 @@ class Referee { struct __attribute__((packed)) { Header frame_header; uint16_t cmd_id; - Referee::InterStudentHeader student_header; //字命令、发送者、接受者 + /* 字命令、发送者、接受者 */ + Referee::InterStudentHeader student_header; } raw; }; @@ -508,9 +516,9 @@ class Referee { } SentryDecisionData; typedef struct __attribute__((packed)) { - Header frame_header; // 0x0301 + Header frame_header; /* 0x0301 */ uint16_t cmd_id; - Referee::InterStudentHeader student_header; //含0x0120 + Referee::InterStudentHeader student_header; /* 含0x0120 */ uint32_t data_cmd; uint16_t crc16; } SentryPack; @@ -593,11 +601,12 @@ class Referee { Message::Event event_; + // NOLINTBEGIN static UIPack ui_pack_; static SentryPack sentry_pack_; static Referee *self_; - SentryDecisionData data_from_sentry_; + // NOLINTEND }; } // namespace Device diff --git a/src/module/chassis/Kconfig b/src/module/chassis/Kconfig index 6229ea47..f81fdf6e 100644 --- a/src/module/chassis/Kconfig +++ b/src/module/chassis/Kconfig @@ -1,4 +1,4 @@ config MODULE_CHASSIS_TASK_STACK_DEPTH int "CHASSIS任务堆栈大小" range 128 4096 - default 384 + default 512 diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp index 6e4d02c6..ebe702f2 100644 --- a/src/module/chassis/mod_chassis.cpp +++ b/src/module/chassis/mod_chassis.cpp @@ -11,10 +11,6 @@ #include "mod_chassis.hpp" -#include - -#include - #include "bsp_time.h" #define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */ @@ -23,6 +19,7 @@ #define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */ #define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */ +#define MOTOR_MAX_ROTATIONAL_SPEED 9600 /* 电机最大转速 */ #if POWER_LIMIT_WITH_CAP /* 保证电容电量宏定义在正确范围内 */ @@ -49,6 +46,8 @@ Chassis::Chassis(Param& param, float control_freq) mode_(Chassis::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_)); @@ -82,6 +81,12 @@ Chassis::Chassis(Param& param, float control_freq) case SET_MODE_INDENPENDENT: chassis->SetMode(INDENPENDENT); break; + case CHANGE_POWER_UP: + chassis->ChangePowerlim(COMMON); + break; + case CHANGE_POWER_DOWN: + chassis->ChangePowerlim(BEAST); + break; default: break; } @@ -91,14 +96,11 @@ Chassis::Chassis(Param& param, float control_freq) Component::CMD::RegisterEvent(event_callback, this, this->param_.EVENT_MAP); - auto chassis_thread = [](Chassis* chassis) { auto raw_ref_sub = Message::Subscriber("referee"); auto cmd_sub = Message::Subscriber("cmd_chassis"); - auto yaw_sub = Message::Subscriber("chassis_yaw"); - auto cap_sub = Message::Subscriber("cap_info"); uint32_t last_online_time = bsp_time_get_ms(); @@ -139,41 +141,7 @@ void Chassis::UpdateFeedback() { this->motor_feedback_[i] = this->motor_[i]->GetSpeed(); } } -template -uint16_t Chassis::MAXSPEEDGET(float power_limit) { - if (param_.get_speed) { - return param_.get_speed(power_limit); - } else { - return 5000; - } -} -template -bool Chassis::LimitChassisOutPower(float power_limit, - float* motor_out, - float* speed_rpm, - uint32_t len) { - if (power_limit < 0.0f) { - return 0; - } - float sum_motor_power = 0.0f; - float motor_power[4]; - for (size_t i = 0; i < len; 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_power[i]; - } - sum_motor_power += this->param_.constant_; - if (sum_motor_power > power_limit) { - for (size_t i = 0; i < len; i++) { - motor_out[i] *= power_limit / sum_motor_power; - } - } - return true; -} template void Chassis::Control() { this->now_ = bsp_time_get(); @@ -182,7 +150,6 @@ void Chassis::Control() { this->last_wakeup_ = this->now_; - max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit); /* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */ /* 计算vx、vy */ switch (this->mode_) { @@ -191,21 +158,48 @@ void Chassis::Control() { this->move_vec_.vy = 0.0f; break; - case Chassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等 - */ + case Chassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等*/ this->move_vec_.vx = this->cmd_.x; this->move_vec_.vy = this->cmd_.y; break; case Chassis::RELAX: - case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 - */ + case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 */ + { + 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 Chassis::ROTOR: { float 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: @@ -220,16 +214,32 @@ void Chassis::Control() { this->move_vec_.wz = this->cmd_.z; break; - case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台 - */ + case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台*/ + { + float direction = 0.0f; this->move_vec_.wz = - this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_); + 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; - - case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转 - */ - this->move_vec_.wz = - this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX); + } + case Chassis::ROTOR: /* 小陀螺模式使底盘根据速度大小调整旋转速度*/ + { /* TODO 改成实际底盘速度 */ + 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: @@ -240,8 +250,6 @@ void Chassis::Control() { /* move_vec -> motor_rpm_set. 通过运动向量计算轮子转速目标值 */ this->mixer_.Apply(this->move_vec_, this->setpoint_.motor_rotational_speed); - /* 根据轮子转速目标值，利用PID计算电机输出值 */ - /* 根据底盘模式计算输出值 */ switch (this->mode_) { case Chassis::BREAK: @@ -252,7 +260,6 @@ void Chassis::Control() { if (ref_.status == Device::Referee::RUNNING) { if (ref_.chassis_pwr_buff > 30) { percentage = 1.0f; - } else { percentage = this->ref_.chassis_pwr_buff / 30.0f; } @@ -262,28 +269,25 @@ void Chassis::Control() { 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_.motor_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_); } for (unsigned i = 0; i < this->mixer_.len_; i++) { if (cap_.online_) { - LimitChassisOutPower(max_power_limit, out_.motor_out, motor_feedback_, - this->mixer_.len_); + LimitChassisOutPower(this->max_power_limit_, out_.motor_out, + motor_feedback_, this->mixer_.len_); this->motor_[i]->Control(out_.motor_out[i]); - } else { + /* 不限功率的兵种使用该底盘时 + * 注意更改chassis_power_limit至电池安全功率 */ LimitChassisOutPower(ref_.chassis_power_limit, out_.motor_out, motor_feedback_, this->mixer_.len_); this->motor_[i]->Control(out_.motor_out[i]); } } - break; } case Chassis::RELAX: /* 放松模式,不输出 */ @@ -297,22 +301,44 @@ void Chassis::Control() { } } +/* 功率限制 */ +template +bool Chassis::LimitChassisOutPower(float power_limit, + float* motor_out, + float* speed_rpm, + uint32_t len) { + if (power_limit < 0.0f) { + return 0; + } + float sum_motor_power = 0.0f; + float motor_power[4]; + for (size_t i = 0; i < len; 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_power[i]; + } + sum_motor_power += this->param_.constant_; + if (sum_motor_power > power_limit) { + for (size_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_power; + } + } + return true; +} + +/* 解析裁判系统数据 */ template void Chassis::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_.status = this->raw_ref_.status; } -template -float Chassis::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 void Chassis::SetMode(Chassis::Mode mode) { if (mode == this->mode_) { @@ -330,108 +356,94 @@ void Chassis::SetMode(Chassis::Mode mode) { this->mode_ = mode; } +/* 功率限制切换 */ +template +void Chassis::ChangePowerlim( + Chassis::Power_Mode power_mode) { + // TODO: 仔细研究正常模式和野兽模式的功率设置 + if (power_mode == this->power_mode_) { + return; + } /* 模式未改变直接返回 */ + if (power_mode == Chassis::COMMON) { + this->max_power_limit_ = ref_.chassis_power_limit; + } else { + this->max_power_limit_ = 100.0f + 100 * 0.2 * this->cap_.percentage_; + } + this->power_mode_ = power_mode; +} + +/* 随机转速小陀螺 结合自身超电与敌方视觉水平使用 */ +template +float Chassis::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 void Chassis::DrawUIStatic( Chassis* 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, - UI_CHAR_DEFAULT_WIDTH, - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H), - "CHAS FLLW INDT ROTR"); + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, + Component::UI::UI_CYAN, UI_DEFAULT_WIDTH * 20, 80, + UI_CHAR_DEFAULT_WIDTH, 1336, 749, "INIT"); Device::Referee::AddUI(chassis->string_); - - float box_pos_left = 0.0f, box_pos_right = 0.0f; - - /* 更新底盘模式选择框 */ - switch (chassis->mode_) { - case FOLLOW_GIMBAL: - box_pos_left = REF_UI_MODE_OFFSET_2_LEFT; - box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT; - break; - case ROTOR: - box_pos_left = REF_UI_MODE_OFFSET_4_LEFT; - box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT; - break; - case INDENPENDENT: - box_pos_left = REF_UI_MODE_OFFSET_3_LEFT; - box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT; - break; - case RELAX: - case BREAK: - default: - box_pos_left = 0.0f; - box_pos_right = 0.0f; - break; - } - - if (box_pos_left != 0.0f && box_pos_right != 0.0f) { - chassis->rectange_.Draw( - "CS", Component::UI::UI_GRAPHIC_OP_ADD, - Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_GREEN, - UI_DEFAULT_WIDTH, - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_left), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_UP_OFFSET), - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_right), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_BOT_OFFSET)); - Device::Referee::AddUI(chassis->rectange_); - } + /* 车头朝向初始化 */ + chassis->cycle_.Draw( + "CS", Component::UI::UI_GRAPHIC_OP_ADD, + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_CYAN, + UI_DEFAULT_WIDTH * 7, + static_cast(Device::Referee::UIGetWidth() * 0.5), + static_cast(Device::Referee::UIGetHeight() * 0.5 + 260), 20); + Device::Referee::AddUI(chassis->cycle_); } +/* 快速刷新 */ template void Chassis::DrawUIDynamic( Chassis* chassis) { - float box_pos_left = 0.0f, box_pos_right = 0.0f; - - /* 更新底盘模式选择框 */ - switch (chassis->mode_) { - case FOLLOW_GIMBAL: - box_pos_left = REF_UI_MODE_OFFSET_2_LEFT; - box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT; - break; - case ROTOR: - box_pos_left = REF_UI_MODE_OFFSET_4_LEFT; - box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT; - break; - case RELAX: - - case BREAK: - - default: - box_pos_left = 0.0f; - box_pos_right = 0.0f; - break; - } - - if (box_pos_left != 0.0f && box_pos_right != 0.0f) { - chassis->rectange_.Draw( + if (chassis->mode_ == chassis->last_mode_) { + /* 车头朝向ui */ + chassis->cycle_.Draw( "CS", Component::UI::UI_GRAPHIC_OP_REWRITE, - Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_GREEN, - UI_DEFAULT_WIDTH, - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_left), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_UP_OFFSET), - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_right), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_BOT_OFFSET)); - Device::Referee::AddUI(chassis->rectange_); + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_CYAN, + UI_DEFAULT_WIDTH * 7, + static_cast(Device::Referee::UIGetWidth() * 0.5 + + 260 * sinf(chassis->yaw_)), + static_cast(Device::Referee::UIGetHeight() * 0.5 + + 260 * cosf(chassis->yaw_)), + 20); + Device::Referee::AddUI(chassis->cycle_); + } else { + chassis->last_mode_ = chassis->mode_; + char mode_ui[5] = "EROR"; + switch (chassis->mode_) { + case RELAX: + strncpy(mode_ui, "RELX", sizeof(mode_ui)); /*1 5!*/ + break; + case BREAK: + strncpy(mode_ui, "BREK", sizeof(mode_ui)); + break; + case FOLLOW_GIMBAL: + strncpy(mode_ui, "FOLW", sizeof(mode_ui)); + break; + case ROTOR: + strncpy(mode_ui, "ROTO", sizeof(mode_ui)); + break; + case INDENPENDENT: + strncpy(mode_ui, "INDP", sizeof(mode_ui)); + break; + default: + break; + } + chassis->string_.Draw("CM", Component::UI::UI_GRAPHIC_OP_REWRITE, + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, + Component::UI::UI_CYAN, UI_DEFAULT_WIDTH * 20, 80, + UI_CHAR_DEFAULT_WIDTH, 1336, 749, mode_ui); + Device::Referee::AddUI(chassis->string_); } } + template class Module::Chassis; diff --git a/src/module/chassis/mod_chassis.hpp b/src/module/chassis/mod_chassis.hpp index 1ac3b21b..7fa9da46 100644 --- a/src/module/chassis/mod_chassis.hpp +++ b/src/module/chassis/mod_chassis.hpp @@ -8,7 +8,6 @@ * @copyright Copyright (c) 2021 * */ - #pragma once #include @@ -29,18 +28,25 @@ class Chassis { public: /* 底盘运行模式 */ typedef enum { - RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ - BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ + RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ + BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ - ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ - INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ + ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ + INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ } Mode; + typedef enum { + COMMON, + BEAST, + } Power_Mode; + typedef enum { SET_MODE_RELAX, SET_MODE_FOLLOW, SET_MODE_ROTOR, SET_MODE_INDENPENDENT, + CHANGE_POWER_UP, + CHANGE_POWER_DOWN, } ChassisEvent; /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 */ @@ -48,26 +54,24 @@ class Chassis { float toque_coefficient_; float speed_2_coefficient_; float out_2_coefficient_; - float constant_; + // TODO: 封装一下 + float constant_; // 功率参数不能封装一下吗 Component::Mixer::Mode type = Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */ - Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */ - - const std::vector EVENT_MAP; + Component::PID::Param xaccl_pid_param{}; /* 加速跟随PID的参数 */ + Component::PID::Param yaccl_pid_param{}; /* y方向加速跟随PID */ std::array actuator_param{}; - std::array motor_param; - float (*get_speed)(float); + const std::vector EVENT_MAP; } Param; typedef struct { Device::Referee::Status status; float chassis_power_limit; float chassis_pwr_buff; - float chassis_watt; } RefForChassis; Chassis(Param ¶m, float control_freq); @@ -78,10 +82,10 @@ class Chassis { void SetMode(Mode mode); + void ChangePowerlim(Power_Mode power_mode_); + bool LimitChassisOutPower(float power_limit, float *motor_out, float *speed, uint32_t len); - uint16_t MAXSPEEDGET(float power_limit); - void PraseRef(); static void DrawUIStatic(Chassis *chassis); @@ -93,8 +97,6 @@ class Chassis { private: Param param_; - float max_motor_rotational_speed_ = 0.0f; - float dt_ = 0.0f; float chassis_current_; @@ -106,6 +108,8 @@ class Chassis { RefForChassis ref_; Mode mode_ = RELAX; + Mode last_mode_ = mode_; + Power_Mode power_mode_ = COMMON; Device::Cap::Info cap_; @@ -116,37 +120,36 @@ class Chassis { /* 底盘设计 */ Component::Mixer mixer_; + float wz_dir_mult_; /* 小陀螺模式旋转方向乘数 */ + + float yaw_; + Component::Type::MoveVector move_vec_; /* 底盘实际的运动向量 */ - float wz_dir_mult_; /* 小陀螺模式旋转方向乘数 */ + Component::PID follow_pid_; /* 跟随云台用的PID */ + Component::PID xaccl_pid_; /* x方向加速跟随PID */ + Component::PID yaccl_pid_; /* y方向加速跟随PID */ - /* PID计算的目标值 */ + float max_power_limit_; struct { float *motor_rotational_speed; /* 电机转速的动态数组，单位：RPM */ } setpoint_; float motor_feedback_[4]; - struct { float motor_out[4]; } out_; - Component::PID follow_pid_; /* 跟随云台用的PID */ - System::Thread thread_; System::Semaphore ctrl_lock_; - float yaw_; Device::Referee::Data raw_ref_; Component::CMD::ChassisCMD cmd_; Component::UI::String string_; - - Component::UI::Line line_; - - Component::UI::Rectangle rectange_; + Component::UI::Cycle cycle_; }; typedef Chassis RMChassis; diff --git a/src/module/dart_gimbal/mod_dart_gimbal.cpp b/src/module/dart_gimbal/mod_dart_gimbal.cpp index 93bc52a4..76d842b1 100644 --- a/src/module/dart_gimbal/mod_dart_gimbal.cpp +++ b/src/module/dart_gimbal/mod_dart_gimbal.cpp @@ -3,24 +3,29 @@ #include "bsp_time.h" #include "comp_cmd.hpp" -#define DGIMBAL_MAXYAW_SPEED (M_2PI * 1.5f) -#define DGIMBAL_MAXPIT_SPEED (M_2PI * 0.1f) +#define DGIMBAL_MAXYAW_SPEED (M_2PI * 0.01f) +#define DGIMBAL_MAXPIT_SPEED (M_2PI * 0.01f) using namespace Module; Dartgimbal::Dartgimbal(Dartgimbal::Param& param, float control_freq) : param_(param), - yaw_actr_(param.yaw_actr, control_freq), - pitch_actr_(this->param_.pitch_actr, control_freq, 500.0f), - yaw_motor_(param.yaw_motor, "dart_yaw") { - auto event_callback = [](GimbalEvent event, Dartgimbal* dart) { + yaw_(this->param_.yaw_param, control_freq, 500.0f), + pitch_(this->param_.pitch_param, control_freq, 500.0f), + ctrl_lock_(true) { + auto event_callback = [](GimbalEvent event, Dartgimbal* dart_gimbal) { + dart_gimbal->ctrl_lock_.Wait(UINT32_MAX); switch (event) { case SET_MODE_RELAX: - dart->setpoint_.eulr_.yaw = 0.5f; - dart->setpoint_.eulr_.pit = 0.0f; + dart_gimbal->SetMode(RELAX); break; - case SET_MODE_ABSOLUTE: + case SET_MODE_STABLE: + dart_gimbal->SetMode(STABLE); + break; + case SET_MODE_CONTROL: + dart_gimbal->SetMode(CONTROL); break; } + dart_gimbal->ctrl_lock_.Post(); }; Component::CMD::RegisterEvent( event_callback, this, this->param_.EVENT_MAP); @@ -33,8 +38,10 @@ Dartgimbal::Dartgimbal(Dartgimbal::Param& param, float control_freq) while (1) { cmd_sub.DumpData(dart_gimbal->cmd_); + dart_gimbal->ctrl_lock_.Wait(UINT32_MAX); dart_gimbal->UpdateFeedback(); dart_gimbal->Control(); + dart_gimbal->ctrl_lock_.Post(); dart_gimbal->thread_.SleepUntil(2, last_online_time); } @@ -45,41 +52,66 @@ Dartgimbal::Dartgimbal(Dartgimbal::Param& param, float control_freq) } void Dartgimbal::UpdateFeedback() { - this->pitch_actr_.UpdateFeedback(); - this->yaw_motor_.Update(); + this->pitch_.UpdateFeedback(); + this->yaw_.UpdateFeedback(); } void Dartgimbal::Control() { this->now_ = bsp_time_get(); - this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); - this->last_wakeup_ = this->now_; - float dart_gimbal_yaw_cmd = 0.0f; - float dart_gimbal_pit_cmd = 0.0f; - - if (this->cmd_.mode == Component::CMD::GIMBAL_RELATIVE_CTRL) { - dart_gimbal_yaw_cmd = - this->cmd_.eulr.yaw * this->dt_ * DGIMBAL_MAXYAW_SPEED; - dart_gimbal_pit_cmd = - this->cmd_.eulr.pit * this->dt_ * DGIMBAL_MAXPIT_SPEED; - this->setpoint_.eulr_.yaw += dart_gimbal_yaw_cmd; - this->setpoint_.eulr_.pit += dart_gimbal_pit_cmd; - } else { - this->setpoint_.eulr_.yaw = this->cmd_.eulr.yaw; - this->setpoint_.eulr_.pit = this->cmd_.eulr.pit; + switch (mode_) { + case RELAX: + this->pitch_.Relax(); + this->yaw_.Relax(); + break; + case STABLE: + this->yaw_.Control( + this->setpoint_.eulr_.x * this->param_.yaw_param.max_range, dt_); + this->pitch_.Control( + this->setpoint_.eulr_.y * this->param_.pitch_param.max_range, dt_); + break; + case CONTROL: + float dart_gimbal_yaw_cmd = 0.0f; + float dart_gimbal_pit_cmd = 0.0f; + + if (this->cmd_.mode == Component::CMD::GIMBAL_RELATIVE_CTRL) { + dart_gimbal_yaw_cmd = + this->cmd_.eulr.yaw * this->dt_ * DGIMBAL_MAXYAW_SPEED; + dart_gimbal_pit_cmd = + this->cmd_.eulr.pit * this->dt_ * DGIMBAL_MAXPIT_SPEED; + this->setpoint_.eulr_.x += dart_gimbal_yaw_cmd; + this->setpoint_.eulr_.y += dart_gimbal_pit_cmd; + } else { + this->setpoint_.eulr_.x = this->cmd_.eulr.yaw; + this->setpoint_.eulr_.y = this->cmd_.eulr.pit; + } + + clampf(&this->setpoint_.eulr_.x, 0.0f, 1.0f); + clampf(&this->setpoint_.eulr_.y, 0.0f, 1.0f); + + this->pitch_.Control( + this->setpoint_.eulr_.y * this->param_.pitch_param.max_range, dt_); + this->yaw_.Control( + this->setpoint_.eulr_.x * this->param_.yaw_param.max_range, dt_); + break; + } +} +void Dartgimbal::SetMode(Mode mode) { + if (mode == this->mode_) { + return; + } + this->mode_ = mode; + switch (mode_) { + case RELAX: + this->setpoint_.eulr_.x = 0.0f; + this->setpoint_.eulr_.y = 0.0f; + break; + case STABLE: + this->setpoint_.eulr_.x = 0.5f; + this->setpoint_.eulr_.y = 0.0f; + break; + case CONTROL: + break; } - - yaw_eulr_ = this->setpoint_.eulr_.yaw; - pit_eulr_ = this->setpoint_.eulr_.pit; - - clampf(&yaw_eulr_, 0.1, 6.18); - - clampf(&pit_eulr_, 0, 1); - - float yaw_out = this->yaw_actr_.Calculate( - yaw_eulr_, this->yaw_motor_.GetSpeed(), - (this->yaw_motor_.GetAngle() + (M_2PI - this->param_.yaw_zero)), dt_); - this->yaw_motor_.Control(yaw_out); - this->pitch_actr_.Control(pit_eulr_ * this->param_.pitch_actr.max_range, dt_); } diff --git a/src/module/dart_gimbal/mod_dart_gimbal.hpp b/src/module/dart_gimbal/mod_dart_gimbal.hpp index dddf5dac..46e20379 100644 --- a/src/module/dart_gimbal/mod_dart_gimbal.hpp +++ b/src/module/dart_gimbal/mod_dart_gimbal.hpp @@ -10,57 +10,49 @@ class Dartgimbal { /* 云台运行模式 */ typedef enum { RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ - ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + STABLE, + CONTROL, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ } Mode; typedef enum { SET_MODE_RELAX, - SET_MODE_ABSOLUTE, + SET_MODE_STABLE, + SET_MODE_CONTROL, } GimbalEvent; typedef struct { - float yaw; - float pitch; - Component::Type::Eulr eulr_; + Component::Type::Vector2 eulr_; } Setpoint; typedef struct { - float dt; - float pos_motor; - Component::Type::CycleValue last_pos; - } Pi; - - typedef struct { - Component::PosActuator::Param yaw_actr; Device::AutoCaliLimitedMech::Param pitch_actr; - Device::RMMotor::Param yaw_motor; + 1>::Param yaw_param; + Device::AutoCaliLimitedMech::Param pitch_param; const std::vector EVENT_MAP; - float yaw_zero; } Param; + Dartgimbal(Param ¶m, float control_freq); void UpdateFeedback(); - void Control(); + void SetMode(Mode mode); private: uint64_t last_wakeup_ = 0; - uint64_t now_ = 0; - float yaw_eulr_; - float pit_eulr_; float dt_ = 0.0f; + Param param_; Setpoint setpoint_; + Mode mode_ = RELAX; - Component::PosActuator yaw_actr_; + Device::AutoCaliLimitedMech yaw_; Device::AutoCaliLimitedMech - pitch_actr_; - - Device::RMMotor yaw_motor_; + pitch_; System::Thread thread_; + System::Semaphore ctrl_lock_; Component::CMD::GimbalCMD cmd_; }; } // namespace Module diff --git a/src/module/dart_launcher/mod_dart_launcher.cpp b/src/module/dart_launcher/mod_dart_launcher.cpp index ba42e39d..0028a5fe 100644 --- a/src/module/dart_launcher/mod_dart_launcher.cpp +++ b/src/module/dart_launcher/mod_dart_launcher.cpp @@ -1,19 +1,14 @@ #include "mod_dart_launcher.hpp" -#include "bsp_time.h" - -#define MAX_FRIC_SPEED (7500.0f) +#define FRIC_NUM 4 +#define FRIC_MAX_SPEED (7500.0f) #define DART_LEN 0.225 using namespace Module; DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) - : ctrl_lock_(true), - param_(param), - rod_actr_(param.rod_actr, control_freq, 500.0f) { - this->setpoint_.rod_position = 0.1f; - - for (int i = 0; i < 4; i++) { + : ctrl_lock_(true), param_(param), rod_(param.rod_, control_freq, 500.0f) { + for (size_t i = 0; i < FRIC_NUM; i++) { fric_actr_[i] = new Component::SpeedActuator(param.fric_actr[i], control_freq); fric_motor_[i] = new Device::RMMotor( @@ -21,130 +16,126 @@ DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) (std::string("dart_fric_") + std::to_string(i)).c_str()); } - auto event_callback = [](DartEvent event, DartLauncher* dart) { + auto event_callback = [](Event event, DartLauncher* dart) { dart->ctrl_lock_.Wait(UINT32_MAX); switch (event) { - case RELAX: - dart->SetFricMode(FRIC_OFF); + case SET_MODE_RELAX: + dart->SetMode(RELAX); break; - case SET_FRIC_OUTOST: - dart->SetFricMode(FRIC_OUTOST); + case SET_MODE_OFF: + dart->SetMode(OFF); break; - case SET_FRIC_BASE: - dart->SetFricMode(FRIC_BASE); + case SET_MODE_ROD: + dart->SetMode(ROD_ON); break; - case SET_FRIC_OFF: - dart->SetFricMode(FRIC_OFF); + case SET_MODE_FRIC: + dart->SetMode(FRIC_ON); break; - case SET_STAY: - dart->SetRodMode(STAY); + case SET_MODE_ON: + dart->SetMode(ON); break; - case RESET_POSITION: - dart->SetRodMode(BACK); + case SET_MODE_STAY: + dart->SetMode(STAY); break; - case FIRE: - dart->SetRodMode(ADVANCE); - default: + case SET_MODE_ADVANCE: + dart->SetMode(ADVANCE); break; } dart->ctrl_lock_.Post(); }; - - Component::CMD::RegisterEvent( - event_callback, this, this->param_.EVENT_MAP); - + Component::CMD::RegisterEvent(event_callback, this, + this->param_.EVENT_MAP); auto thread_fn = [](DartLauncher* dart) { uint32_t last_online_time = bsp_time_get_ms(); - - while (true) { + while (1) { dart->ctrl_lock_.Wait(UINT32_MAX); - - dart->UpdateFeedback(); + dart->Feedback(); dart->Control(); - dart->ctrl_lock_.Post(); - dart->thread_.SleepUntil(2, last_online_time); } }; - this->thread_.Create(thread_fn, this, "dart_launcher", 512, System::Thread::MEDIUM); } - -void DartLauncher::UpdateFeedback() { - this->rod_actr_.UpdateFeedback(); - for (size_t i = 0; i < 4; i++) { +void DartLauncher::Feedback() { + this->rod_.UpdateFeedback(); + for (size_t i = 0; i < FRIC_NUM; i++) { this->fric_motor_[i]->Update(); } } - void DartLauncher::Control() { this->now_ = bsp_time_get(); - this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); - this->last_wakeup_ = this->now_; - switch (this->fric_mode_) { - case FRIC_OUTOST: - this->setpoint_.fric_speed = 0.933f; + if (!relax_) { + this->rod_.Control(this->setpoint_.rod_pos * this->param_.rod_.max_range, + dt_); + for (size_t i = 0; i < FRIC_NUM; i++) { + if (i % 2) { + fric_out_[i] = this->fric_actr_[i]->Calculate( + this->setpoint_.fric_speed2 * FRIC_MAX_SPEED, + this->fric_motor_[i]->GetSpeed(), dt_); + } else { + fric_out_[i] = this->fric_actr_[i]->Calculate( + this->setpoint_.fric_speed1 * FRIC_MAX_SPEED, + this->fric_motor_[i]->GetSpeed(), dt_); + } + } + for (size_t i = 0; i < FRIC_NUM; i++) { + this->fric_motor_[i]->Control(fric_out_[i]); + } + } else { + this->rod_.Relax(); + for (size_t i = 0; i < FRIC_NUM; i++) { + this->fric_motor_[i]->Relax(); + } + } +} +void DartLauncher::SetMode(Mode mode) { + if (mode == this->mode_) { + return; + } + this->mode_ = mode; + switch (this->mode_) { + case RELAX: + this->relax_ = true; + this->setpoint_.fric_speed1 = 0.0f; + this->setpoint_.fric_speed2 = 0.0f; + this->setpoint_.rod_pos = 0.01f; break; - case FRIC_BASE: - this->setpoint_.fric_speed = 1.0f; + case OFF: + this->relax_ = false; + this->setpoint_.fric_speed1 = 0.0f; + this->setpoint_.fric_speed2 = 0.0f; + this->setpoint_.rod_pos = 0.01f; break; - case FRIC_OFF: - this->setpoint_.fric_speed = 0.0f; + case ON: + this->relax_ = false; + this->setpoint_.fric_speed1 = 1.0f; + this->setpoint_.fric_speed1 = 1.0f; + this->setpoint_.rod_pos = 1.0f; + break; + case ROD_ON: + this->relax_ = false; + this->setpoint_.fric_speed1 = 0.0f; + this->setpoint_.fric_speed2 = 0.0f; + this->setpoint_.rod_pos = 1.0f; + break; + case FRIC_ON: + this->relax_ = false; + this->setpoint_.fric_speed1 = 1.0f; + this->setpoint_.fric_speed2 = 1.0f; break; - } - switch (this->rod_mode_) { case ADVANCE: - if (this->fric_mode_ == FRIC_OUTOST || - this->fric_mode_ == - FRIC_BASE) { /*只有在摩擦轮开启状态下飞镖被向前推进*/ - this->setpoint_.rod_position = - (static_cast(rod_position_)) * DART_LEN + 0.1; - } + this->relax_ = false; + this->setpoint_.fric_speed1 = 1.0f; + this->setpoint_.fric_speed2 = 1.0f; + this->setpoint_.rod_pos = static_cast(rod_pos_ * DART_LEN) + 0.1f; + this->rod_pos_ = ((this->rod_pos_ + 1) % 5); break; case STAY: break; - case BACK: - this->setpoint_.rod_position = 0.1f; - if (rod_position_ == 4) { - rod_position_ = 0; - } - } - /* 上电自动校准 和控制电机输出 */ - this->rod_actr_.Control( - this->setpoint_.rod_position * this->param_.rod_actr.max_range, dt_); - - /* fric */ - for (int i = 0; i < 4; i++) { - motor_out_[i] = this->fric_actr_[i]->Calculate( - this->setpoint_.fric_speed * MAX_FRIC_SPEED, - this->fric_motor_[i]->GetSpeed(), dt_); - } - for (int i = 0; i < 4; i++) { - this->fric_motor_[i]->Control(motor_out_[i]); - } -} -/*判断是否切换模式*/ -void DartLauncher::SetRodMode(RodMode mode) { - if (mode == this->rod_mode_) { /* 未更改，return */ - return; - } else { - this->rod_mode_ = mode; - /*如果切换到前进模式，位置向前推进*/ - if (mode == ADVANCE && - (this->fric_mode_ == FRIC_OUTOST || this->fric_mode_ == FRIC_BASE)) { - rod_position_ = ((rod_position_ + 1) % 5); - } - } -} - -void DartLauncher::SetFricMode(FricMode mode) { - if (mode == this->fric_mode_) { /* 未更改，return */ - return; } - this->fric_mode_ = mode; } diff --git a/src/module/dart_launcher/mod_dart_launcher.hpp b/src/module/dart_launcher/mod_dart_launcher.hpp index ad6063a1..65147f26 100644 --- a/src/module/dart_launcher/mod_dart_launcher.hpp +++ b/src/module/dart_launcher/mod_dart_launcher.hpp @@ -1,79 +1,64 @@ -#include - -#include "comp_actuator.hpp" #include "comp_cmd.hpp" #include "dev_mech.hpp" -#include "dev_rm_motor.hpp" #include "module.hpp" namespace Module { class DartLauncher { public: typedef struct { - Device::AutoCaliLimitedMech::Param rod_actr; const std::vector EVENT_MAP; - + Device::AutoCaliLimitedMech::Param rod_; std::array fric_actr; std::array fric_motor; } Param; - - typedef struct { - float rod_position; - float fric_speed; - } Setpoint; - typedef enum { - FRIC_OUTOST, /*摩擦轮前哨战模式*/ - FRIC_BASE, /*摩擦轮基地模式*/ - FRIC_OFF, - } FricMode; typedef enum { - ADVANCE, /* 推进1个单位 */ + RELAX, + OFF, + ROD_ON, + FRIC_ON, + ON, STAY, - BACK, /* 回到初始位置0,1 */ - } RodMode; + ADVANCE, + } Mode; typedef enum { - RELAX, /* 摩擦轮不输出，回到初始位置0.1 */ - SET_FRIC_OFF, - SET_STAY, - RESET_POSITION, /* 回到初始位置0.1 */ - FIRE, /* 发射 */ - SET_FRIC_OUTOST, /*攻击前哨站*/ - SET_FRIC_BASE, /*攻击基地*/ - } DartEvent; /* 事件 */ - + SET_MODE_RELAX, + SET_MODE_OFF, + SET_MODE_ROD, + SET_MODE_FRIC, + SET_MODE_ON, + SET_MODE_STAY, + SET_MODE_ADVANCE, + } Event; + typedef struct { + float rod_pos; + float fric_speed1; + float fric_speed2; + } Setpoint; DartLauncher(Param& param, float control_freq); - - void UpdateFeedback(); - + void Feedback(); void Control(); - void SetRodMode(RodMode mode); - void SetFricMode(FricMode mode); + void SetMode(Mode mode); private: System::Semaphore ctrl_lock_; - - Param& param_; - - FricMode fric_mode_; - RodMode rod_mode_; - + System::Thread thread_; + Param& param_; // NOLINT Setpoint setpoint_; - float dt_ = 0.0f; + float dt_; + uint64_t last_wakeup_; + uint64_t now_; - uint64_t last_wakeup_ = 0; + bool relax_ = true; + int rod_pos_ = 0; + Mode mode_ = RELAX; - uint64_t now_ = 0; - int rod_position_ = 0; /* 丝杆位置0,1,2,3循环 */ + Device::AutoCaliLimitedMech rod_; - Device::AutoCaliLimitedMech - rod_actr_; /* 内含电机、执行器、防堵数据 */ std::array fric_actr_; std::array fric_motor_; - std::array motor_out_; - - System::Thread thread_; + std::array fric_out_; }; } // namespace Module diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index b0b494c8..d8e28e68 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(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(event))); break; } gimbal->ctrl_lock_.Post(); @@ -80,6 +81,22 @@ void Gimbal::UpdateFeedback() { this->yaw_motor_.Update(); this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + + this->yaw_motor_value_ = this->yaw_motor_.GetAngle().Value(); + this->pit_motor_value_ = this->pit_motor_.GetAngle().Value(); +} + +float Gimbal::ChangeAngleRange(float angle) { + angle = fmodf(angle, 2 * M_PI); + if (angle < 0) { + angle += 2 * M_PI; + } + /* 将角度从 [0, 2π] 转换到 [-π, π] */ + if (angle > M_PI) { + angle -= 2 * M_PI; + } + + return angle; } void Gimbal::Control() { @@ -95,7 +112,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 +165,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; } } @@ -161,13 +188,10 @@ void Gimbal::SetMode(Mode mode) { this->pit_actuator_.Reset(); this->yaw_actuator_.Reset(); + /* 切换模式后重置设定值 */ memcpy(&(this->setpoint_.eulr_), &(this->eulr_), - sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ - if (this->mode_ == RELAX) { - if (mode == ABSOLUTE) { - this->setpoint_.eulr_.yaw = this->eulr_.yaw; - } - } + sizeof(this->setpoint_.eulr_)); + 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..a8710271 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,16 @@ class Gimbal { void Control(); + float ChangeAngleRange(float angle); + void SetMode(Mode mode); static void DrawUIStatic(Gimbal *gimbal); static void DrawUIDynamic(Gimbal *gimbal); + void GetSlopeAngle(); + private: uint64_t last_wakeup_ = 0; @@ -80,6 +87,9 @@ class Gimbal { float dt_ = 0.0f; + float yaw_motor_value_; + float pit_motor_value_; + Param param_; Gimbal::Mode mode_ = RELAX; /* 云台模式 */ @@ -92,6 +102,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 +113,13 @@ class Gimbal { System::Semaphore ctrl_lock_; Message::Topic yaw_tp_ = Message::Topic("chassis_yaw"); + Message::Topic eulr_tp_ = Message::Topic("ahrs_eulr"); + Message::Topic quat_tp_ = Message::Topic("ahrs_quat"); float yaw_; + float pit_; + + 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..cdaa9e85 100644 --- a/src/module/helm_chassis/mod_helm_chassis.cpp +++ b/src/module/helm_chassis/mod_helm_chassis.cpp @@ -152,7 +152,6 @@ void HelmChassis::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_.status = this->raw_ref_.status; } template diff --git a/src/module/helm_chassis/mod_helm_chassis.hpp b/src/module/helm_chassis/mod_helm_chassis.hpp index b120d573..bb26f713 100644 --- a/src/module/helm_chassis/mod_helm_chassis.hpp +++ b/src/module/helm_chassis/mod_helm_chassis.hpp @@ -37,10 +37,10 @@ class HelmChassis { typedef enum { RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ - CHASSIS_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ + CHASSIS_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ CHASSIS_6020_FOLLOW_GIMBAL, /*6020电机直接跟随车头方向*/ - ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ - INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ + ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ + INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ } Mode; typedef enum { @@ -72,7 +72,6 @@ class HelmChassis { Device::Referee::Status status; float chassis_power_limit; float chassis_pwr_buff; - float chassis_watt; } RefForChassis; HelmChassis(Param ¶m, float control_freq); diff --git a/src/module/hero_launcher/Kconfig b/src/module/hero_launcher/Kconfig new file mode 100644 index 00000000..aae3a244 --- /dev/null +++ b/src/module/hero_launcher/Kconfig @@ -0,0 +1,4 @@ +config MODULE_HERO_LAUNCHER_TASK_STACK_DEPTH + int "LAUNCHER任务堆栈大小" + range 128 4096 + default 384 diff --git a/src/module/hero_launcher/info.cmake b/src/module/hero_launcher/info.cmake new file mode 100644 index 00000000..cd1b64d7 --- /dev/null +++ b/src/module/hero_launcher/info.cmake @@ -0,0 +1,8 @@ +CHECK_SUB_ENABLE(MODULE_ENABLE module) + +if(${MODULE_ENABLE}) + file(GLOB CUR_SOURCES "${SUB_DIR}/*.cpp") + + SUB_ADD_SRC(CUR_SOURCES) + SUB_ADD_INC(SUB_DIR) +endif() diff --git a/src/module/hero_launcher/mod_hero_launcher.cpp b/src/module/hero_launcher/mod_hero_launcher.cpp new file mode 100644 index 00000000..4514c544 --- /dev/null +++ b/src/module/hero_launcher/mod_hero_launcher.cpp @@ -0,0 +1,362 @@ +#include "mod_hero_launcher.hpp" +#include "bsp_time.h" +using namespace Module; + +#define LAUNCHER_TRIG_SPEED_MAX (8191) + +using namespace Module; + +Launcher::Launcher(Param& param, float control_freq) + : param_(param), ctrl_lock_(true) { + for (size_t i = 0; i < LAUNCHER_ACTR_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()); + } + + for (size_t i = 0; i < LAUNCHER_ACTR_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()); + } + + auto event_callback = [](LauncherEvent event, Launcher* launcher) { + launcher->ctrl_lock_.Wait(UINT32_MAX); + switch (event) { /* 根据event设置模式 */ + case CHANGE_FIRE_MODE_RELAX: + launcher->SetFireMode(static_cast(RELAX)); + break; + case CHANGE_FIRE_MODE_SAFE: + launcher->SetFireMode(static_cast(SAFE)); + break; + case CHANGE_FIRE_MODE_LOADED: + launcher->SetFireMode(static_cast(LOADED)); + break; + case LAUNCHER_START_FIRE: /* 摩擦轮开启条件下，开火控制fire为ture */ + if (launcher->fire_ctrl_.fire_mode_ == LOADED) { + launcher->fire_ctrl_.fire = true; + } + break; + + case CHANGE_TRIG_MODE_SINGLE: + launcher->SetTrigMode(static_cast(SINGLE)); + break; + case CHANGE_TRIG_MODE_BURST: + launcher->SetTrigMode(static_cast(BURST)); + break; + case CHANGE_TRIG_MODE_CONTINUED: + launcher->SetTrigMode(static_cast(CONTINUED)); + break; + case CHANGE_TRIG_MODE_STOP: + launcher->SetTrigMode(static_cast(STOP)); + break; + case CHANGE_TRIG_MODE: + launcher->SetTrigMode(static_cast( + (launcher->fire_ctrl_.trig_mode_ + 1) % CONTINUED)); + break; + default: + break; + } + + launcher->ctrl_lock_.Post(); + }; + + Component::CMD::RegisterEvent( + event_callback, this, this->param_.EVENT_MAP); + + + + auto launcher_thread = [](Launcher* launcher) { + auto ref_sub = Message::Subscriber("referee"); + + uint32_t last_online_time = bsp_time_get_ms(); + + while (1) { + ref_sub.DumpData(launcher->raw_ref_); + + launcher->PraseRef(); + + launcher->ctrl_lock_.Wait(UINT32_MAX); + + launcher->UpdateFeedback(); + launcher->Control(); + + launcher->ctrl_lock_.Post(); + + /* 运行结束，等待下一次唤醒 */ + launcher->thread_.SleepUntil(2, last_online_time); + } + }; + + this->thread_.Create(launcher_thread, this, "launcher_thread", + MODULE_HERO_LAUNCHER_TASK_STACK_DEPTH, + System::Thread::MEDIUM); + +} + +void Launcher::UpdateFeedback() +{ + const float LAST_TRIG_MOTOR_ANGLE = this->trig_motor_[0]->GetAngle(); + + for (size_t i =0; i < LAUNCHER_ACTR_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++) + { + this->trig_motor_[i]->Update(); + } + + const float DELTA_MOTOR_ANGLE = + this->trig_motor_[0]->GetAngle() - LAST_TRIG_MOTOR_ANGLE; + this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio; +} + + +void Launcher::Control() { + this->now_ = bsp_time_get(); + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); + + this->last_wakeup_ = this->now_; + + this->HeatLimit(); + + /* 发弹量设置 */ + uint32_t max_burst = 0; + + switch (this->fire_ctrl_.trig_mode_) + { + case SINGLE: /* 点射开火模式 */ + max_burst = 1; + break; + case BURST: /* 爆发开火模式 */ + max_burst = 5; + break; + case STOP: + max_burst = 0; + break; + case CONTINUED: + max_burst = this->heat_ctrl_.available_shot; + default: + max_burst = 1; + break; + } + /*执行发弹量*/ + switch (this->fire_ctrl_.trig_mode_) { + case SINGLE: /* 点射开火模式 */ + case BURST: /* 爆发开火模式 */ + case STOP: + // case CONTINUED: + + /* 计算是否是第一次按下开火键 */ + this->fire_ctrl_.first_pressed_fire = this->fire_ctrl_.fire && !this->fire_ctrl_.last_fire; + this->fire_ctrl_.last_fire = this->fire_ctrl_.fire; + + /* 设置要发射多少弹丸 */ + if (this->fire_ctrl_.first_pressed_fire && !this->fire_ctrl_.to_launch) + { + this->fire_ctrl_.to_launch = + MIN(max_burst,(this->heat_ctrl_.available_shot - this->fire_ctrl_.launched)); + } + + /* 以下逻辑保证触发后一定会打完预设的弹丸，完成爆发 */ + if (this->fire_ctrl_.launched >= this->fire_ctrl_.to_launch) { + this->fire_ctrl_.launch_delay = UINT32_MAX; + this->fire_ctrl_.launched = 0; + this->fire_ctrl_.to_launch = 0; + this->fire_ctrl_.fire = false; + } else { + this->fire_ctrl_.launch_delay = this->param_.min_launch_delay; + } + break; + + case CONTINUED: + /* 持续开火模式 */ + this->fire_ctrl_.launch_delay = UINT32_MAX; + + if (max_burst > 0) + { + this->fire_ctrl_.launch_delay = this->param_.min_launch_delay; + } + + break; + + default: + break; + } + + /* 根据模式选择是否使用计算出来的值 */ + switch (this->fire_ctrl_.fire_mode_) + { + case RELAX: + case SAFE: + this->fire_ctrl_.bullet_speed = 0.0f; + this->fire_ctrl_.launch_delay = UINT32_MAX; + for (size_t i =0; i < LAUNCHER_ACTR_FRIC_NUM; i++) + {this->setpoint_.fric_rpm_[i]=0.0f;} + break; + case LOADED: + this->setpoint_.fric_rpm_[1] = this->param_.fric_speed_1; + this->setpoint_.fric_rpm_[0] = -this->setpoint_.fric_rpm_[1]; + this->setpoint_.fric_rpm_[2] = this->param_.fric_speed_2; + this->setpoint_.fric_rpm_[3] = -this->setpoint_.fric_rpm_[2]; + + break; + } + + + + /*一个拨弹周期到了之后 */ + //TODO英雄拨弹周期较长 防卡弹执行速度较慢 建议修改 + if ((bsp_time_get_ms() - this->fire_ctrl_.last_launch) >= this->fire_ctrl_.launch_delay) + { + /*如果拨弹盘没转到位 就认为是卡弹*/ + if ((fire_ctrl_.last_trig_angle - trig_angle_) / M_2PI * this->param_.num_trig_tooth < 0.8) + { + /*如果该拨弹盘允许反转*/ + if(this->param_.allow_reverse) + { + if(fire_ctrl_.stall)/*先复位*/ + { + float tmp = this->setpoint_.trig_angle_; + this->setpoint_.trig_angle_ = fire_ctrl_.last_trig_angle; + fire_ctrl_.last_trig_angle = tmp; + this->fire_ctrl_.last_launch = bsp_time_get_ms(); + fire_ctrl_.stall = false; + } + else/*再拨一次试试*/ + { + fire_ctrl_.last_trig_angle = this->setpoint_.trig_angle_; + this->setpoint_.trig_angle_ -= M_2PI / this->param_.num_trig_tooth; + this->fire_ctrl_.last_launch = bsp_time_get_ms(); + fire_ctrl_.stall = true; + } + } + } + else/*正常拨弹*/ + { + fire_ctrl_.last_trig_angle = this->setpoint_.trig_angle_; + this->setpoint_.trig_angle_ -= M_2PI / this->param_.num_trig_tooth; + this->fire_ctrl_.launched++; + this->fire_ctrl_.last_launch = bsp_time_get_ms(); + } + } + + + + + /* 计算摩擦轮和拨弹盘并输出 */ + switch (this->fire_ctrl_.fire_mode_) { + case RELAX: + for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { + this->trig_motor_[i]->Relax(); + } + for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + this->fric_motor_[i]->Relax(); + } + + break; + + case SAFE: + case LOADED: + for (int i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { + /* 控制拨弹电机 */ + float trig_out = this->trig_actuator_[i]->Calculate( + this->setpoint_.trig_angle_, + this->trig_motor_[i]->GetSpeed() / LAUNCHER_TRIG_SPEED_MAX, + this->trig_angle_, this->dt_); + + this->trig_motor_[i]->Control(trig_out); + } + + for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + /* 控制摩擦轮 */ + float fric_out = this->fric_actuator_[i]->Calculate( + this->setpoint_.fric_rpm_[i], this->fric_motor_[i]->GetSpeed(), + this->dt_); + + this->fric_motor_[i]->Control(fric_out); + } + break; + } +} +/* 拨弹盘模式 */ +/* SINGLE,BURST,CONTINUED, */ +void Launcher::SetTrigMode(TrigMode mode) { + if (mode == this->fire_ctrl_.trig_mode_) { + return; + } + + this->fire_ctrl_.trig_mode_ = mode; +} +/* 设置摩擦轮模式 */ +/* RELEX SAFE LOADED三种模式可以选择 */ +void Launcher::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++) { + this->fric_actuator_[i]->Reset(); + } /* reset 所有电机执行器PID等参数 */ + + if (mode == LOADED) { + this->fire_ctrl_.to_launch = 0; + } + + this->fire_ctrl_.fire_mode_ = mode; +} + +void Launcher::HeatLimit() { + if (this->ref_.status == Device::Referee::RUNNING) { + /* 根据机器人型号获得对应数据 */ + if (this->param_.model == LAUNCHER_MODEL_42MM) { + this->heat_ctrl_.heat = this->ref_.power_heat.launcher_42_heat; + this->heat_ctrl_.heat_limit = this->ref_.robot_status.shooter_heat_limit; + this->heat_ctrl_.speed_limit = BULLET_SPEED_LIMIT_42MM; + this->heat_ctrl_.cooling_rate = + this->ref_.robot_status.shooter_cooling_value; + this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_42MM; + } else if (this->param_.model == LAUNCHER_MODEL_17MM) { + this->heat_ctrl_.heat = this->ref_.power_heat.launcher_id1_17_heat; + this->heat_ctrl_.heat_limit = this->ref_.robot_status.shooter_heat_limit; + this->heat_ctrl_.speed_limit = BULLET_SPEED_LIMIT_17MM; + this->heat_ctrl_.cooling_rate = + this->ref_.robot_status.shooter_cooling_value; + this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_17MM; + } + /* 检测热量更新后,计算可发射弹丸（裁判系统数据更新有延迟） */ + if ((this->heat_ctrl_.heat != this->heat_ctrl_.last_heat) || + this->heat_ctrl_.available_shot == 0 || (this->heat_ctrl_.heat == 0)) { + this->heat_ctrl_.available_shot = static_cast( + floorf((this->heat_ctrl_.heat_limit - this->heat_ctrl_.heat) / + this->heat_ctrl_.heat_increase)); + this->heat_ctrl_.last_heat = this->heat_ctrl_.heat; + } + // this->fire_ctrl_.bullet_speed = this->heat_ctrl_.speed_limit; + } else { + /* 裁判系统离线，不启用热量控制 */ + this->heat_ctrl_.available_shot = 10; + // this->fire_ctrl_.bullet_speed = this->param_.default_bullet_speed; + } +} + +void Launcher::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), + sizeof(this->ref_.robot_status)); + memcpy(&(this->ref_.launcher_data), &(this->raw_ref_.launcher_data), + sizeof(this->ref_.launcher_data)); + this->ref_.status = this->raw_ref_.status; +} diff --git a/src/module/hero_launcher/mod_hero_launcher.hpp b/src/module/hero_launcher/mod_hero_launcher.hpp new file mode 100644 index 00000000..b80cd618 --- /dev/null +++ b/src/module/hero_launcher/mod_hero_launcher.hpp @@ -0,0 +1,178 @@ +#pragma once + +#include + +#include "comp_actuator.hpp" +#include "comp_cmd.hpp" +#include "dev_referee.hpp" +#include "dev_rm_motor.hpp" + /* 英雄发射机构代码仅有摩擦轮数量和转速参数有所不同 发射逻辑相同 */ +namespace Module { +class Launcher { + public: + /* 发射器运行模式 */ + typedef enum { + RELAX, /* 放松模式，电机不输出 */ + SAFE, /* 保险模式，电机闭环控制保持静止 */ + LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */ + } FireMode; + + /* 开火模式 */ + typedef enum { + SINGLE, /* 单发开火模式 */ + BURST, /* N爆发开火模式 */ + CONTINUED, /* 持续开火模式 */ + STOP, + } TrigMode; + + + + typedef enum { + CHANGE_FIRE_MODE_RELAX, + CHANGE_FIRE_MODE_SAFE, + CHANGE_FIRE_MODE_LOADED, + LAUNCHER_START_FIRE, /* 开火，拨弹盘开始发弹 */ + + CHANGE_TRIG_MODE_SINGLE, + CHANGE_TRIG_MODE_BURST, + CHANGE_TRIG_MODE_CONTINUED, + CHANGE_TRIG_MODE_STOP, + CHANGE_TRIG_MODE, + } LauncherEvent; + + enum { + LAUNCHER_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */ + LAUNCHER_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */ + LAUNCHER_ACTR_FRIC3_IDX, /* 3号摩擦轮相关的索引值 */ + LAUNCHER_ACTR_FRIC4_IDX, /* 4号摩擦轮相关的索引值 */ + LAUNCHER_ACTR_FRIC_NUM, /* 总共的动作器数量 */ + }; + + enum { + LAUNCHER_ACTR_TRIG_IDX, /* 拨弹电机相关的索引值 */ + LAUNCHER_ACTR_TRIG_NUM, /* 总共的动作器数量 */ + }; + + + typedef enum { + LAUNCHER_MODEL_17MM = 0, /* 17mm发射机构 */ + LAUNCHER_MODEL_42MM, /* 42mm发射机构 */ + } Model; + + typedef struct { + float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */ + float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */ + Model model; /* 发射机构型号 */ + uint32_t min_launch_delay; /* 最小发射间隔(1s/最大射频) */ + bool allow_reverse; /* 拨弹盘是否能反转 取决于机械 */ + float fric_speed_1; /* 一级摩擦轮转速 */ + float fric_speed_2; /* 二级摩擦轮转速 */ + + std::array trig_actr; + std::array fric_actr; + std::array trig_motor; + std::array fric_motor; + + const std::vector EVENT_MAP; + } Param; + + /* 热量控制 */ + struct HeatControl { + float heat; /* 现在热量水平 */ + float last_heat; /* 之前的热量水平 */ + float heat_limit; /* 热量上限 */ + float speed_limit; /* 弹丸初速是上限 */ + float cooling_rate; /* 冷却速率 */ + float heat_increase; /* 每发热量增加值 */ + + uint32_t available_shot; /* 热量范围内还可以发射的数量 */ + }; + + struct FireControl { + bool fire = false; + bool stall = true; + uint32_t last_launch = 0; /* 上次发射器时间 单位：ms */ + bool last_fire = false; /* 上次开火状态 */ + float last_trig_angle = 1.0f; + bool first_pressed_fire; /* 第一次收到开火指令 */ + uint32_t launched; /* 已经发射的弹丸 */ + uint32_t to_launch; /* 计划发射的弹丸 */ + uint32_t launch_delay; /* 弹丸击发延迟 */ + float bullet_speed; /* 弹丸初速度 */ + TrigMode trig_mode_ = SINGLE; /* 发射器模式 */ + FireMode fire_mode_ = RELAX; + }; + + typedef struct { + Device::Referee::Status status; + Device::Referee::PowerHeat power_heat; + Device::Referee::RobotStatus robot_status; + Device::Referee::LauncherData launcher_data; + } RefForLauncher; + + Launcher(Param ¶m, float control_freq); + + + void UpdateFeedback(); + + void Control(); + + void PackOutput(); + + void SetTrigMode(TrigMode mode); + + void SetFireMode(FireMode mode); + + void HeatLimit(); + + float LimitLauncherFreq(); + + void PraseRef(); + + static void DrawUIStatic(Launcher *launcher); + + static void DrawUIDynamic(Launcher *launcher); + + private: + uint64_t last_wakeup_ = 0; + + uint64_t now_ = 0; + + float dt_ = 0.0f; + + float trig_angle_ = 0.0f; + + std::array speed_; /* 方便调试 */ + + Param param_; + + /* PID计算的目标值 */ + struct { + std::array fric_rpm_; /* 摩擦轮电机转速，单位：RPM */ + float trig_angle_ = 0.0f; /* 拨弹电机角度，单位：弧度 */ + } setpoint_; + + HeatControl heat_ctrl_; + FireControl fire_ctrl_; + + std::array trig_actuator_; + std::array fric_actuator_; + + std::array trig_motor_; + std::array fric_motor_; + + RefForLauncher ref_; + + System::Thread thread_; + + System::Semaphore ctrl_lock_; + + Device::Referee::Data raw_ref_; + + Component::UI::String string_; + + Component::UI::Rectangle rectangle_; + + Component::UI::Arc arc_; +}; +} // namespace Module diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index 7df68f5d..23e15780 100644 --- a/src/module/launcher/mod_launcher.cpp +++ b/src/module/launcher/mod_launcher.cpp @@ -3,33 +3,34 @@ #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 +Launcher::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) { launcher->ctrl_lock_.Wait(UINT32_MAX); - switch (event) { /* 根据event设置模式 */ + /* 根据event设置模式 */ + switch (event) { case CHANGE_FIRE_MODE_RELAX: launcher->SetFireMode(static_cast(RELAX)); break; @@ -109,15 +110,16 @@ Launcher::Launcher(Param& param, float control_freq) System::Timer::Create(this->DrawUIDynamic, this, 100); } -void Launcher::UpdateFeedback() { +template +void Launcher::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 +128,8 @@ void Launcher::UpdateFeedback() { this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio; } -void Launcher::Control() { +template +void Launcher::Control() { this->now_ = bsp_time_get(); this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); @@ -236,10 +239,10 @@ 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); @@ -247,8 +250,10 @@ void Launcher::Control() { 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 +262,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(), @@ -279,7 +284,9 @@ void Launcher::Control() { } /* 拨弹盘模式 */ /* SINGLE,BURST,CONTINUED, */ -void Launcher::SetTrigMode(TrigMode mode) { +template +void Launcher::SetTrigMode( + TrigMode mode) { if (mode == this->fire_ctrl_.trig_mode_) { return; } @@ -288,14 +295,16 @@ void Launcher::SetTrigMode(TrigMode mode) { } /* 设置摩擦轮模式 */ /* RELEX SAFE LOADED三种模式可以选择 */ -void Launcher::SetFireMode(FireMode mode) { +template +void Launcher::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 +315,8 @@ void Launcher::SetFireMode(FireMode mode) { this->fire_ctrl_.fire_mode_ = mode; } -void Launcher::HeatLimit() { +template +void Launcher::HeatLimit() { if (this->ref_.status == Device::Referee::RUNNING) { /* 根据机器人型号获得对应数据 */ if (this->param_.model == LAUNCHER_MODEL_42MM) { @@ -340,7 +350,8 @@ void Launcher::HeatLimit() { } } -void Launcher::PraseRef() { +template +void Launcher::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 +361,30 @@ void Launcher::PraseRef() { this->ref_.status = this->raw_ref_.status; } -float Launcher::LimitLauncherFreq() { /* 热量限制计算 */ +template +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.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.0f / this->param_.min_launch_delay; } } -void Launcher::DrawUIStatic(Launcher* launcher) { + +template +void Launcher::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 +523,9 @@ void Launcher::DrawUIStatic(Launcher* launcher) { Device::Referee::AddUI(launcher->arc_); } -void Launcher::DrawUIDynamic(Launcher* launcher) { +template +void Launcher::DrawUIDynamic( + Launcher* launcher) { float box_pos_left = 0.0f, box_pos_right = 0.0f; /* 更新发射器模式选择框 */ @@ -655,3 +669,5 @@ void Launcher::DrawUIDynamic(Launcher* launcher) { 40, 40); Device::Referee::AddUI(launcher->arc_); } + +template class Module::Launcher; 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 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 trig_actr; - std::array - fric_actr; - std::array trig_motor; - std::array fric_motor; + std::array trig_actr; + std::array fric_actr; + std::array trig_param; + std::array fric_param; const std::vector EVENT_MAP; } Param; @@ -165,11 +148,11 @@ class Launcher { HeatControl heat_ctrl_; FireControl fire_ctrl_; - std::array trig_actuator_; - std::array fric_actuator_; + std::array trig_actuator_; + std::array fric_actuator_; - std::array trig_motor_; - std::array fric_motor_; + std::array trig_motor_; + std::array fric_motor_; RefForLauncher ref_; @@ -185,4 +168,5 @@ class Launcher { Component::UI::Arc arc_; }; +typedef class Launcher RMLauncher; } // namespace Module diff --git a/src/module/launcher_drone/mod_launcher_drone.cpp b/src/module/launcher_drone/mod_launcher_drone.cpp index 15d20114..7a4b2b9e 100644 --- a/src/module/launcher_drone/mod_launcher_drone.cpp +++ b/src/module/launcher_drone/mod_launcher_drone.cpp @@ -1,188 +1,199 @@ #include "mod_launcher_drone.hpp" -#include -#include - #include "bsp_pwm.h" -#include "bsp_time.h" -#define LAUNCHER_TRIG_SPEED_MAX (8191) + +#define FRIC_NUM 2 +#define TRIG_NUM 1 +#define TRIG_MAX_SPEED (8191) +#define FRIC_MAX_SPEED (7500.0f) using namespace Module; -UVALauncher::UVALauncher(Param& param, float control_freq) + +DroneLauncher::DroneLauncher(Param& param, float control_freq) : param_(param), ctrl_lock_(true) { - for (size_t i = 0; i < 1; i++) { - this->trig_actuator_.at(i) = + for (size_t i = 0; i < TRIG_NUM; i++) { + this->trig_actr_.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()); } - auto event_callback = [](LauncherEvent event, UVALauncher* launcher) { + auto event_callback = [](Event event, DroneLauncher* drone) { + drone->ctrl_lock_.Wait(UINT32_MAX); switch (event) { - case CHANGE_FIRE_MODE_SAFE: - - launcher->SetTrigMode(SAFE); - launcher->fire_ctrl_.fire = false; /*拨弹盘静止*/ - launcher->fire_ctrl_.fric_on = false; /*摩擦轮静止*/ - + case SET_RELAX: + drone->SetTrigMode(RELAX); + drone->SetFricMode(SAFE); + break; + case CHANGE_FRIC_MODE_LOADED: + drone->SetTrigMode(RELAX); + drone->SetFricMode(LOADED); + break; + case CHANGE_FRIC_MODE_SAFE: + drone->SetTrigMode(RELAX); + drone->SetFricMode(SAFE); break; case CHANGE_TRIG_MODE_SINGLE: - - launcher->fire_ctrl_.fire = true; /*拨弹盘转动*/ - launcher->fire_ctrl_.fric_on = true; /*摩擦轮转动*/ - - launcher->SetTrigMode(SINGLE); + drone->SetTrigMode(SINGLE); break; - case CHANGE_TRIG_MODE_BURST: - - launcher->fire_ctrl_.fire = true; /*拨弹盘转动*/ - launcher->fire_ctrl_.fric_on = true; /*摩擦轮转动*/ - - launcher->SetTrigMode(BURST); + drone->SetTrigMode(BURST); break; - - default: + case CHANGE_TRIG_MODE_CONTINUED: + drone->SetTrigMode(CONTINUED); + break; + case SET_START_FIRE: + drone->SetFricMode(LOADED); + drone->SetTrigMode(BURST); break; } + drone->ctrl_lock_.Post(); }; + Component::CMD::RegisterEvent(event_callback, this, + this->param_.EVENT_MAP); - Component::CMD::RegisterEvent( - event_callback, this, this->param_.EVENT_MAP); - - auto launcher_thread = [](UVALauncher* launcher) { + auto drone_launcher_thread = [](DroneLauncher* drone) { auto ref_sub = Message::Subscriber("referee"); uint32_t last_online_time = bsp_time_get_ms(); - while (1) { - ref_sub.DumpData(launcher->raw_ref_); - - launcher->PraseRef(); - launcher->ctrl_lock_.Wait(UINT32_MAX); - - launcher->FricControl(); - launcher->Control(); - - launcher->ctrl_lock_.Post(); - - /* 运行结束，等待下一次唤醒 */ - launcher->thread_.SleepUntil(2, last_online_time); + ref_sub.DumpData(drone->raw_ref_); + + drone->PraseRef(); + drone->ctrl_lock_.Wait(UINT32_MAX); + drone->Feedback(); + drone->Control(); + drone->ctrl_lock_.Post(); + drone->thread_.SleepUntil(2, last_online_time); } }; - - this->thread_.Create(launcher_thread, this, "launcher_thread", 384, - System::Thread::MEDIUM); + this->thread_.Create(drone_launcher_thread, this, "drone_launcher_thread", + 384, System::Thread::MEDIUM); } -void UVALauncher::SetTrigMode(TrigMode mode) { - if (mode == this->fire_ctrl_.trig_mode_) { - return; - } - - this->fire_ctrl_.trig_mode_ = mode; -} -void UVALauncher::Control() { - const float LAST_TRIG_MOTOR_ANGLE = this->trig_motor_[0]->GetAngle(); - - for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { +void DroneLauncher::Feedback() { + float trig_pos_last = this->trig_motor_[0]->GetAngle(); + for (size_t i = 0; i < TRIG_NUM; i++) { this->trig_motor_[i]->Update(); } + float trig_pos_delta = this->trig_motor_[0]->GetAngle() - trig_pos_last; + this->trig_pos_ += trig_pos_delta / this->param_.trig_gear_ratio; +} - const float DELTA_MOTOR_ANGLE = - this->trig_motor_[0]->GetAngle() - LAST_TRIG_MOTOR_ANGLE; - - this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio; - +void DroneLauncher::Control() { this->now_ = bsp_time_get(); - this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); /*注意单位是US*/ + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); this->last_wakeup_ = this->now_; - /* 根据开火模式计算发射行为 */ - uint32_t max_burst = 0; - switch (this->fire_ctrl_.trig_mode_) { - case SINGLE: /* 点射开火模式 */ - max_burst = 1; - break; - case BURST: /* 爆发开火模式 */ - max_burst = 8; + this->FricControl(); + + this->TrigControl(); +} + +void DroneLauncher::FricControl() { + switch (this->fricmode_) { + case SAFE: + bsp_pwm_start(BSP_PWM_SERVO_A); + bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.02f); + bsp_pwm_start(BSP_PWM_SERVO_B); + bsp_pwm_set_comp(BSP_PWM_SERVO_B, 0.02f); break; - default: - max_burst = 0; + case LOADED: + bsp_pwm_start(BSP_PWM_SERVO_A); + bsp_pwm_start(BSP_PWM_SERVO_B); + bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.08f); + bsp_pwm_set_comp(BSP_PWM_SERVO_B, 0.08f); break; } +} - switch (this->fire_ctrl_.trig_mode_) { - case SINGLE: /* 点射开火模式 */ - case BURST: /* 爆发开火模式 */ - /* 计算是否是第一次按下开火键 */ - this->fire_ctrl_.first_pressed_fire = - this->fire_ctrl_.fire && !this->fire_ctrl_.last_fire; - this->fire_ctrl_.last_fire = 0; - - /* 设置要发射多少弹丸 */ - if (this->fire_ctrl_.first_pressed_fire && !this->fire_ctrl_.to_launch) { - this->fire_ctrl_.to_launch = max_burst; - this->fire_ctrl_.fire = false; +void DroneLauncher::TrigControl() { + switch (this->trigmode_) { + case RELAX: + this->setpoint_.trig_pos = this->trig_pos_; + break; + case SINGLE: + case BURST: + if ((bsp_time_get_ms() - this->last_launch_time_) >= + this->launch_delay_) { + if ((trig_last_pos_ - trig_pos_) / M_2PI * + this->param_.bullet_circle_num > + 0.9) { + if (trig_set_freq_ > 0) { + if (!stall_) { + this->trig_last_pos_ = this->setpoint_.trig_pos; + this->setpoint_.trig_pos -= + M_2PI / this->param_.bullet_circle_num; + } + trig_set_freq_--; + this->last_launch_time_ = bsp_time_get_ms(); + this->stall_ = false; + } + } } - - this->fire_ctrl_.launch_delay = this->param_.min_launch_delay; - break; - case SAFE: - this->fire_ctrl_.launch_delay = UINT32_MAX; + case CONTINUED: + this->continued_rotation_speed_ = + M_2PI / (8.0f * static_cast(this->param_.min_launcher_delay)); + this->setpoint_.trig_pos -= + this->continued_rotation_speed_ * dt_ * 1000.0f; break; - default: + } + switch (this->trigmode_) { + case RELAX: + for (size_t i = 0; i < TRIG_NUM; i++) { + this->trig_motor_[i]->Relax(); + } + break; + case SINGLE: + case BURST: + case CONTINUED: + for (size_t i = 0; i < TRIG_NUM; i++) { + trig_out_ = this->trig_actr_[i]->Calculate( + this->setpoint_.trig_pos, + this->trig_motor_[i]->GetSpeed() / TRIG_MAX_SPEED, this->trig_pos_, + dt_); + this->trig_motor_[i]->Control(trig_out_); + } break; } +} - /* 计算拨弹电机位置的目标值 */ - if ((bsp_time_get_ms() - this->fire_ctrl_.last_launch) >= - this->fire_ctrl_.launch_delay && - this->fire_ctrl_.to_launch) { - if ((fire_ctrl_.last_trig_angle - trig_angle_) / M_2PI * - this->param_.num_trig_tooth > - 0.9) { - fire_ctrl_.last_trig_angle = this->setpoint_.trig_angle_; - /* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */ - this->setpoint_.trig_angle_ -= M_2PI / this->param_.num_trig_tooth; - this->fire_ctrl_.to_launch--; - this->fire_ctrl_.last_launch = bsp_time_get_ms(); - } +void DroneLauncher::SetFricMode(FricMode mode) { + if (this->fricmode_ == mode) { + return; } + this->fricmode_ = mode; +} - for (int i = 0; i < 1; i++) { - /* 控制拨弹电机 */ - float trig_out = this->trig_actuator_[i]->Calculate( - this->setpoint_.trig_angle_, - this->trig_motor_[i]->GetSpeed() / LAUNCHER_TRIG_SPEED_MAX, - this->trig_angle_, this->dt_); - - this->trig_motor_[i]->Control(trig_out); +void DroneLauncher::SetTrigMode(TrigMode mode) { + if (this->trigmode_ == mode) { + return; + } + this->trigmode_ = mode; + switch (this->trigmode_) { + case RELAX: + this->trig_set_freq_ = 0; + this->launch_delay_ = UINT32_MAX; + break; + case SINGLE: + this->trig_set_freq_ = 1; + this->launch_delay_ = this->param_.min_launcher_delay; + break; + case BURST: + this->trig_set_freq_ = 8; + this->launch_delay_ = this->param_.min_launcher_delay; + break; + case CONTINUED: + this->trig_set_freq_ = 0; + this->launch_delay_ = this->param_.min_launcher_delay; + break; } } -void UVALauncher::PraseRef() { - memcpy(&(this->ref_.robot_status), &(this->raw_ref_.robot_status), - sizeof(this->ref_.robot_status)); +void DroneLauncher::PraseRef() { + memcpy(&(this->ref_.robot_status), &(raw_ref_.robot_status), + sizeof(this->ref_.robot_status)); this->ref_.status = this->raw_ref_.status; } -void UVALauncher::FricControl() { - if (/*this->raw_ref_.robot_status.power_launcher_output == 1 &&*/ - this->fire_ctrl_.fric_on == true) { - /*摩擦轮开启模式*/ - bsp_pwm_start(BSP_PWM_SERVO_A); - bsp_pwm_start(BSP_PWM_SERVO_B); - - bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.08f); /*此处最高不得超过0.10f*/ - bsp_pwm_set_comp(BSP_PWM_SERVO_B, 0.08f); - } else { - /*摩擦轮预热模式*/ - bsp_pwm_start(BSP_PWM_SERVO_A); - bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.02f); - bsp_pwm_start(BSP_PWM_SERVO_B); - bsp_pwm_set_comp(BSP_PWM_SERVO_B, 0.02f); - } -} diff --git a/src/module/launcher_drone/mod_launcher_drone.hpp b/src/module/launcher_drone/mod_launcher_drone.hpp index 7fb4576f..485339db 100644 --- a/src/module/launcher_drone/mod_launcher_drone.hpp +++ b/src/module/launcher_drone/mod_launcher_drone.hpp @@ -1,106 +1,90 @@ -#include "module.hpp" -#pragma once - #include "comp_actuator.hpp" #include "comp_cmd.hpp" -#include "comp_filter.hpp" -#include "comp_pid.hpp" #include "dev_referee.hpp" #include "dev_rm_motor.hpp" +#include "module.hpp" + +#define TRIG_NUM 1 +#define FRIC_NUM 2 namespace Module { -class UVALauncher { +class DroneLauncher { public: typedef enum { - SINGLE, /* 单发开火模式 */ - BURST, /* N爆发开火模式 */ + SET_RELAX, + CHANGE_FRIC_MODE_SAFE, + CHANGE_FRIC_MODE_LOADED, + CHANGE_TRIG_MODE_SINGLE, + CHANGE_TRIG_MODE_BURST, + CHANGE_TRIG_MODE_CONTINUED, + SET_START_FIRE, + } Event; + typedef enum { SAFE, + LOADED, + } FricMode; + typedef enum { + RELAX, + SINGLE, + BURST, + CONTINUED, } TrigMode; - typedef struct { - float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */ - - float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */ - - uint32_t min_launch_delay; /* 最小发射间隔(1s/最大射频) */ - - std::array trig_actr; - - std::array trig_motor; - + float trig_gear_ratio; + float bullet_circle_num; + uint32_t min_launcher_delay; + std::array trig_actr; + std::array trig_motor; const std::vector EVENT_MAP; } Param; - - typedef enum { - CHANGE_FIRE_MODE_RELAX, - CHANGE_FIRE_MODE_SAFE, - CHANGE_FIRE_MODE_LOADED, /*摩擦轮*/ - CHANGE_TRIG_MODE_SINGLE, - CHANGE_TRIG_MODE_BURST, - CHANGE_TRIG_MODE, /*拨弹盘*/ - LAUNCHER_START_FIRE, - } LauncherEvent; - - enum { - LAUNCHER_ACTR_TRIG_IDX, /* 拨弹电机相关的索引值 */ - LAUNCHER_ACTR_TRIG_NUM, /* 总共的动作器数量 */ - }; - - struct FireControl { - bool fire = false; /*拨弹盘开启状态*/ - bool fric_on = false; /*摩擦轮开启状态*/ - uint32_t last_launch = 0; /* 上次发射器时间 单位：ms */ - bool last_fire = false; /* 上次开火状态 */ - float last_trig_angle = 1.0f; - bool first_pressed_fire; /* 第一次收到开火指令 */ - uint32_t to_launch = 0; /* 计划发射的弹丸 */ - uint32_t launch_delay; /* 弹丸击发延迟 */ - TrigMode trig_mode_ = SAFE; /* 发射器模式 */ - }; - UVALauncher(Param ¶m, float control_freq); - + typedef struct { + float trig_pos; + } Setpoint; typedef struct { Device::Referee::Status status; - Device::Referee::RobotStatus robot_status; - } RefForLauncher; - - void Control(); - + DroneLauncher(Param& param, float control_freq); void SetTrigMode(TrigMode mode); - - void PraseRef(); - + void SetFricMode(FricMode mode); + void Feedback(); + void Control(); void FricControl(); + void TrigControl(); + void PraseRef(); private: - float last_wakeup_ = 0; - - float now_ = 0; + float dt_; + uint64_t now_; + uint64_t last_wakeup_; - float dt_ = 0; /*单位us*/ + Param& param_; + Setpoint setpoint_; - float trig_angle_ = 0; + TrigMode trigmode_ = RELAX; + FricMode fricmode_ = SAFE; - Param param_; + RefForLauncher ref_; + Device::Referee::Data raw_ref_; - struct { - float trig_angle_ = 0; /* 拨弹电机角度，单位：弧度 */ - } setpoint_; + System::Thread thread_; + System::Semaphore ctrl_lock_; - FireControl fire_ctrl_; + float trig_out_ = 0.0f; - RefForLauncher ref_; + float trig_pos_ = 0.0f; + float trig_last_pos_ = 1.0f; - Device::Referee::Data raw_ref_; + float continued_rotation_speed_; - std::array trig_actuator_; + uint32_t last_launch_time_; + uint32_t launch_delay_; - std::array trig_motor_; + float trig_set_freq_; - System::Thread thread_; + bool stall_ = false; - System::Semaphore ctrl_lock_; + std::array trig_actr_; + std::array trig_motor_; }; } // namespace Module diff --git a/src/module/omni_chassis/mod_omni_chassis.cpp b/src/module/omni_chassis/mod_omni_chassis.cpp index 6f11c638..b86c4ce8 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 -OmniChassis::OmniChassis(Param& param, float control_freq) +nOmniChassis::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::OmniChassis(Param& param, float control_freq) (std::string("Chassis_") + std::to_string(i)).c_str()); } - this->setpoint_.motor_rotational_speed = - reinterpret_cast(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::OmniChassis(Param& param, float control_freq) chassis->ctrl_lock_.Post(); }; - Component::CMD::RegisterEvent( + Component::CMD::RegisterEvent( event_callback, this, this->param_.EVENT_MAP); - auto chassis_thread = [](OmniChassis* chassis) { + auto chassis_thread = [](nOmniChassis* chassis) { auto raw_ref_sub = Message::Subscriber("referee"); auto cmd_sub = Message::Subscriber("cmd_chassis"); @@ -137,36 +137,26 @@ OmniChassis::OmniChassis(Param& param, float control_freq) } template -void OmniChassis::UpdateFeedback() { +void nOmniChassis::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 -uint16_t OmniChassis::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::MAXSPEEDGET(float power_limit) { + if (param_.get_speed) { + return param_.get_speed(power_limit); } else { - speed = 7500; + return 5500; } - return speed; } template -bool OmniChassis::LimitChassisOutput(float power_limit, - float* motor_out, - float* speed_rpm, - uint32_t len) { +bool nOmniChassis::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::LimitChassisOutput(float power_limit, return 0; } template -bool OmniChassis::LimitChassisOutPower(float power_limit, - float* motor_out, - float* speed_rpm, - uint32_t len) { +bool nOmniChassis::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::LimitChassisOutPower(float power_limit, return true; } template -void OmniChassis::Control() { +void nOmniChassis::Control() { this->now_ = bsp_time_get(); this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); @@ -260,27 +238,58 @@ void OmniChassis::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,32 @@ void OmniChassis::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: 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: { 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 +338,11 @@ void OmniChassis::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 +353,39 @@ void OmniChassis::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_); + // TODO: 忘了删了 + 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 +397,38 @@ void OmniChassis::Control() { } template -void OmniChassis::PraseRef() { +void nOmniChassis::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 -float OmniChassis::CalcWz(const float LO, const float HI) { +float nOmniChassis::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 -void OmniChassis::SetMode(OmniChassis::Mode mode) { +void nOmniChassis::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 +438,10 @@ void OmniChassis::SetMode(OmniChassis::Mode mode) { this->mode_ = mode; } +/* 慢速刷新 */ template -void OmniChassis::DrawUIStatic( - OmniChassis* chassis) { +void nOmniChassis::DrawUIStatic( + nOmniChassis* 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 +500,8 @@ void OmniChassis::DrawUIStatic( } template -void OmniChassis::DrawUIDynamic( - OmniChassis* chassis) { +void nOmniChassis::DrawUIDynamic( + nOmniChassis* chassis) { float box_pos_left = 0.0f, box_pos_right = 0.0f; /* 更新底盘模式选择框 */ @@ -519,4 +545,4 @@ void OmniChassis::DrawUIDynamic( Device::Referee::AddUI(chassis->rectange_); } } -template class Module::OmniChassis; +template class Module::nOmniChassis; diff --git a/src/module/omni_chassis/mod_omni_chassis.hpp b/src/module/omni_chassis/mod_omni_chassis.hpp index 38ded65e..579857ac 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 -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 EVENT_MAP; std::array actuator_param{}; std::array 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 *chassis); + static void DrawUIStatic(nOmniChassis *chassis); - static void DrawUIDynamic(OmniChassis *chassis); + static void DrawUIDynamic(nOmniChassis *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 RMChassis; +typedef nOmniChassis RMChassis; } // namespace Module diff --git a/src/module/robot_arm/mod_robot_arm.cpp b/src/module/robot_arm/mod_robot_arm.cpp index 7cba3696..082fb2f8 100644 --- a/src/module/robot_arm/mod_robot_arm.cpp +++ b/src/module/robot_arm/mod_robot_arm.cpp @@ -3,27 +3,27 @@ #include #include "bsp_time.h" -#include "dev_rm_motor.hpp" - using namespace Module; RobotArm::RobotArm(Param& param, float control_freq) + /* 必须按照顺序初始化 */ : param_(param), - mode_(RobotArm::WORK_BOT), + yaw1_actr_(this->param_.yaw1_actr, control_freq), + yaw2_actr_(this->param_.yaw2_actr, control_freq), + pitch1_actr_(this->param_.pitch1_actr, control_freq), + pitch2_actr_(this->param_.pitch2_actr, control_freq), + roll1_actr_(this->param_.roll1_actr, control_freq), roll2_actr_(this->param_.roll2_actr, control_freq), - yaw1_motor_(this->param_.yaw1_motor, "RobotArm_Yaw1"), + yaw2_motor_(this->param_.yaw2_motor, "RobotArm_Yaw2"), pitch1_motor_(this->param_.pitch1_motor, "RobotArm_Pitch1"), pitch2_motor_(this->param_.pitch2_motor, "RobotArm_Pitch2"), roll1_motor_(this->param_.roll1_motor, "RobotArm_Roll1"), - yaw2_motor_(this->param_.yaw2_motor, "RobotArm_Yaw2"), roll2_motor_(this->param_.roll2_motor, "RobotArm_Roll2"), custom_ctrl_(param.cust_ctrl), ctrl_lock_(true) { memset(&(this->cmd_), 0, sizeof(this->cmd_)); - memset(&(this->setpoint_), 0, sizeof(this->setpoint_)); - auto event_callback = [](RobotArmEvent event, RobotArm* robotarm) { robotarm->ctrl_lock_.Wait(UINT32_MAX); @@ -40,9 +40,16 @@ RobotArm::RobotArm(Param& param, float control_freq) case SET_MODE_WORK_BOT: robotarm->SetMode(WORK_BOT); break; + case SET_MODE_XIKUANG: + // TODO: 编译错误，不存在的枚举 + robotarm->SetMode(XIKUANG); + break; case SET_MODE_SAFE: robotarm->SetMode(SAFE); break; + case SET_MODE_CUSTOM_CTRL: + robotarm->SetMode(WORK_CUSTOM_CTRL); + break; default: break; } @@ -55,15 +62,15 @@ RobotArm::RobotArm(Param& param, float control_freq) auto robotarm_thread = [](RobotArm* robotarm) { auto cmd_sub = Message::Subscriber("cmd_gimbal"); - + // robotarm->DamiaoSetAble();不行 uint32_t last_online_time = bsp_time_get_ms(); - while (1) { cmd_sub.DumpData(robotarm->cmd_); + robotarm->ctrl_lock_.Wait(UINT32_MAX); robotarm->DamiaoSetAble(); - robotarm->UpdateFeedback(); robotarm->Control(); + robotarm->ctrl_lock_.Post(); robotarm->thread_.SleepUntil(2, last_online_time); @@ -74,187 +81,139 @@ RobotArm::RobotArm(Param& param, float control_freq) System::Thread::MEDIUM); } -void RobotArm::DamiaoSetAble() { - if (this->yaw1_able_ == 0) { - this->yaw1_motor_.Disable(); - } else { - this->yaw1_motor_.Enable(); - }; - if (this->pitch1_able_ == 0) { - this->pitch1_motor_.Disable(); - } else { - this->pitch1_motor_.Enable(); - }; - if (this->pitch2_able_ == 0) { - this->pitch2_motor_.Disable(); - } else { +void RobotArm:: + DamiaoSetAble() { // 这个函数不是使能函数，而是根据0,1设置电机使能状态//配合relax函数，兼职完美。 + + if (this->state_.motor_last == 0 && this->state_.motor_current == 1) { this->pitch2_motor_.Enable(); - }; - if (this->roll1_able_ == 0) { - this->roll1_motor_.Disable(); - } else { + this->pitch1_motor_.Enable(); this->roll1_motor_.Enable(); - }; - if (this->yaw2_able_ == 0) { - this->yaw2_motor_.Disable(); - } else { + this->yaw1_motor_.Enable(); this->yaw2_motor_.Enable(); - }; + this->state_.motor_last = 1; + } + if (this->state_.motor_last == 1 && this->state_.motor_current == 0) { + this->pitch2_motor_.Relax(); + this->pitch1_motor_.Relax(); + this->roll1_motor_.Relax(); + this->yaw1_motor_.Relax(); + this->yaw2_motor_.Relax(); + this->state_.motor_last = 0; + } } -void RobotArm::UpdateFeedback() { - this->yaw1_motor_.Update(); - this->pitch1_motor_.Update(); - this->pitch2_motor_.Update(); - this->roll1_motor_.Update(); - this->yaw2_motor_.Update(); - this->roll2_motor_.Update(); +void RobotArm::DMable() { + this->pitch1_motor_.Relax(); + this->pitch2_motor_.Relax(); + this->yaw2_motor_.Relax(); + this->yaw1_motor_.Relax(); + this->roll1_motor_.Relax(); } void RobotArm::Control() { this->now_ = bsp_time_get(); - + /* 时间差 */ this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); this->last_wakeup_ = this->now_; - // this->setpoint_.yaw1_theta_ = this->cmd_.theta.yaw1; - // this->setpoint_.pitch1_theta_ = this->cmd_.theta.pitch1; - // this->setpoint_.pitch2_theta_ = this->cmd_.theta.pitch2; - // this->setpoint_.pitch3_theta_ = this->cmd_.theta.pitch3; - // this->setpoint_.yaw2_theta_ = this->cmd_.theta.yaw2; - // this->setpoint_.roll_theta_ = this->cmd_.theta.roll; - float pit_cmd = 0.0f; float yaw_cmd = 0.0f; - yaw_cmd = this->cmd_.eulr.yaw * this->dt_; pit_cmd = this->cmd_.eulr.pit * this->dt_; - /*电机限幅*/ - // clampf(&(this->setpoint_.yaw1_theta_), this->param_.limit.yaw1_min, - // this->param_.limit.yaw1_max); - // clampf(&(this->setpoint_.pitch1_theta_), this->param_.limit.pitch1_min, - // this->param_.limit.pitch1_max); - // clampf(&(this->setpoint_.pitch2_theta_), this->param_.limit.pitch2_min, - // this->param_.limit.pitch2_max); - // clampf(&(this->setpoint_.roll1_theta_), this->param_.limit.roll1_min, - // this->param_.limit.roll1_max); - // clampf(&(this->setpoint_.yaw2_theta_), this->param_.limit.yaw2_min, - // this->param_.limit.yaw2_max); - - // this->yaw1_motor_.SetPosSpeed(this->setpoint_.yaw1_theta_, 0.1f); - // this->pitch1_motor_.SetPosSpeed(this->setpoint_.pitch1_theta_, 0.3f); - // this->pitch2_motor_.SetPosSpeed(this->setpoint_.pitch2_theta_, 0.3f); - // this->roll1_motor_.SetPosSpeed(this->setpoint_.roll1_theta_, 0.3f); - // this->yaw2_motor_.SetPosSpeed(this->setpoint_.yaw2_theta_, 0.3f); - - // float roll2_out = this->roll2_actr_.Calculate( - // this->setpoint_roll2_, this->roll2_motor_.GetSpeed(), - // this->roll2_motor_.GetAngle(), this->dt_); - // this->roll2_motor_.Control(roll2_out); - switch (this->mode_) { - case RobotArm::WORK_BOT: { - this->setpoint_.yaw1_theta_ += yaw_cmd; - this->setpoint_.pitch1_theta_ += pit_cmd; - - clampf(&(this->setpoint_.yaw1_theta_), this->param_.limit.yaw1_min, - this->param_.limit.yaw1_max); - clampf(&(this->setpoint_.pitch1_theta_), this->param_.limit.pitch1_min, - this->param_.limit.pitch1_max); - - this->yaw1_motor_.SetPosSpeed(this->setpoint_.yaw1_theta_, 0.1f); - this->pitch1_motor_.SetPosSpeed(this->setpoint_.pitch1_theta_, 0.3f); + case RobotArm::WORK_TOP: { + this->setpoint_.yaw1_theta_ = Component::Type::CycleValue( + this->setpoint_.yaw1_theta_ + yaw_cmd * 2.0f); + this->setpoint_.yaw1_out_ = this->yaw1_actr_.Calculate( + this->setpoint_.yaw1_theta_, this->yaw1_motor_.raw_speed, + this->yaw1_motor_.raw_pos_, this->dt_); + this->yaw1_motor_.SetMit(this->setpoint_.yaw1_out_); + + this->setpoint_.pitch1_theta_ = Component::Type::CycleValue( + this->setpoint_.pitch1_theta_ + pit_cmd * 2.0f); + this->setpoint_.pitch1_out_ = this->pitch1_actr_.Calculate( + this->setpoint_.pitch1_theta_, this->pitch1_motor_.raw_speed, + this->pitch1_motor_.raw_pos_, this->dt_); + this->pitch1_motor_.SetMit(this->setpoint_.pitch1_out_); break; } case RobotArm::WORK_MID: { - this->setpoint_.roll1_theta_ += yaw_cmd; - this->setpoint_.pitch2_theta_ += pit_cmd; + this->setpoint_.pitch2_theta_ = Component::Type::CycleValue( + this->setpoint_.pitch2_theta_ + pit_cmd * 2.0f); + this->setpoint_.pitch2_out_ = this->pitch2_actr_.Calculate( + this->setpoint_.pitch2_theta_, this->pitch2_motor_.raw_speed, + this->pitch2_motor_.raw_pos_, this->dt_); + this->pitch2_motor_.SetMit(this->setpoint_.pitch2_out_); - clampf(&(this->setpoint_.pitch2_theta_), this->param_.limit.pitch2_min, - this->param_.limit.pitch2_max); - - this->pitch2_motor_.SetPosSpeed(this->setpoint_.pitch2_theta_, 0.3f); - this->roll1_motor_.SetPosSpeed(this->setpoint_.roll1_theta_, 0.3f); break; } - case RobotArm::WORK_TOP: { - this->setpoint_.yaw2_theta_ += pit_cmd; - this->setpoint_roll2_ = - Component::Type::CycleValue(this->setpoint_roll2_ + yaw_cmd); - - clampf(&(this->setpoint_.yaw2_theta_), this->param_.limit.yaw2_min, - this->param_.limit.yaw2_max); - - this->yaw2_motor_.SetPosSpeed(this->setpoint_.yaw2_theta_, 0.3f); - - float roll2_out = this->roll2_actr_.Calculate( + case RobotArm::WORK_BOT: { + this->setpoint_.yaw2_theta_ = Component::Type::CycleValue( + this->setpoint_.yaw2_theta_ + yaw_cmd * 5.0f); + this->setpoint_.yaw2_out_ = this->yaw2_actr_.Calculate( + this->setpoint_.yaw2_theta_, this->yaw2_motor_.raw_speed, + this->yaw2_motor_.raw_pos_, this->dt_); + this->yaw2_motor_.SetMit(this->setpoint_.yaw2_out_); + + this->setpoint_.roll1_theta_ += pit_cmd * 3.0f; + clampf(&(this->setpoint_.roll1_theta_), this->param_.limit.roll1_min, + this->param_.limit.roll1_max); + this->roll1_motor_.SetPos(this->setpoint_.roll1_theta_); + + this->roll2_speed_ = this->roll2_actr_.Calculate( this->setpoint_roll2_, this->roll2_motor_.GetSpeed(), this->roll2_motor_.GetAngle(), this->dt_); - this->roll2_motor_.Control(roll2_out); - + this->roll2_motor_.Control(roll2_speed_); break; } case RobotArm::SAFE: { break; } - case RobotArm::RELAX: - this->yaw1_able_ = 0; - this->pitch1_able_ = 0; - this->pitch2_able_ = 0; - this->roll1_able_ = 0; - this->yaw2_able_ = 0; - this->roll2_motor_.Relax(); + case RobotArm::RELAX: { + if (this->state_.initflag) { + /* 上电先读取角度 */ + this->DMable(); + this->state_.initflag = 0; + + for (int i = 0; i < 500; i++) { + this->setpoint_.yaw1_theta_ = this->yaw1_motor_.raw_pos_; + this->setpoint_.pitch1_theta_ = this->pitch1_motor_.raw_pos_; + this->setpoint_.pitch2_theta_ = this->pitch2_motor_.raw_pos_; + this->setpoint_.yaw2_theta_ = this->yaw2_motor_.raw_pos_; + this->setpoint_.roll1_theta_ = this->roll1_motor_.raw_pos_; + } + } break; - case RobotArm::WORK_CUSTOM_CTRL: - if (custom_ctrl_.online_) { - this->setpoint_.yaw1_theta_ = custom_ctrl_.data_.angle[0]; - this->setpoint_.pitch1_theta_ = custom_ctrl_.data_.angle[1]; - - clampf(&(this->setpoint_.yaw1_theta_), this->param_.limit.yaw1_min, - this->param_.limit.yaw1_max); - clampf(&(this->setpoint_.pitch1_theta_), this->param_.limit.pitch1_min, - this->param_.limit.pitch1_max); - - this->yaw1_motor_.SetPosSpeed(this->setpoint_.yaw1_theta_, 0.1f); - this->pitch1_motor_.SetPosSpeed(this->setpoint_.pitch1_theta_, 0.3f); - - this->setpoint_.roll1_theta_ = custom_ctrl_.data_.angle[2]; - this->setpoint_.pitch2_theta_ = custom_ctrl_.data_.angle[3]; - - clampf(&(this->setpoint_.pitch2_theta_), this->param_.limit.pitch2_min, - this->param_.limit.pitch2_max); - - this->pitch2_motor_.SetPosSpeed(this->setpoint_.pitch2_theta_, 0.3f); - this->roll1_motor_.SetPosSpeed(this->setpoint_.roll1_theta_, 0.3f); - - this->setpoint_.yaw2_theta_ = custom_ctrl_.data_.angle[4]; - this->setpoint_roll2_ = custom_ctrl_.data_.angle[5]; - - clampf(&(this->setpoint_.yaw2_theta_), this->param_.limit.yaw2_min, - this->param_.limit.yaw2_max); - - this->yaw2_motor_.SetPosSpeed(this->setpoint_.yaw2_theta_, 0.3f); - - float roll2_out = this->roll2_actr_.Calculate( - this->setpoint_roll2_, this->roll2_motor_.GetSpeed(), - this->roll2_motor_.GetAngle(), this->dt_); - this->roll2_motor_.Control(roll2_out); - } else { - this->yaw1_able_ = 0; - this->pitch1_able_ = 0; - this->pitch2_able_ = 0; - this->roll1_able_ = 0; - this->yaw2_able_ = 0; - this->roll2_motor_.Relax(); + } + case RobotArm::WORK_CUSTOM_CTRL: { + int i = 0; + for (i = 0; i < 6; i++) { + if (custom_ctrl_.data_.angle[i] < 0.00000000f) { + this->buffer_.current[i] = 0.000000f; + + } else if (custom_ctrl_.data_.angle[i] > M_2PI) { + // TODO: 不能这么搞 + this->buffer_.current[i] = 6.2831; + } else { + this->buffer_.current[i] = custom_ctrl_.data_.angle[i]; + } } + this->setpoint_.yaw1_theta_ = + Component::Type::CycleValue(this->buffer_.current[5] - 2.0f); + this->setpoint_.yaw1_out_ = this->yaw1_actr_.Calculate( + this->setpoint_.yaw1_theta_, this->yaw1_motor_.raw_speed, + this->yaw1_motor_.raw_pos_, this->dt_); + this->yaw1_motor_.SetMit(this->setpoint_.yaw1_out_); break; - // default: - // XB_ASSERT(false); - // return; + } + + default: + XB_ASSERT(false); + return; } } @@ -262,26 +221,26 @@ void RobotArm::SetMode(RobotArm::Mode mode) { if (mode == this->mode_) { return; } + if (mode != WORK_CUSTOM_CTRL) { + /* 切换到其他模式，重置is_first */ + this->state_.is_first = 1; + } if (mode == WORK_BOT) { - this->yaw1_able_ = 1; - this->pitch1_able_ = 1; - this->pitch2_able_ = 1; - this->roll1_able_ = 1; - this->yaw2_able_ = 1; + this->state_.motor_current = 1; } if (mode == WORK_MID) { - this->yaw1_able_ = 1; - this->pitch1_able_ = 1; - this->pitch2_able_ = 1; - this->roll1_able_ = 1; - this->yaw2_able_ = 1; + this->state_.motor_current = 1; } if (mode == WORK_TOP) { - this->yaw1_able_ = 1; - this->pitch1_able_ = 1; - this->pitch2_able_ = 1; - this->roll1_able_ = 1; - this->yaw2_able_ = 1; + this->state_.motor_current = 1; } + if (mode == SAFE) { + this->state_.motor_current = 1; + } + if (mode == RELAX) { + this->state_.motor_current = 0; + this->state_.xipan_state_ = 0; + } + this->mode_ = mode; } diff --git a/src/module/robot_arm/mod_robot_arm.hpp b/src/module/robot_arm/mod_robot_arm.hpp index 154bf9b4..fa99d1ff 100644 --- a/src/module/robot_arm/mod_robot_arm.hpp +++ b/src/module/robot_arm/mod_robot_arm.hpp @@ -1,13 +1,13 @@ #include #include +#include "bsp_gpio.h" +#include "bsp_pwm.h" #include "comp_actuator.hpp" #include "comp_cmd.hpp" #include "comp_pid.hpp" -#include "dev_damiaomotor.hpp" -#include "dev_motor.hpp" -// #include "dev_referee.hpp" #include "dev_custom_controller.hpp" +#include "dev_mit_motor.hpp" #include "dev_rm_motor.hpp" #include "module.hpp" @@ -24,24 +24,36 @@ class RobotArm { SAFE, } Mode; + // TODO: pinyin typedef enum { SET_MODE_RELAX, SET_MODE_WORK_TOP, SET_MODE_WORK_MID, SET_MODE_WORK_BOT, + SET_MODE_CUSTOM_CTRL, SET_MODE_SAFE, + SET_MODE_XIKUANG, + SET_MODE_YINKUANG, + SET_MODE_SAVE1, + SET_MODE_SAVE2, + SET_MODE_SKUANG, + SET_MODE_QKONE, + SET_MODE_QKTWO, } RobotArmEvent; typedef struct Param { const std::vector EVENT_MAP; - + Component::PosActuator::Param yaw1_actr; + Component::PosActuator::Param yaw2_actr; + Component::PosActuator::Param pitch1_actr; + Component::PosActuator::Param pitch2_actr; + Component::PosActuator::Param roll1_actr; Component::PosActuator::Param roll2_actr; - - Device::DamiaoMotor::Param yaw1_motor; - Device::DamiaoMotor::Param pitch1_motor; - Device::DamiaoMotor::Param pitch2_motor; - Device::DamiaoMotor::Param roll1_motor; - Device::DamiaoMotor::Param yaw2_motor; + Device::MitMotor::Param yaw1_motor; + Device::MitMotor::Param yaw2_motor; + Device::MitMotor::Param pitch1_motor; + Device::MitMotor::Param pitch2_motor; + Device::MitMotor::Param roll1_motor; Device::RMMotor::Param roll2_motor; struct { @@ -53,18 +65,19 @@ class RobotArm { float pitch2_min; float yaw2_max; /*小yaw,-90,+90*/ float yaw2_min; - } limit; + float roll1_max; /*roll1,0,360*/ + float roll1_min; + } limit; Device::CustomController::Param cust_ctrl; } Param; RobotArm(Param ¶m, float control_freq); - void UpdateFeedback(); - void Control(); void DamiaoSetAble(); + void DMable(); void SetMode(Mode mode); @@ -77,40 +90,61 @@ class RobotArm { uint64_t now_ = 0; - Mode mode_ = WORK_BOT; - + Mode mode_ = RELAX; + Component::PosActuator yaw1_actr_; + Component::PosActuator yaw2_actr_; + Component::PosActuator pitch1_actr_; + Component::PosActuator pitch2_actr_; + Component::PosActuator roll1_actr_; Component::PosActuator roll2_actr_; - Device::DamiaoMotor yaw1_motor_; - Device::DamiaoMotor pitch1_motor_; - Device::DamiaoMotor pitch2_motor_; - Device::DamiaoMotor roll1_motor_; - Device::DamiaoMotor yaw2_motor_; + Device::MitMotor yaw1_motor_; + Device::MitMotor yaw2_motor_; + Device::MitMotor pitch1_motor_; + Device::MitMotor pitch2_motor_; + Device::MitMotor roll1_motor_; Device::RMMotor roll2_motor_; Device::CustomController custom_ctrl_; - - bool yaw1_able_ = 1; - bool pitch1_able_ = 1; - bool pitch2_able_ = 1; - bool roll1_able_ = 1; - bool yaw2_able_ = 1; + struct { + float yaw1_theta_ = 0.019f; + float pitch1_theta_ = 0; //-2.634f; + float pitch2_theta_ = 0; // 2.924f; + float roll1_theta_ = 0.0f; + float yaw2_theta_ = 3.298f; // 这样的花，他是平的。 + + float yaw1_out_ = 0; + float pitch1_out_ = 0; + float pitch2_out_ = 0; + float roll1_out_ = 0; + float yaw2_out_ = 0; + } setpoint_; + struct { + bool motor_last = 0; + bool motor_current = 0; + bool xipan_state_ = 0; + bool save1 = 0; // 存矿功能 + bool save2 = 0; + bool initflag = 1; // 上电时u就是1,给电机发送一次数据。得到反馈值。 + bool is_first = 1; // 自定义控制器第一次传角度标志。 + } state_; // 状态汇总 System::Thread thread_; + struct { + float current[6]; + float prev[6]; + float err[6]; + float angle[6]; - System::Semaphore ctrl_lock_; + } buffer_; // 自定义控制器缓冲区 - struct { - float yaw1_theta_ = 0; - float pitch1_theta_ = 0; - float pitch2_theta_ = 0; - float roll1_theta_ = 0; - float yaw2_theta_ = 0; - } setpoint_; /*用于接收控制器发来的各关节角度*/ + System::Semaphore ctrl_lock_; - float setpoint_roll2_; + float setpoint_roll2_ = 0.0f; + float roll2_speed_ = 0.0f; + uint32_t last_time_ = 0; + // uint64_t prev_time_ = 0; // 用于自定义控制器的计时器 Component::CMD::GimbalCMD cmd_; }; - } // namespace Module diff --git a/src/robot/dart/robot.cpp b/src/robot/dart/robot.cpp index e7511d0c..fb249448 100644 --- a/src/robot/dart/robot.cpp +++ b/src/robot/dart/robot.cpp @@ -1,126 +1,205 @@ #include "robot.hpp" #include - -#include "comp_actuator.hpp" -#include "dev_rm_motor.hpp" +#include using namespace Robot; /* clang-format off */ -Dart::Param param = { - .dart = { - .rod_actr = { - .stall_detect = { - .speed_thld = 500.0f, - .current_thld = 1.2f, - .stop_current_thld = 2.5f, - .temp_thld = 40.0f, - .timeout = 0.1f +Dart::Param param={ + .gimbal={ + .yaw_param={ + .stall_detect={ + .speed_thld=500.0f, + .current_thld=1.2f, + .stop_current_thld=2.5f, + .temp_thld=40.0f, + .timeout=0.1f }, - - .pos_actuator = { + .pos_actuator={ Component::PosActuator::Param{ - .speed = { - .k = 0.00005f, - .p = 1.0f, - .i = 0.8f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.5f, - .d_cutoff_freq = -1.0f, - .cycle = false, + .speed={ + .k=0.00005f, + .p=1.0f, + .i=0.8f, + .d=0.0f, + .i_limit=0.5f, + .out_limit=0.5f, + .d_cutoff_freq=-1.0f, + .cycle=false, }, - - .position = { - .k = 2000.0f, - .p = 1.0f, - .i = 0.6f, - .d = 0.0f, - .i_limit = 4000.0f, - .out_limit = 8000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, + .position={ + .k=2000.0f, + .p=0.8f, + .i=0.0f, + .d=0.0f, + .i_limit=4000.0f, + .out_limit=8000.0f, + .d_cutoff_freq=-1.0f, + .cycle=false, }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, + .in_cutoff_freq=10.0f, + .out_cutoff_freq=30.0f, }, }, - - .motor_param = { + .motor_param={ Device::RMMotor::Param{ - .id_feedback = 0x205, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_1, - .reverse = false + .id_feedback=0x207, + .id_control=M3508_M2006_CTRL_ID_EXTAND, + .model=Device::RMMotor::MOTOR_M2006, + .can=BSP_CAN_1, + .reverse=false, }, }, - - .motor_name = { - "rod_actr" + .motor_name={ + "yaw" }, - - .cali_speed = -2000.0f, - - .max_range = 750.0f, - - .margin_error = 3.0f, - - .reduction_ratio = 3591.0f / 187.0f + .cali_speed=-2000.0f, + .max_range=250.0f, + .margin_error=3.0f, + .reduction_ratio=3591.0f/187.0f, }, -/* -目前左边： -top 回到位置0（0.1处）fric不动 -mid relax（0.1）fric 不动 -bot 往前推 fric动 - -左边：（推板） -top 上电位置0.1（包含后撤） -mid 不写（保证触发bot) -bot 往前推1 - -右边： -top 摩擦轮relax -mid 摩擦轮on -bot 摩擦轮on - - -*/ - .EVENT_MAP = { + .pitch_param={ + .stall_detect={ + .speed_thld=500.0f, + .current_thld=1.2f, + .stop_current_thld=2.5f, + .temp_thld=40.0f, + .timeout=0.1f + }, + .pos_actuator={ + Component::PosActuator::Param{ + .speed={ + .k=0.00005f, + .p=1.0f, + .i=0.8f, + .d=0.0f, + .i_limit=0.5f, + .out_limit=0.5f, + .d_cutoff_freq=-1.0f, + .cycle=false, + }, + .position={ + .k=2000.0f, + .p=1.0f, + .i=0.0f, + .d=0.0f, + .i_limit=4000.0f, + .out_limit=8000.0f, + .d_cutoff_freq=-1.0f, + .cycle=false, + }, + .in_cutoff_freq=10.0f, + .out_cutoff_freq=30.0f, + }, + }, + .motor_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, + }, + }, + .motor_name={ + "pitch" + }, + .cali_speed=-2000.0f, + .max_range=400.0f, + .margin_error=3.0f, + .reduction_ratio=3591.0f/187.0f + }, + .EVENT_MAP={ Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, - Module::DartLauncher::RELAX + Module::Dartgimbal::SET_MODE_RELAX, }, - Component::CMD::EventMapItem{ + Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_TOP, - Module::DartLauncher::RESET_POSITION + Module::Dartgimbal::SET_MODE_RELAX, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_MID, - Module::DartLauncher::SET_STAY + Module::Dartgimbal::SET_MODE_STABLE, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_BOT, - Module::DartLauncher::FIRE + Module::Dartgimbal::SET_MODE_CONTROL, + }, + }, + }, + .launcher={ + .EVENT_MAP={ + Component::CMD::EventMapItem{ + Component::CMD::CMD_EVENT_LOST_CTRL, + Module::DartLauncher::SET_MODE_RELAX, }, - Component::CMD::EventMapItem{ + Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, - Module::DartLauncher::SET_FRIC_OFF + Module::DartLauncher::SET_MODE_OFF, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::DartLauncher::SET_FRIC_OUTOST + Module::DartLauncher::SET_MODE_STAY, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::DartLauncher::SET_FRIC_BASE + Module::DartLauncher::SET_MODE_ADVANCE, }, }, - + .rod_={ + .stall_detect={ + .speed_thld=500.0f, + .current_thld=1.2f, + .stop_current_thld=2.5f, + .temp_thld=40.0f, + .timeout=0.1f + }, + .pos_actuator={ + Component::PosActuator::Param{ + .speed={ + .k=0.00005f, + .p=1.0f, + .i=0.8f, + .d=0.0f, + .i_limit=0.5f, + .out_limit=0.5f, + .d_cutoff_freq=-1.0f, + .cycle=false, + }, + .position={ + .k=2000.0f, + .p=1.0f, + .i=0.6f, + .d=0.0f, + .i_limit=4000.0f, + .out_limit=8000.0f, + .d_cutoff_freq=-1.0f, + .cycle=false, + }, + .in_cutoff_freq=10.0f, + .out_cutoff_freq=30.0f, + }, + }, + .motor_param={ + Device::RMMotor::Param{ + .id_feedback=0x205, + .id_control=M3508_M2006_CTRL_ID_EXTAND, + .model=Device::RMMotor::MOTOR_M2006, + .can=BSP_CAN_1, + .reverse=false, + }, + }, + .motor_name={ + "rod" + }, + .cali_speed=-2000.0f, + .max_range=750.0f, + .margin_error=3.0f, + .reduction_ratio=3591.0f/187.0f + }, .fric_actr = { Component::SpeedActuator::Param{ .speed = { @@ -187,7 +266,6 @@ bot 摩擦轮on } }, - .fric_motor = { Device::RMMotor::Param{ .id_feedback = 0x201, @@ -201,7 +279,7 @@ bot 摩擦轮on .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, - .reverse = true , + .reverse = true, }, Device::RMMotor::Param{ .id_feedback = 0x203, @@ -219,15 +297,14 @@ bot 摩擦轮on } } }, - .bmi088 = { - .rot_mat = { - { +1, +0, +0}, - { +0, +1, +0}, - { +0, +0, +1}, + .bmi088={ + .rot_mat={ + {+1,+0,+0}, + {+0,+1,+0}, + {+0,+0,+1}, }, } }; - /* clang-format on */ void robot_init() { diff --git a/src/robot/dart/robot.hpp b/src/robot/dart/robot.hpp index ff1b3be9..5593ace3 100644 --- a/src/robot/dart/robot.hpp +++ b/src/robot/dart/robot.hpp @@ -1,11 +1,10 @@ -#include - +/* #include "dev_xxx.hpp" */ #include "dev_ahrs.hpp" #include "dev_bmi088.hpp" #include "dev_can.hpp" #include "dev_dr16.hpp" #include "dev_led_rgb.hpp" -#include "dev_referee.hpp" +#include "mod_dart_gimbal.hpp" #include "mod_dart_launcher.hpp" void robot_init(); @@ -13,21 +12,24 @@ namespace Robot { class Dart { public: typedef struct Param { - Module::DartLauncher::Param dart{}; + Module::Dartgimbal::Param gimbal{}; + Module::DartLauncher::Param launcher{}; Device::BMI088::Rotation bmi088{}; } Param; Component::CMD cmd_; - Device::RGB rgb_{}; - Device::Referee referee; + Device::AHRS ahrs_; + Device::BMI088 bmi088_; Device::Can can_; Device::DR16 dr16_; - Device::BMI088 bmi088_; - Device::AHRS ahrs_; + Device::RGB led_; Module::DartLauncher dartlauncher_; + Module::Dartgimbal dartgimbal_; Dart(Param& param, float control_freq) - : bmi088_(param.bmi088), dartlauncher_(param.dart, control_freq) {} + : bmi088_(param.bmi088), + dartlauncher_(param.launcher, control_freq), + dartgimbal_(param.gimbal, control_freq) {} }; } // namespace Robot diff --git a/src/robot/drone_gimbal/robot.cpp b/src/robot/drone_gimbal/robot.cpp index 1faaf33e..ee076ac0 100644 --- a/src/robot/drone_gimbal/robot.cpp +++ b/src/robot/drone_gimbal/robot.cpp @@ -1,28 +1,39 @@ #include "robot.hpp" -#include #include #include -#include "dev_rm_motor.hpp" - using namespace Robot; /* clang-format off */ -Robot::UVA::Param param={ +//TODO: write your param +Robot::Drone::Param param={ + .gimbal{ - .ff = { - }, + .ff = { + /* GIMBAL_CTRL_PIT_FEEDFORWARD */ + .a = 0.0439f, + .b = -0.0896f, + .c = 0.077f, + .max = 0.1f, + .min = -0.2f, + }, /* ff */ - .st = { - }, + .st = { + /* GIMBAL_CTRL_YAW_SELF_TUNING */ + .a = 0.0677f, + .b = 0.1653f, + .c = 0.3379f, + .max = 0.37f, + .min = 0.29f, + }, /* st */ .yaw_actr = { .speed = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ - .k = 0.22f, - .p = 1.0f, - .i = 0.5f, + .k = 0.2f, + .p = 0.8f, + .i = 0.1f, .d = 0.0f, .i_limit = 0.2f, .out_limit = 0.1f, @@ -32,10 +43,10 @@ Robot::UVA::Param param={ .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 20.0f, - .p = 2.5f, - .i = 0.0f, - .d = 0.0f, + .k = 18.0f, + .p = 3.6f, + .i = 0.1f, + .d = 0.05f, .i_limit = 0.1f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -50,8 +61,8 @@ Robot::UVA::Param param={ .speed = { /* GIMBAL_CTRL_PIT_OMEGA_IDX */ .k = 0.25f, - .p = 1.0f, - .i = 0.3f, + .p = 0.8f, + .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, .out_limit = 1.0f, @@ -61,11 +72,70 @@ Robot::UVA::Param param={ .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ - .k = 20.0f, - .p = 1.0f, + .k = 10.0f, + .p = 0.5f, + .i = 0.0f, + .d = 0.05f, + .i_limit = 1.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.2f, + .p = 0.8f, + .i = 0.1f, + .d = 0.0f, + .i_limit = 0.2f, + .out_limit = 0.1f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_YAW_ANGLE_IDX */ + .k = 18.0f, + .p = 3.6f, + .i = 0.1f, + .d = 0.05f, + .i_limit = 0.1f, + .out_limit = 10.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + .pit_ai_actr = { + .speed = { + /* GIMBAL_CTRL_PIT_OMEGA_IDX */ + .k = 0.25f, + .p = 0.8f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 10.0f, + .p = 0.5f, + .i = 0.0f, + .d = 0.05f, + .i_limit = 1.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, .cycle = true, @@ -77,10 +147,10 @@ Robot::UVA::Param param={ }, .yaw_motor = { - .id_feedback = 0x207, + .id_feedback = 0x208, .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, - .can = BSP_CAN_2, + .can = BSP_CAN_1, .reverse = true }, @@ -93,19 +163,19 @@ Robot::UVA::Param param={ }, .mech_zero = { - .yaw = 1.65f, - .pit = 1.7f, + .yaw = 0.0f, + .pit = 0.f, .rol = 0.0f, }, .limit = { - .pitch_max = M_2PI - 0.4, - .pitch_min = M_2PI - 1.08, - .yaw_max = M_2PI - 0.7, - .yaw_min = M_2PI - 2.7, - + .pitch_max = M_2PI-5.97f, + .pitch_min = M_2PI-1.f, + .yaw_max = 5.5f, + .yaw_min = 3.5f, }, - .EVENT_MAP = { + + .EVENT_MAP = { Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, Module::Gimbal::SET_MODE_RELAX @@ -122,33 +192,30 @@ Robot::UVA::Param param={ Device::DR16::DR16_SW_L_POS_BOT, Module::Gimbal::SET_MODE_ABSOLUTE } - - } + } }, - .launcher={ - .trig_gear_ratio = 36.0f, - .num_trig_tooth = 8.0f, - .min_launch_delay = static_cast(1000.0f / 20.0f), - - .trig_actr = { + .trig_gear_ratio=36.0f, + .bullet_circle_num=8.0f, + .min_launcher_delay= static_cast(1000.0f/20.0f), + .trig_actr={ Component::PosActuator::Param{ .speed = { - .k = 1.5, + .k = 3.0f, .p = 1.0f, - .i = 0.0f, - .d = 0.03f, + .i = 0.5f, + .d = 0.0f, .i_limit = 0.5f, - .out_limit = 0.6f, + .out_limit = 0.5f, .d_cutoff_freq = -1.0f, .cycle = false, }, .position = { - .k = 1.2f, + .k = 1.5f, .p = 1.0f, .i = 0.0f, - .d = 0.012f, + .d = 0.0f, .i_limit = 1.0f, .out_limit = 1.0f, .d_cutoff_freq = -1.0f, @@ -160,49 +227,36 @@ Robot::UVA::Param param={ .out_cutoff_freq = -1.0f, }, }, - - .trig_motor = { + .trig_motor={ Device::RMMotor::Param{ .id_feedback = 0x203, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = false + .can = BSP_CAN_1, + .reverse=false, } }, - .EVENT_MAP = { + .EVENT_MAP={ Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, - Module::UVALauncher::CHANGE_FIRE_MODE_SAFE + Module::DroneLauncher::SET_RELAX, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, - Module::UVALauncher::CHANGE_FIRE_MODE_SAFE + Module::DroneLauncher::SET_RELAX, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::UVALauncher::CHANGE_TRIG_MODE_SINGLE + Module::DroneLauncher::CHANGE_FRIC_MODE_LOADED, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::UVALauncher::CHANGE_TRIG_MODE_BURST + Module::DroneLauncher::CHANGE_TRIG_MODE_CONTINUED, }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, - Module::UVALauncher::LAUNCHER_START_FIRE - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_L_PRESS, - Module::UVALauncher::CHANGE_TRIG_MODE_SINGLE - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_L_RELEASE, - Module::UVALauncher::CHANGE_FIRE_MODE_SAFE - }, - }, + } }, - .bmi088_rot = { - .rot_mat = { + .bmi088_rot={ + .rot_mat={ { +1, +0, +0}, { +0, +1, +0}, { +0, +0, +1}, @@ -210,8 +264,7 @@ Robot::UVA::Param param={ }, }; /* clang-format on */ -Robot::UVA *Robot::UVA::self_; void robot_init() { - System::Start(param, 500.0f); + System::Start(param, 500.0f); } diff --git a/src/robot/drone_gimbal/robot.hpp b/src/robot/drone_gimbal/robot.hpp index 1e21d4bf..23665feb 100644 --- a/src/robot/drone_gimbal/robot.hpp +++ b/src/robot/drone_gimbal/robot.hpp @@ -1,10 +1,9 @@ - +/* #include "dev_xxx.hpp" */ #include "comp_cmd.hpp" #include "dev_ahrs.hpp" #include "dev_bmi088.hpp" #include "dev_can.hpp" #include "dev_dr16.hpp" -#include "dev_led_rgb.hpp" #include "dev_referee.hpp" #include "mod_gimbal.hpp" #include "mod_launcher_drone.hpp" @@ -12,11 +11,11 @@ void robot_init(); namespace Robot { -class UVA { +class Drone { public: typedef struct { Module::Gimbal::Param gimbal; - Module::UVALauncher::Param launcher; + Module::DroneLauncher::Param launcher; Device::BMI088::Rotation bmi088_rot; } Param; @@ -26,17 +25,14 @@ class UVA { Device::BMI088 bmi088_; Device::Can can_; Device::DR16 dr16_; - Device::RGB led_; Device::Referee referee_; Module::Gimbal gimbal_; - Module::UVALauncher launcher_; - static UVA* self_; - UVA(Param& param, float control_freq) + Module::DroneLauncher launcher_; + + Drone(Param& param, float control_freq) : bmi088_(param.bmi088_rot), gimbal_(param.gimbal, control_freq), - launcher_(param.launcher, control_freq) { - self_ = this; - } + launcher_(param.launcher, control_freq) {} }; } // namespace Robot 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(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/hero/robot.cpp b/src/robot/hero/robot.cpp index 6b233af3..558dfb9b 100644 --- a/src/robot/hero/robot.cpp +++ b/src/robot/hero/robot.cpp @@ -2,9 +2,7 @@ #include -#include "bsp_can.h" #include "dev_rm_motor.hpp" -#include "mod_chassis.hpp" #include "system.hpp" /* clang-format off */ @@ -16,48 +14,45 @@ Robot::Hero::Param param = { .constant_ = 1.9021463852937033f, .type = Component::Mixer::MECANUM, .follow_pid_param = { - .k = 0.5f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, + .k = 1.0f, + .p = 2.5f, + .i = 0.f, + .d = 0.f, .i_limit = 1.0f, .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .cycle = true, }, - - .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_MID, - Module::RMChassis::SET_MODE_FOLLOW - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_BOT, - Module::RMChassis::SET_MODE_ROTOR - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_V, - Module::RMChassis::SET_MODE_ROTOR - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_B, - Module::RMChassis::SET_MODE_FOLLOW - } + .xaccl_pid_param = + { + .k = 0.4f, + .p = 0.8f, + .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, }, + + .actuator_param = { Component::SpeedActuator::Param{ .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 1.6f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -74,7 +69,7 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 1.6f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -90,7 +85,7 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 1.6f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -106,7 +101,7 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 1.6f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -123,52 +118,61 @@ Robot::Hero::Param param = { .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, + .can = BSP_CAN_2, .reverse = false }, Device::RMMotor::Param{ .id_feedback = 0x202, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_1, + .can = BSP_CAN_2, .reverse = false }, Device::RMMotor::Param{ - .id_feedback = 0x203, + .id_feedback = 0x204, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_1, + .can = BSP_CAN_2, .reverse = false }, Device::RMMotor::Param{ - .id_feedback = 0x204, + .id_feedback = 0x201, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_1, + .can = BSP_CAN_2, .reverse = 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_MID, + Module::RMChassis::SET_MODE_FOLLOW + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_BOT, + Module::RMChassis::SET_MODE_ROTOR + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_E, + Module::RMChassis::SET_MODE_ROTOR + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_Q, + Module::RMChassis::SET_MODE_FOLLOW + }, - .get_speed = [](float power_limit){ - float speed = 0.0f; - if (power_limit <= 50.0f) { - speed = 5500; - } else if (power_limit <= 60.0f) { - speed = 5500; - } else if (power_limit <= 70.0f) { - speed = 5500; - } else if (power_limit <= 80.0f) { - speed = 6200; - } else if (power_limit <= 100.0f) { - speed = 7000; - } else { - speed = 7500; - } - return speed; - }, + }, }, @@ -194,10 +198,39 @@ Robot::Hero::Param param = { .yaw_actr = { .speed = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ - .k = 0.85f, + .k = 2.0f, + .p = 1.6f, + .i = 1.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 2.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_YAW_ANGLE_IDX */ + .k = 20.0f, .p = 1.0f, - .i = 0.3f, + .i = 0.0f, .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 15.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + .pit_actr = { + .speed = { + /* GIMBAL_CTRL_PIT_OMEGA_IDX */ + .k = 0.6f, + .p = 2.0f, + .i = 0.8f, + .d = 0.f, .i_limit = 0.8f, .out_limit = 1.0f, .d_cutoff_freq = -1.0f, @@ -205,7 +238,7 @@ Robot::Hero::Param param = { }, .position = { - /* GIMBAL_CTRL_YAW_ANGLE_IDX */ + /* GIMBAL_CTRL_PIT_ANGLE_IDX */ .k = 20.0f, .p = 1.0f, .i = 0.0f, @@ -220,12 +253,41 @@ Robot::Hero::Param param = { .out_cutoff_freq = -1.0f, }, - .pit_actr = { + .yaw_ai_actr = { + .speed = { + /* GIMBAL_CTRL_YAW_OMEGA_IDX */ + .k = 2.0f, + .p = 1.6f, + .i = 1.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 2.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_YAW_ANGLE_IDX */ + .k = 20.0f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 15.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + .pit_ai_actr = { .speed = { /* GIMBAL_CTRL_PIT_OMEGA_IDX */ - .k = 0.5f, - .p = 1.0f, - .i = 0.6f, + .k = 0.6f, + .p = 2.0f, + .i = 0.8f, .d = 0.f, .i_limit = 0.8f, .out_limit = 1.0f, @@ -249,9 +311,8 @@ Robot::Hero::Param param = { .out_cutoff_freq = -1.0f, }, - .yaw_motor = { - .id_feedback = 0x209, + .id_feedback = 0x20A, .id_control = GM6020_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_2, @@ -259,22 +320,22 @@ Robot::Hero::Param param = { }, .pit_motor = { - .id_feedback = 0x20A, + .id_feedback = 0x209, .id_control = GM6020_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_GM6020, - .can = BSP_CAN_2, + .can = BSP_CAN_1, .reverse = true, }, .mech_zero = { - .yaw = 1.3f, - .pit = 4.0f, + .yaw = M_2PI-4.377214f, + .pit = 0.0f, .rol = 0.0f, }, .limit = { - .pitch_max = M_2PI - 3.51895213f, - .pitch_min = M_2PI - 4.46541834f, + .pitch_max =M_2PI-4.348, + .pitch_min = M_2PI-5.13111, .yaw_max = 0.0f, .yaw_min = 0.0f, }, @@ -286,45 +347,48 @@ Robot::Hero::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, - Module::Gimbal::ABSOLUTE + Module::Gimbal::SET_MODE_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 = 6.0f, .trig_gear_ratio = 3591.0f / 187.0f, - .fric_radius = 0.03f, - .cover_open_duty = 0.125f, - .cover_close_duty = 0.075f, - .model = Module::Launcher::LAUNCHER_MODEL_42MM, - .default_bullet_speed = 16.0f, + .model=Module::Launcher::LAUNCHER_MODEL_42MM, .min_launch_delay = 800, + .allow_reverse = true, + .fric_speed_1 = 4700, + .fric_speed_2 = 5690, + /* 新launcher參數 */ .trig_actr = { Component::PosActuator::Param{ .speed = { - .k = 10.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, + .k = 8.0f, + .p = 1.1f, + .i = 0.f, + .d = 0.f, .i_limit = 1.0f, .out_limit = 1.0f, .d_cutoff_freq = -1.0f, @@ -333,11 +397,11 @@ Robot::Hero::Param param = { .position = { .k = 0.8f, - .p = 1.0f, + .p = 1.9f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, - .out_limit = 1.0f, + .out_limit = 2.0f, .d_cutoff_freq = -1.0f, .cycle = true, }, @@ -352,11 +416,11 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.001f, - .p = 1.2f, - .i = 0.0f, + .p = 1.5f, + .i = 0.00f, .d = 0.0f, .i_limit = 0.3f, - .out_limit = 1.0f, + .out_limit = 1.50f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -368,11 +432,43 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.001f, - .p = 1.2f, - .i = 0.0f, + .p = 1.5f, + .i = 0.00f, .d = 0.0f, .i_limit = 0.3f, - .out_limit = 1.0f, + .out_limit = 1.50f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + Component::SpeedActuator::Param{ + .speed = { + .k = 0.001f, + .p = 1.5f, + .i = 0.00f, + .d = 0.0f, + .i_limit = 0.3f, + .out_limit = 1.50f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + Component::SpeedActuator::Param{ + .speed = { + .k = 0.001f, + .p = 1.5f, + .i = 0.00f, + .d = 0.0f, + .i_limit = 0.3f, + .out_limit = 1.50f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -385,29 +481,49 @@ Robot::Hero::Param param = { .trig_motor = { Device::RMMotor::Param{ - .id_feedback = 0x207, + .id_feedback = 0x205, .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_1, - .reverse = false + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_2, + .reverse = false, } }, + +/* + + 1 3 +-------->弹丸射出方向 + 0 2(反) +*/ .fric_motor = { Device::RMMotor::Param{ - .id_feedback = 0x206, - .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 + .can = BSP_CAN_1, + .reverse = false, }, - Device::RMMotor::Param{ - .id_feedback = 0x205, - .id_control = M3508_M2006_CTRL_ID_EXTAND, + .id_feedback = 0x204, + .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = false + .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 = true, + }, + Device::RMMotor::Param{ + .id_feedback = 0x202, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_1, + .reverse = false, }, }, @@ -436,18 +552,8 @@ Robot::Hero::Param param = { Device::DR16::KEY_L_PRESS, Module::Launcher::LAUNCHER_START_FIRE }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_G, - Module::Launcher::CHANGE_TRIG_MODE - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_R, - Module::Launcher::OPEN_COVER - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_F, - Module::Launcher::CLOSE_COVER - } + + }, }, /* launcher */ @@ -460,7 +566,7 @@ Robot::Hero::Param param = { }, .cap = { - .can = BSP_CAN_1, + .can = BSP_CAN_2, }, }; /* clang-format on */ diff --git a/src/robot/hero/robot.hpp b/src/robot/hero/robot.hpp index 9296d3ea..113f4323 100644 --- a/src/robot/hero/robot.hpp +++ b/src/robot/hero/robot.hpp @@ -9,7 +9,7 @@ #include "dev_referee.hpp" #include "mod_chassis.hpp" #include "mod_gimbal.hpp" -#include "mod_launcher.hpp" +#include "mod_hero_launcher.hpp" void robot_init(); namespace Robot { diff --git a/src/robot/infantry/robot.cpp b/src/robot/infantry/robot.cpp index 4f5113ab..678962a5 100644 --- a/src/robot/infantry/robot.cpp +++ b/src/robot/infantry/robot.cpp @@ -26,33 +26,6 @@ Robot::Infantry::Param param = { .cycle = true, }, - .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_MID, - Module::RMChassis::SET_MODE_INDENPENDENT - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_BOT, - Module::RMChassis::SET_MODE_ROTOR - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_V, - Module::RMChassis::SET_MODE_ROTOR - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_B, - Module::RMChassis::SET_MODE_FOLLOW - } - }, - .actuator_param = { Component::SpeedActuator::Param{ .speed = { @@ -64,6 +37,7 @@ Robot::Infantry::Param param = { .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .cycle = false, + }, .in_cutoff_freq = -1.0f, @@ -151,22 +125,31 @@ Robot::Infantry::Param param = { .reverse = false }, }, - .get_speed = [](float power_limit){ - float speed = 0.0f; - if (power_limit <= 50.0f) { - speed = 0.0f; - } else if (power_limit <= 60.0f) { - speed = 3800; - } else if (power_limit <= 70.0f) { - speed = 5000; - } else if (power_limit <= 80.0f) { - speed = 5500; - } else if (power_limit <= 100.0f) { - speed = 6000; - } else { - speed = 6500; - } - return speed; + .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_MID, + Module::RMChassis::SET_MODE_INDENPENDENT + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_BOT, + Module::RMChassis::SET_MODE_ROTOR + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_V, + Module::RMChassis::SET_MODE_ROTOR + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_B, + Module::RMChassis::SET_MODE_FOLLOW + } }, }, @@ -247,6 +230,64 @@ Robot::Infantry::Param param = { .out_cutoff_freq = -1.0f, }, + .yaw_ai_actr = { + .speed = { + /* GIMBAL_CTRL_YAW_OMEGA_IDX */ + .k = 0.28f, + .p = 1.f, + .i = 1.f, + .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, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 0.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + .pit_ai_actr = { + .speed = { + /* GIMBAL_CTRL_PIT_OMEGA_IDX */ + .k = 0.25f, + .p = 1.0f, + .i = 0.f, + .d = 0.f, + .i_limit = 0.8f, + .out_limit = 0.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 20.0f, + .p = 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 = 0x209, @@ -288,19 +329,19 @@ Robot::Infantry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Gimbal::START_AUTO_AIM + Module::Gimbal::SET_MODE_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Gimbal::START_AUTO_AIM + Module::Gimbal::SET_MODE_AUTO_AIM }, 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 } }, @@ -312,7 +353,7 @@ Robot::Infantry::Param param = { .fric_radius = 0.03f, .cover_open_duty = 0.125f, .cover_close_duty = 0.075f, - .model = Module::Launcher::LAUNCHER_MODEL_17MM, + .model = Module::RMLauncher::LAUNCHER_MODEL_17MM, .default_bullet_speed = 15.f, .min_launch_delay = static_cast(1000.0f / 20.0f), @@ -381,7 +422,7 @@ Robot::Infantry::Param param = { }, }, - .trig_motor = { + .trig_param = { Device::RMMotor::Param{ .id_feedback = 0x201, .id_control = M3508_M2006_CTRL_ID_BASE, @@ -391,7 +432,7 @@ Robot::Infantry::Param param = { } }, - .fric_motor = { + .fric_param = { Device::RMMotor::Param{ .id_feedback = 0x203, .id_control = M3508_M2006_CTRL_ID_BASE, @@ -411,39 +452,39 @@ Robot::Infantry::Param param = { .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_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::KEY_L_PRESS, - Module::Launcher::LAUNCHER_START_FIRE + Module::RMLauncher::LAUNCHER_START_FIRE }, 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 } }, }, /* launcher */ diff --git a/src/robot/infantry/robot.hpp b/src/robot/infantry/robot.hpp index b04806a6..0c43794f 100644 --- a/src/robot/infantry/robot.hpp +++ b/src/robot/infantry/robot.hpp @@ -18,7 +18,7 @@ class Infantry { 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; @@ -36,7 +36,7 @@ class Infantry { Module::RMChassis chassis_; Module::Gimbal gimbal_; - Module::Launcher launcher_; + Module::RMLauncher launcher_; Infantry(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 #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(1000.0f / 20.0f), + .model = Module::RMLauncher::LAUNCHER_MODEL_17MM, + .default_bullet_speed = 25.0f, + .min_launch_delay = static_cast(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), diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index bac02e7c..da50368c 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -10,13 +10,13 @@ Robot::Sentry::Param param = { .chassis={ .toque_coefficient_ = 0.0327120418848f, - .speed_2_coefficient_ = 2.300974248103511e-07f, - .out_2_coefficient_ = 2.1455244766462685e-29f, - .constant_ = 0.23958431845825284f, + .speed_2_coefficient_ = 1.227822928729637e-07, + .out_2_coefficient_ = 1.1108430132455055e-24, + .constant_ = 1.8135014050213443, .type = Component::Mixer::OMNICROSS, .follow_pid_param = { - .k = 0.5f, + .k = 1.f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -25,29 +25,29 @@ Robot::Sentry::Param param = { .d_cutoff_freq = -1.0f, .cycle = true, }, - - .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_INDENPENDENT - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_MID, - Module::RMChassis::SET_MODE_FOLLOW - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_BOT, - Module::RMChassis::SET_MODE_ROTOR + .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, }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_ROTOR, - Module::RMChassis::SET_MODE_ROTOR - } - }, + .actuator_param = { Component::SpeedActuator::Param{ @@ -119,14 +119,14 @@ Robot::Sentry::Param param = { .motor_param = { Device::RMMotor::Param{ - .id_feedback = 0x201, + .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 = 0x202, + .id_feedback = 0x201, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, @@ -140,30 +140,34 @@ Robot::Sentry::Param param = { .reverse = false }, Device::RMMotor::Param{ - .id_feedback = 0x204, + .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 = 0.0f; - } else if (power_limit <= 60.0f) { - speed = 3800; - } else if (power_limit <= 70.0f) { - speed = 5000; - } else if (power_limit <= 80.0f) { - speed = 5500; - } else if (power_limit <= 100.0f) { - speed = 6000; - } else { - speed = 6500; + .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_MID, + Module::RMChassis::SET_MODE_FOLLOW + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_BOT, + Module::RMChassis::SET_MODE_INDENPENDENT + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::AI_ROTOR, + Module::RMChassis::SET_MODE_ROTOR } - return speed; }, }, @@ -191,7 +195,7 @@ Robot::Sentry::Param param = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ .k = 0.28f, .p = 1.f, - .i = 1.f, + .i = 10.f, .d = 0.f, .i_limit = 0.2f, .out_limit = 1.0f, @@ -201,10 +205,10 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 25.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, + .k = 15.0f, + .p = 1.5f, + .i = 5.f, + .d = 1.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -230,10 +234,68 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 15.0f, + .p = 1.5f, + .i = 5.0f, + .d = 1.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.28f, + .p = 1.1f, + .i = 10.f, + .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 = 25.0f, - .p = 1.0f, + .p = 1.5f, + .i = 5.f, + .d = 1.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, + }, + .pit_ai_actr = { + .speed = { + /* GIMBAL_CTRL_PIT_OMEGA_IDX */ + .k = 0.1f, + .p = 1.2f, .i = 0.0f, - .d = 0.0f, + .d = 0.f, + .i_limit = 0.8f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 27.0f, + .p = 2.2f, + .i = 6.0f, + .d = 1.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -249,29 +311,29 @@ Robot::Sentry::Param param = { .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_1, - .reverse = false + .reverse = true }, .pit_motor = { - .id_feedback = 0x209, - .id_control = GM6020_CTRL_ID_EXTAND, + .id_feedback = 0x205, + .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_2, - .reverse = false + .reverse = true }, .mech_zero = { - .yaw = 1.58f, - .pit = 4.6f, + .yaw = M_2PI-0.46f, + .pit = M_2PI-2.2, .rol = 0.0f, }, .limit = { - .pitch_max = 4.9f, - .pitch_min = 4.46f, + .pitch_max = M_2PI-1.7f, + .pitch_min = M_2PI-2.4f, .yaw_max = 0.0f, - .yaw_min = 0.0f, - }, + .yaw_min = 0.0f + }, .EVENT_MAP = { Component::CMD::EventMapItem{ @@ -284,7 +346,7 @@ Robot::Sentry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::SET_MODE_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, @@ -294,15 +356,15 @@ Robot::Sentry::Param param = { }, - .launcher1 = { + .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, + .model = Module::RMLauncher::LAUNCHER_MODEL_17MM, .default_bullet_speed = 15.f, - .min_launch_delay = static_cast(1000.0f / 20.0f), + .min_launch_delay = static_cast(1000.0f / 10.0f), .trig_actr = { Component::PosActuator::Param{ @@ -369,23 +431,23 @@ Robot::Sentry::Param param = { }, }, - .trig_motor = { + .trig_param = { Device::RMMotor::Param{ - .id_feedback = 0x207, + .id_feedback = 0x205, .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = true, + .can = BSP_CAN_1, + .reverse = false, } }, - .fric_motor = { + .fric_param = { Device::RMMotor::Param{ - .id_feedback = 0x204, + .id_feedback = 0x202, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, - .reverse = false + .reverse = false, }, Device::RMMotor::Param{ .id_feedback = 0x203, @@ -396,179 +458,48 @@ Robot::Sentry::Param param = { } }, - .EVENT_MAP = { - Component::CMD::EventMapItem{ - Component::CMD::CMD_EVENT_LOST_CTRL, - Module::Launcher::CHANGE_FIRE_MODE_RELAX - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_TOP, - Module::Launcher::CHANGE_FIRE_MODE_SAFE - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_MID, - Module::Launcher::CHANGE_FIRE_MODE_LOADED - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::CHANGE_FIRE_MODE_LOADED - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::LAUNCHER_START_FIRE - }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_FIRE_COMMAND, - Module::Launcher::CHANGE_TRIG_MODE_BURST - }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_STOP_FIRE, - Module::Launcher::LAUNCHER_STOP_TRIG - } - - }, - }, /* launcher1 */ -.launcher2 = { - .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 = 15.f, - .min_launch_delay = static_cast(1000.0f / 20.0f), - - .trig_actr = { - Component::PosActuator::Param{ - .speed = { - .k = 3.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 1.5f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 1.0f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = true, - }, - - .in_cutoff_freq = -1.0f, - .out_cutoff_freq = -1.0f, - }, - }, - - .fric_actr = { - Component::SpeedActuator::Param{ - .speed = { - .k = 0.00035f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .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, - }, - Component::SpeedActuator::Param{ - .speed = { - .k = 0.00035f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .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 = { - Device::RMMotor::Param{ - .id_feedback =0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = true, - }, - }, - - .fric_motor = { - Device::RMMotor::Param{ - .id_feedback = 0x202, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = false, - }, - Device::RMMotor::Param{ - .id_feedback = 0x201, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = false, - }, - }, - - .EVENT_MAP = { + .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_SAFE }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::CHANGE_FIRE_MODE_LOADED + Module::RMLauncher::CHANGE_FIRE_MODE_SAFE }, 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::AI::AIControlData::AI_FIRE_COMMAND, - Module::Launcher::CHANGE_TRIG_MODE_BURST + Module::RMLauncher::CHANGE_TRIG_MODE_BURST }, Component::CMD::EventMapItem{ Device::AI::AIControlData::AI_STOP_FIRE, - Module::Launcher::LAUNCHER_STOP_TRIG + Module::RMLauncher::LAUNCHER_STOP_TRIG } }, - }, /* launcher */ - .bmi088_rot = { - .rot_mat = { - { +1, +0, +0}, - { +0, +1, +0}, - { +0, +0, +1}, - }, - }, + }, /* launcher*/ + .bmi088_rot = + { + .rot_mat = + { + {+0, +1, +0}, + {-1, +0, +0}, + {+0, +0, +1}, + }, + }, .cap = { .can = BSP_CAN_1, diff --git a/src/robot/sentry/robot.hpp b/src/robot/sentry/robot.hpp index 95e1b8cb..56dc9380 100644 --- a/src/robot/sentry/robot.hpp +++ b/src/robot/sentry/robot.hpp @@ -18,8 +18,7 @@ class Sentry { typedef struct Param { Module::RMChassis::Param chassis; Module::Gimbal::Param gimbal; - Module::Launcher::Param launcher1; - Module::Launcher::Param launcher2; + Module::RMLauncher::Param launcher; Device::BMI088::Rotation bmi088_rot{}; Device::Cap::Param cap{}; } Param; @@ -38,8 +37,7 @@ class Sentry { Module::RMChassis chassis_; Module::Gimbal gimbal_; - Module::Launcher launcher1_; - Module::Launcher launcher2_; + Module::RMLauncher launcher_; Sentry(Param& param, float control_freq) : cmd_(Component::CMD::CMD_AUTO_CTRL), ai_(false), @@ -47,7 +45,6 @@ class Sentry { cap_(param.cap), chassis_(param.chassis, control_freq), gimbal_(param.gimbal, control_freq), - launcher1_(param.launcher1, control_freq), - launcher2_(param.launcher2, control_freq) {} + launcher_(param.launcher, control_freq) {} }; } // namespace Robot diff --git a/src/robot/sim_mecanum/robot.cpp b/src/robot/sim_mecanum/robot.cpp index d834ee7e..4f51b0e3 100644 --- a/src/robot/sim_mecanum/robot.cpp +++ b/src/robot/sim_mecanum/robot.cpp @@ -33,17 +33,6 @@ Robot::Simulator::Param param = { .cycle = true, }, - .EVENT_MAP = { - Component::CMD::EventMapItem{ - Device::TerminalController::STOP_CTRL, - Module::RMChassis::SET_MODE_RELAX - }, - Component::CMD::EventMapItem{ - Device::TerminalController::START_CTRL, - Module::RMChassis::SET_MODE_INDENPENDENT - }, - }, - .actuator_param = { Component::SpeedActuator::Param{ .speed = { @@ -126,23 +115,15 @@ Robot::Simulator::Param param = { .model = Device::RMMotor::MOTOR_M3508, }, }, - - .get_speed = [](float power_limit){ - float speed = 0.0f; - if (power_limit <= 50.0f) { - speed = 5500; - } else if (power_limit <= 60.0f) { - speed = 5500; - } else if (power_limit <= 70.0f) { - speed = 5500; - } else if (power_limit <= 80.0f) { - speed = 6200; - } else if (power_limit <= 100.0f) { - speed = 7000; - } else { - speed = 7500; - } - return speed; + .EVENT_MAP = { + Component::CMD::EventMapItem{ + Device::TerminalController::STOP_CTRL, + Module::RMChassis::SET_MODE_RELAX + }, + Component::CMD::EventMapItem{ + Device::TerminalController::START_CTRL, + Module::RMChassis::SET_MODE_INDENPENDENT + }, }, }, };