diff --git a/hw/bsp/c-mini/drivers/bsp_can.c b/hw/bsp/c-mini/drivers/bsp_can.c index f99563ac..fb48472d 100644 --- a/hw/bsp/c-mini/drivers/bsp_can.c +++ b/hw/bsp/c-mini/drivers/bsp_can.c @@ -269,6 +269,9 @@ bsp_status_t bsp_can_register_callback( bsp_status_t bsp_ext_can_trans_packet(bsp_can_t can, bsp_can_format_t format, uint32_t id, uint8_t *data) { + if (format == CAN_FORMAT_EXT_REMOTE || format == CAN_FORMAT_STD_REMOTE) { + return BSP_ERR; + } tx_ext_buff[can - BSP_CAN_BASE_NUM].id = id; tx_ext_buff[can - BSP_CAN_BASE_NUM].type = format; memcpy(tx_ext_buff[can - BSP_CAN_BASE_NUM].data, data, 8); @@ -285,22 +288,40 @@ bsp_status_t bsp_ext_can_trans_packet(bsp_can_t can, bsp_can_format_t format, bsp_status_t bsp_can_trans_packet(bsp_can_t can, bsp_can_format_t format, uint32_t id, uint8_t *data) { + CAN_TxHeaderTypeDef header; if (can >= BSP_CAN_BASE_NUM) { return bsp_ext_can_trans_packet(can, format, id, data); } - if (format == CAN_FORMAT_STD) { - tx_buff[can].StdId = id; - tx_buff[can].IDE = CAN_ID_STD; - } else { - tx_buff[can].ExtId = id; - tx_buff[can].IDE = CAN_ID_EXT; + switch (format) { + case CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; } - - tx_buff[can].RTR = CAN_RTR_DATA; - tx_buff[can].TransmitGlobalTime = DISABLE; - tx_buff[can].DLC = 8; - uint32_t tsr = READ_REG(bsp_can_get_handle(can)->Instance->TSR); while (((tsr & CAN_TSR_TME0) == 0U) && ((tsr & CAN_TSR_TME1) == 0U) && diff --git a/hw/bsp/c-mini/drivers/bsp_can.h b/hw/bsp/c-mini/drivers/bsp_can.h index e7825b49..4abc6955 100644 --- a/hw/bsp/c-mini/drivers/bsp_can.h +++ b/hw/bsp/c-mini/drivers/bsp_can.h @@ -25,8 +25,10 @@ typedef enum { } bsp_can_callback_t; typedef enum { - CAN_FORMAT_STD, - CAN_FORMAT_EXT, + CAN_FORMAT_STD_DATA, + CAN_FORMAT_EXT_DATA, + CAN_FORMAT_STD_REMOTE, + CAN_FORMAT_EXT_REMOTE, } bsp_can_format_t; void bsp_can_init(void); diff --git a/hw/bsp/esp32-c3-arduino/sdkconfig b/hw/bsp/esp32-c3-arduino/sdkconfig index 9afd699a..b951894f 100644 --- a/hw/bsp/esp32-c3-arduino/sdkconfig +++ b/hw/bsp/esp32-c3-arduino/sdkconfig @@ -241,10 +241,7 @@ CONFIG_BT_CTRL_HCI_MODE_VHCI=y # CONFIG_BT_CTRL_HCI_MODE_UART_H4 is not set CONFIG_BT_CTRL_HCI_TL=1 CONFIG_BT_CTRL_ADV_DUP_FILT_MAX=30 -CONFIG_BT_BLE_CCA_MODE_NONE=y -# CONFIG_BT_BLE_CCA_MODE_HW is not set -# CONFIG_BT_BLE_CCA_MODE_SW is not set -CONFIG_BT_BLE_CCA_MODE=0 +# CONFIG_BT_CTRL_HW_CCA is not set CONFIG_BT_CTRL_HW_CCA_VAL=20 CONFIG_BT_CTRL_HW_CCA_EFF=0 CONFIG_BT_CTRL_CE_LENGTH_TYPE_ORIG=y @@ -283,7 +280,6 @@ CONFIG_BT_CTRL_SCAN_DUPL_TYPE_DEVICE=y # CONFIG_BT_CTRL_SCAN_DUPL_TYPE_DATA_DEVICE is not set CONFIG_BT_CTRL_SCAN_DUPL_TYPE=0 CONFIG_BT_CTRL_SCAN_DUPL_CACHE_SIZE=100 -CONFIG_BT_CTRL_DUPL_SCAN_CACHE_REFRESH_PERIOD=0 # CONFIG_BT_CTRL_BLE_MESH_SCAN_DUPL_EN is not set # CONFIG_BT_CTRL_COEX_PHY_CODED_TX_RX_TLIM_EN is not set CONFIG_BT_CTRL_COEX_PHY_CODED_TX_RX_TLIM_DIS=y @@ -322,9 +318,6 @@ CONFIG_BT_GATT_MAX_SR_ATTRIBUTES=100 # CONFIG_BT_GATTS_SEND_SERVICE_CHANGE_MANUAL is not set CONFIG_BT_GATTS_SEND_SERVICE_CHANGE_AUTO=y CONFIG_BT_GATTS_SEND_SERVICE_CHANGE_MODE=0 -# CONFIG_BT_GATTS_ROBUST_CACHING_ENABLED is not set -# CONFIG_BT_GATTS_DEVICE_NAME_WRITABLE is not set -# CONFIG_BT_GATTS_APPEARANCE_WRITABLE is not set CONFIG_BT_GATTC_ENABLE=y CONFIG_BT_GATTC_MAX_CACHE_CHAR=40 # CONFIG_BT_GATTC_CACHE_NVS_FLASH is not set @@ -515,10 +508,9 @@ CONFIG_BT_SMP_ENABLE=y # CONFIG_BT_BLE_ACT_SCAN_REP_ADV_SCAN is not set CONFIG_BT_BLE_ESTAB_LINK_CONN_TOUT=30 CONFIG_BT_MAX_DEVICE_NAME_LEN=32 -CONFIG_BT_BLE_RPA_TIMEOUT=900 +CONFIG_BT_BLE_RPA_SUPPORTED=y CONFIG_BT_BLE_50_FEATURES_SUPPORTED=y CONFIG_BT_BLE_42_FEATURES_SUPPORTED=y -# CONFIG_BT_BLE_HIGH_DUTY_ADV_INTERVAL is not set # end of Bluedroid Options # end of Bluetooth @@ -533,8 +525,6 @@ CONFIG_BT_BLE_42_FEATURES_SUPPORTED=y # # CONFIG_ADC_FORCE_XPD_FSM is not set CONFIG_ADC_DISABLE_DAC=y -# CONFIG_ADC_CONTINUOUS_FORCE_USE_ADC2_ON_C3_S3 is not set -# CONFIG_ADC_ONESHOT_FORCE_USE_ADC2_ON_C3 is not set # end of ADC configuration # @@ -556,7 +546,6 @@ CONFIG_SPI_SLAVE_ISR_IN_IRAM=y # TWAI configuration # # CONFIG_TWAI_ISR_IN_IRAM is not set -# CONFIG_TWAI_ERRATA_FIX_LISTEN_ONLY_DOM is not set # end of TWAI configuration # @@ -604,11 +593,6 @@ CONFIG_ESP32C3_DEFAULT_CPU_FREQ_MHZ=160 CONFIG_ESP32C3_REV_MIN_3=y # CONFIG_ESP32C3_REV_MIN_4 is not set CONFIG_ESP32C3_REV_MIN=3 -CONFIG_ESP32C3_REV_MIN_FULL=3 -CONFIG_ESP_REV_MIN_FULL=3 -CONFIG_ESP32C3_REV_MAX_FULL_STR_OPT=y -CONFIG_ESP32C3_REV_MAX_FULL=99 -CONFIG_ESP_REV_MAX_FULL=99 CONFIG_ESP32C3_DEBUG_OCDAWARE=y CONFIG_ESP32C3_BROWNOUT_DET=y CONFIG_ESP32C3_BROWNOUT_DET_LVL_SEL_7=y @@ -714,8 +698,6 @@ CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND=y # CONFIG_ESP_SLEEP_MSPI_NEED_ALL_IO_PU is not set # end of Sleep Config -CONFIG_ESP_SLEEP_SYSTIMER_STALL_WORKAROUND=y - # # RTC Clock Config # @@ -745,12 +727,7 @@ CONFIG_ESP_PHY_CALIBRATION_AND_DATA_STORAGE=y # CONFIG_ESP_PHY_INIT_DATA_IN_PARTITION is not set CONFIG_ESP_PHY_MAX_WIFI_TX_POWER=20 CONFIG_ESP_PHY_MAX_TX_POWER=20 -# CONFIG_ESP_PHY_REDUCE_TX_POWER is not set CONFIG_ESP_PHY_ENABLE_USB=y -CONFIG_ESP_PHY_RF_CAL_PARTIAL=y -# CONFIG_ESP_PHY_RF_CAL_NONE is not set -# CONFIG_ESP_PHY_RF_CAL_FULL is not set -CONFIG_ESP_PHY_CALIBRATION_MODE=0 # end of PHY # @@ -992,7 +969,6 @@ CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y CONFIG_LWIP_LOCAL_HOSTNAME="espressif" # CONFIG_LWIP_NETIF_API is not set # CONFIG_LWIP_TCPIP_CORE_LOCKING is not set -# CONFIG_LWIP_CHECK_THREAD_SAFETY is not set CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y # CONFIG_LWIP_L2_TO_L3_COPY is not set # CONFIG_LWIP_IRAM_OPTIMIZATION is not set @@ -1013,15 +989,12 @@ CONFIG_LWIP_IP6_FRAG=y # CONFIG_LWIP_ETHARP_TRUST_IP_MAC is not set CONFIG_LWIP_ESP_GRATUITOUS_ARP=y CONFIG_LWIP_GARP_TMR_INTERVAL=60 -CONFIG_LWIP_ESP_MLDV6_REPORT=y -CONFIG_LWIP_MLDV6_TMR_INTERVAL=40 CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32 CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y # CONFIG_LWIP_DHCP_DISABLE_CLIENT_ID is not set CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID=y # CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set CONFIG_LWIP_DHCP_OPTIONS_LEN=68 -CONFIG_LWIP_DHCP_COARSE_TIMER_SECS=1 # # DHCP server @@ -1695,7 +1668,6 @@ CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y # CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION is not set CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20 CONFIG_ESP32_PHY_MAX_TX_POWER=20 -# CONFIG_ESP32_REDUCE_PHY_TX_POWER is not set CONFIG_ESP_SYSTEM_PM_POWER_DOWN_CPU=y CONFIG_ESP32S2_PANIC_PRINT_HALT=y # CONFIG_ESP32S2_PANIC_PRINT_REBOOT is not set diff --git a/hw/bsp/esp32-c3-idf/sdkconfig b/hw/bsp/esp32-c3-idf/sdkconfig index 4c1cdc9e..59cc86f8 100644 --- a/hw/bsp/esp32-c3-idf/sdkconfig +++ b/hw/bsp/esp32-c3-idf/sdkconfig @@ -197,10 +197,7 @@ CONFIG_BT_CTRL_HCI_MODE_VHCI=y # CONFIG_BT_CTRL_HCI_MODE_UART_H4 is not set CONFIG_BT_CTRL_HCI_TL=1 CONFIG_BT_CTRL_ADV_DUP_FILT_MAX=30 -CONFIG_BT_BLE_CCA_MODE_NONE=y -# CONFIG_BT_BLE_CCA_MODE_HW is not set -# CONFIG_BT_BLE_CCA_MODE_SW is not set -CONFIG_BT_BLE_CCA_MODE=0 +# CONFIG_BT_CTRL_HW_CCA is not set CONFIG_BT_CTRL_HW_CCA_VAL=20 CONFIG_BT_CTRL_HW_CCA_EFF=0 CONFIG_BT_CTRL_CE_LENGTH_TYPE_ORIG=y @@ -239,7 +236,6 @@ CONFIG_BT_CTRL_SCAN_DUPL_TYPE_DEVICE=y # CONFIG_BT_CTRL_SCAN_DUPL_TYPE_DATA_DEVICE is not set CONFIG_BT_CTRL_SCAN_DUPL_TYPE=0 CONFIG_BT_CTRL_SCAN_DUPL_CACHE_SIZE=100 -CONFIG_BT_CTRL_DUPL_SCAN_CACHE_REFRESH_PERIOD=0 # CONFIG_BT_CTRL_BLE_MESH_SCAN_DUPL_EN is not set # CONFIG_BT_CTRL_COEX_PHY_CODED_TX_RX_TLIM_EN is not set CONFIG_BT_CTRL_COEX_PHY_CODED_TX_RX_TLIM_DIS=y @@ -278,9 +274,6 @@ CONFIG_BT_GATT_MAX_SR_ATTRIBUTES=100 # CONFIG_BT_GATTS_SEND_SERVICE_CHANGE_MANUAL is not set CONFIG_BT_GATTS_SEND_SERVICE_CHANGE_AUTO=y CONFIG_BT_GATTS_SEND_SERVICE_CHANGE_MODE=0 -# CONFIG_BT_GATTS_ROBUST_CACHING_ENABLED is not set -# CONFIG_BT_GATTS_DEVICE_NAME_WRITABLE is not set -# CONFIG_BT_GATTS_APPEARANCE_WRITABLE is not set CONFIG_BT_GATTC_ENABLE=y CONFIG_BT_GATTC_MAX_CACHE_CHAR=40 # CONFIG_BT_GATTC_CACHE_NVS_FLASH is not set @@ -471,10 +464,9 @@ CONFIG_BT_SMP_ENABLE=y # CONFIG_BT_BLE_ACT_SCAN_REP_ADV_SCAN is not set CONFIG_BT_BLE_ESTAB_LINK_CONN_TOUT=30 CONFIG_BT_MAX_DEVICE_NAME_LEN=32 -CONFIG_BT_BLE_RPA_TIMEOUT=900 +CONFIG_BT_BLE_RPA_SUPPORTED=y CONFIG_BT_BLE_50_FEATURES_SUPPORTED=y CONFIG_BT_BLE_42_FEATURES_SUPPORTED=y -# CONFIG_BT_BLE_HIGH_DUTY_ADV_INTERVAL is not set # end of Bluedroid Options # end of Bluetooth @@ -489,8 +481,6 @@ CONFIG_BT_BLE_42_FEATURES_SUPPORTED=y # # CONFIG_ADC_FORCE_XPD_FSM is not set CONFIG_ADC_DISABLE_DAC=y -# CONFIG_ADC_CONTINUOUS_FORCE_USE_ADC2_ON_C3_S3 is not set -# CONFIG_ADC_ONESHOT_FORCE_USE_ADC2_ON_C3 is not set # end of ADC configuration # @@ -512,7 +502,6 @@ CONFIG_SPI_SLAVE_ISR_IN_IRAM=y # TWAI configuration # # CONFIG_TWAI_ISR_IN_IRAM is not set -CONFIG_TWAI_ERRATA_FIX_LISTEN_ONLY_DOM=y # end of TWAI configuration # @@ -549,11 +538,6 @@ CONFIG_ESP32C3_DEFAULT_CPU_FREQ_MHZ=160 CONFIG_ESP32C3_REV_MIN_3=y # CONFIG_ESP32C3_REV_MIN_4 is not set CONFIG_ESP32C3_REV_MIN=3 -CONFIG_ESP32C3_REV_MIN_FULL=3 -CONFIG_ESP_REV_MIN_FULL=3 -CONFIG_ESP32C3_REV_MAX_FULL_STR_OPT=y -CONFIG_ESP32C3_REV_MAX_FULL=99 -CONFIG_ESP_REV_MAX_FULL=99 CONFIG_ESP32C3_DEBUG_OCDAWARE=y CONFIG_ESP32C3_BROWNOUT_DET=y CONFIG_ESP32C3_BROWNOUT_DET_LVL_SEL_7=y @@ -629,8 +613,6 @@ CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND=y # CONFIG_ESP_SLEEP_MSPI_NEED_ALL_IO_PU is not set # end of Sleep Config -CONFIG_ESP_SLEEP_SYSTIMER_STALL_WORKAROUND=y - # # RTC Clock Config # @@ -660,12 +642,7 @@ CONFIG_ESP_PHY_CALIBRATION_AND_DATA_STORAGE=y # CONFIG_ESP_PHY_INIT_DATA_IN_PARTITION is not set CONFIG_ESP_PHY_MAX_WIFI_TX_POWER=20 CONFIG_ESP_PHY_MAX_TX_POWER=20 -# CONFIG_ESP_PHY_REDUCE_TX_POWER is not set CONFIG_ESP_PHY_ENABLE_USB=y -CONFIG_ESP_PHY_RF_CAL_PARTIAL=y -# CONFIG_ESP_PHY_RF_CAL_NONE is not set -# CONFIG_ESP_PHY_RF_CAL_FULL is not set -CONFIG_ESP_PHY_CALIBRATION_MODE=0 # end of PHY # @@ -872,7 +849,6 @@ CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y CONFIG_LWIP_LOCAL_HOSTNAME="espressif" # CONFIG_LWIP_NETIF_API is not set # CONFIG_LWIP_TCPIP_CORE_LOCKING is not set -# CONFIG_LWIP_CHECK_THREAD_SAFETY is not set CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y # CONFIG_LWIP_L2_TO_L3_COPY is not set # CONFIG_LWIP_IRAM_OPTIMIZATION is not set @@ -893,15 +869,12 @@ CONFIG_LWIP_IP6_FRAG=y # CONFIG_LWIP_ETHARP_TRUST_IP_MAC is not set CONFIG_LWIP_ESP_GRATUITOUS_ARP=y CONFIG_LWIP_GARP_TMR_INTERVAL=60 -CONFIG_LWIP_ESP_MLDV6_REPORT=y -CONFIG_LWIP_MLDV6_TMR_INTERVAL=40 CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32 CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y # CONFIG_LWIP_DHCP_DISABLE_CLIENT_ID is not set CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID=y # CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set CONFIG_LWIP_DHCP_OPTIONS_LEN=68 -CONFIG_LWIP_DHCP_COARSE_TIMER_SECS=1 # # DHCP server @@ -1480,7 +1453,6 @@ CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y # CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION is not set CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20 CONFIG_ESP32_PHY_MAX_TX_POWER=20 -# CONFIG_ESP32_REDUCE_PHY_TX_POWER is not set CONFIG_ESP_SYSTEM_PM_POWER_DOWN_CPU=y CONFIG_ESP32S2_PANIC_PRINT_HALT=y # CONFIG_ESP32S2_PANIC_PRINT_REBOOT is not set diff --git a/hw/bsp/f103_can/drivers/bsp_can.c b/hw/bsp/f103_can/drivers/bsp_can.c index 89081b75..792abc8f 100644 --- a/hw/bsp/f103_can/drivers/bsp_can.c +++ b/hw/bsp/f103_can/drivers/bsp_can.c @@ -109,12 +109,34 @@ bsp_status_t bsp_can_trans_packet(bsp_can_t can, bsp_can_format_t format, uint32_t id, uint8_t *data) { CAN_TxHeaderTypeDef header; - if (format == CAN_FORMAT_STD) { - header.StdId = id; - header.IDE = CAN_ID_STD; - } else { - header.ExtId = id; - header.IDE = CAN_ID_EXT; + switch (format) { + case CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; } header.RTR = CAN_RTR_DATA; diff --git a/hw/bsp/f103_can/drivers/bsp_can.h b/hw/bsp/f103_can/drivers/bsp_can.h index 260a8e46..3e4e7fcd 100644 --- a/hw/bsp/f103_can/drivers/bsp_can.h +++ b/hw/bsp/f103_can/drivers/bsp_can.h @@ -19,8 +19,10 @@ typedef enum { } bsp_can_callback_t; typedef enum { - CAN_FORMAT_STD, - CAN_FORMAT_EXT, + CAN_FORMAT_STD_DATA, + CAN_FORMAT_EXT_DATA, + CAN_FORMAT_STD_REMOTE, + CAN_FORMAT_EXT_REMOTE, } bsp_can_format_t; void bsp_can_init(void); diff --git a/hw/bsp/microswitch/drivers/bsp_can.c b/hw/bsp/microswitch/drivers/bsp_can.c index 2d6d235d..5402a49a 100644 --- a/hw/bsp/microswitch/drivers/bsp_can.c +++ b/hw/bsp/microswitch/drivers/bsp_can.c @@ -111,12 +111,35 @@ bsp_status_t bsp_can_trans_packet(bsp_can_t can, bsp_can_format_t format, uint32_t id, uint8_t *data) { CAN_TxHeaderTypeDef header; i++; - if (format == CAN_FORMAT_STD) { - header.StdId = id; - header.IDE = CAN_ID_STD; - } else { - header.ExtId = id; - header.IDE = CAN_ID_EXT; + + switch (format) { + case CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; } header.RTR = CAN_RTR_DATA; diff --git a/hw/bsp/microswitch/drivers/bsp_can.h b/hw/bsp/microswitch/drivers/bsp_can.h index 260a8e46..3e4e7fcd 100644 --- a/hw/bsp/microswitch/drivers/bsp_can.h +++ b/hw/bsp/microswitch/drivers/bsp_can.h @@ -19,8 +19,10 @@ typedef enum { } bsp_can_callback_t; typedef enum { - CAN_FORMAT_STD, - CAN_FORMAT_EXT, + CAN_FORMAT_STD_DATA, + CAN_FORMAT_EXT_DATA, + CAN_FORMAT_STD_REMOTE, + CAN_FORMAT_EXT_REMOTE, } bsp_can_format_t; void bsp_can_init(void); diff --git a/hw/bsp/node_imu/drivers/bsp_can.c b/hw/bsp/node_imu/drivers/bsp_can.c index 040614b8..e27e3354 100644 --- a/hw/bsp/node_imu/drivers/bsp_can.c +++ b/hw/bsp/node_imu/drivers/bsp_can.c @@ -155,12 +155,34 @@ bsp_status_t bsp_can_trans_packet(bsp_can_t can, bsp_can_format_t format, uint32_t id, uint8_t *data) { CAN_TxHeaderTypeDef header; - if (format == CAN_FORMAT_STD) { - header.StdId = id; - header.IDE = CAN_ID_STD; - } else { - header.ExtId = id; - header.IDE = CAN_ID_EXT; + switch (format) { + case CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; } header.RTR = CAN_RTR_DATA; diff --git a/hw/bsp/node_imu/drivers/bsp_can.h b/hw/bsp/node_imu/drivers/bsp_can.h index 260a8e46..3e4e7fcd 100644 --- a/hw/bsp/node_imu/drivers/bsp_can.h +++ b/hw/bsp/node_imu/drivers/bsp_can.h @@ -19,8 +19,10 @@ typedef enum { } bsp_can_callback_t; typedef enum { - CAN_FORMAT_STD, - CAN_FORMAT_EXT, + CAN_FORMAT_STD_DATA, + CAN_FORMAT_EXT_DATA, + CAN_FORMAT_STD_REMOTE, + CAN_FORMAT_EXT_REMOTE, } bsp_can_format_t; void bsp_can_init(void); diff --git a/hw/bsp/rm-c/drivers/bsp_can.c b/hw/bsp/rm-c/drivers/bsp_can.c index 646a802c..6c107dfb 100644 --- a/hw/bsp/rm-c/drivers/bsp_can.c +++ b/hw/bsp/rm-c/drivers/bsp_can.c @@ -178,19 +178,35 @@ bsp_status_t bsp_can_register_callback( bsp_status_t bsp_can_trans_packet(bsp_can_t can, bsp_can_format_t format, uint32_t id, uint8_t *data) { CAN_TxHeaderTypeDef header; - - if (format == CAN_FORMAT_STD) { - header.StdId = id; - header.IDE = CAN_ID_STD; - } else { - header.ExtId = id; - header.IDE = CAN_ID_EXT; + switch (format) { + case CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; + break; + case CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + header.TransmitGlobalTime = DISABLE; + header.DLC = 8; } - - header.RTR = CAN_RTR_DATA; - header.TransmitGlobalTime = DISABLE; - header.DLC = 8; - uint32_t tsr = READ_REG(bsp_can_get_handle(can)->Instance->TSR); while (((tsr & CAN_TSR_TME0) == 0U) && ((tsr & CAN_TSR_TME1) == 0U) && diff --git a/hw/bsp/rm-c/drivers/bsp_can.h b/hw/bsp/rm-c/drivers/bsp_can.h index 708ba1d7..0873e55e 100644 --- a/hw/bsp/rm-c/drivers/bsp_can.h +++ b/hw/bsp/rm-c/drivers/bsp_can.h @@ -20,8 +20,10 @@ typedef enum { } bsp_can_callback_t; typedef enum { - CAN_FORMAT_STD, - CAN_FORMAT_EXT, + CAN_FORMAT_STD_DATA, + CAN_FORMAT_EXT_DATA, + CAN_FORMAT_STD_REMOTE, + CAN_FORMAT_EXT_REMOTE, } bsp_can_format_t; void bsp_can_init(void); diff --git a/src/component/comp_utils.cpp b/src/component/comp_utils.cpp index 774a0fff..18f55c55 100644 --- a/src/component/comp_utils.cpp +++ b/src/component/comp_utils.cpp @@ -92,7 +92,7 @@ float bullet_speed_to_fric_rpm(float bullet_speed, float fric_radius, return 4450.f; } if (bullet_speed == 16.0f) { - return 5750.f; + return 5700.f; } } diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index a5b309cc..6acd13f0 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -35,13 +35,13 @@ AI::AI(bool autoscan_enable) auto quat_sub = Message::Subscriber("imu_quat"); auto ref_sub = Message::Subscriber("referee"); - auto gimbal_sub = Message::Subscriber("gimbal_data"); + auto yaw_sub = Message::Subscriber("chassis_yaw"); auto eulr_sub = Message::Subscriber("imu_eulr"); while (1) { /* 接收云台数据 */ - gimbal_sub.DumpData(ai->chassis_yaw_offset_); + yaw_sub.DumpData(ai->chassis_yaw_offset_); eulr_sub.DumpData(ai->eulr_); /* imu */ /* 接收裁判系统数据 */ diff --git a/src/device/can/dev_can.cpp b/src/device/can/dev_can.cpp index 46de2507..a0467f23 100644 --- a/src/device/can/dev_can.cpp +++ b/src/device/can/dev_can.cpp @@ -44,16 +44,32 @@ bool Can::SendPack(bsp_can_t can, bsp_can_format_t format, Pack& pack) { bool Can::SendStdPack(bsp_can_t can, Pack& pack) { can_sem_[can]->Wait(UINT32_MAX); - bool ans = bsp_can_trans_packet(can, CAN_FORMAT_STD, pack.index, pack.data) == - BSP_OK; + bool ans = bsp_can_trans_packet(can, CAN_FORMAT_STD_DATA, pack.index, + pack.data) == BSP_OK; can_sem_[can]->Post(); return ans; } bool Can::SendExtPack(bsp_can_t can, Pack& pack) { can_sem_[can]->Wait(UINT32_MAX); - bool ans = bsp_can_trans_packet(can, CAN_FORMAT_EXT, pack.index, pack.data) == - BSP_OK; + bool ans = bsp_can_trans_packet(can, CAN_FORMAT_EXT_DATA, pack.index, + pack.data) == BSP_OK; + can_sem_[can]->Post(); + return ans; +} + +bool Can::SendStdRemotePack(bsp_can_t can, Pack& pack) { + can_sem_[can]->Wait(UINT32_MAX); + bool ans = bsp_can_trans_packet(can, CAN_FORMAT_STD_REMOTE, pack.index, + pack.data) == BSP_OK; + can_sem_[can]->Post(); + return ans; +} + +bool Can::SendExtRemotePack(bsp_can_t can, Pack& pack) { + can_sem_[can]->Wait(UINT32_MAX); + bool ans = bsp_can_trans_packet(can, CAN_FORMAT_EXT_REMOTE, pack.index, + pack.data) == BSP_OK; can_sem_[can]->Post(); return ans; } diff --git a/src/device/can/dev_can.hpp b/src/device/can/dev_can.hpp index 633a8b38..6c15a19c 100644 --- a/src/device/can/dev_can.hpp +++ b/src/device/can/dev_can.hpp @@ -20,6 +20,10 @@ class Can { static bool SendExtPack(bsp_can_t can, Pack& pack); + static bool SendStdRemotePack(bsp_can_t can, Pack& pack); + + static bool SendExtRemotePack(bsp_can_t can, Pack& pack); + static bool Subscribe(Message::Topic& tp, bsp_can_t can, uint32_t index, uint32_t num); diff --git a/src/device/cap/dev_cap.cpp b/src/device/cap/dev_cap.cpp index c8fafa2a..2aacda98 100644 --- a/src/device/cap/dev_cap.cpp +++ b/src/device/cap/dev_cap.cpp @@ -4,38 +4,42 @@ #include "dev_referee.hpp" #define CAP_RES (100.0f) /* 电容数据分辨率 */ - +#define V_MAX (23.0f) +#define V_MIN (16.0f) using namespace Device; Cap::Cap(Cap::Param ¶m) : param_(param), info_tp_("cap_info") { - XB_ASSERT(param.cutoff_volt > 3.0f && param.cutoff_volt < 24.0f); - out_.power_limit_ = 40.0f; auto rx_callback = [](Can::Pack &rx, Cap *cap) { - rx.index -= cap->param_.index; - - if (rx.index == 0) { - cap->control_feedback_.Overwrite(rx); - } - + cap->control_feedback_.Overwrite(rx); + cap->can_recv_.Post(); return true; }; Message::Topic cap_tp("can_cap"); cap_tp.RegisterCallback(rx_callback, this); - Can::Subscribe(cap_tp, this->param_.can, this->param_.index, 1); + Can::Subscribe(cap_tp, this->param_.can, 0x600, 19); auto ref_cb = [](Device::Referee::Data &ref, Cap *cap) { if (ref.status != Device::Referee::RUNNING) { cap->out_.power_limit_ = 40.0f; } else { - cap->out_.power_limit_ = ref.robot_status.chassis_power_limit; + float power_buff_percentage = 0.0f; + if (ref.power_heat.chassis_pwr_buff >= 40) { + power_buff_percentage = 1.0f; + } else { + power_buff_percentage = + (ref.power_heat.chassis_pwr_buff - 20.0f) / 20.0f; + } + clampf(&power_buff_percentage, 0.0f, 1.0f); + cap->out_.power_limit_ = + static_cast(ref.robot_status.chassis_power_limit) - 3.0f + + 10.0f * power_buff_percentage; } - - clampf(&cap->out_.power_limit_, 20.0f, 100.0f); - + clampf(&cap->out_.power_limit_, 0.0f, 150.0f); + cap->power_limit_update_.Post(); return true; }; @@ -44,19 +48,24 @@ Cap::Cap(Cap::Param ¶m) : param_(param), info_tp_("cap_info") { .RegisterCallback(ref_cb, this); auto cap_thread = [](Cap *cap) { - uint32_t last_online_time = bsp_time_get_ms(); while (1) { /* 读取裁判系统信息 */ if (!cap->Update()) { /* 一定时间长度内接收不到电容反馈值，使电容离线 */ cap->Offline(); } - cap->info_tp_.Publish(cap->info_); - - cap->Control(); + cap->InstructUpdata(); + cap->Control(INSTRUCT); + cap->Control(OUTPUT_VOLT); + cap->Control(OUTPUT); + if (cap->power_limit_update_.Wait(10)) { + cap->Control(POWER_LIMIT); + } - /* 运行结束，等待下一次唤醒 */ - cap->thread_.SleepUntil(100, last_online_time); + if (cap->can_recv_.Wait(10)) { + cap->Update(); + } + cap->info_tp_.Publish(cap->info_); } }; @@ -67,6 +76,14 @@ Cap::Cap(Cap::Param ¶m) : param_(param), info_tp_("cap_info") { System::Timer::Create(this->DrawUIDynamic, this, 200); } +bool Cap::InstructUpdata() { + if (info_.online_ == 1) { + instruct_ = 2; + } else { + instruct_ = 0; + } + return true; +} bool Cap::Update() { Can::Pack rx; while (this->control_feedback_.Receive(rx)) { @@ -85,45 +102,81 @@ bool Cap::Update() { void Cap::Decode(Can::Pack &rx) { uint8_t *raw = rx.data; - this->info_.input_volt_ = - static_cast((raw[1] << 8) | raw[0]) / CAP_RES; - this->info_.cap_volt_ = static_cast((raw[3] << 8) | raw[2]) / CAP_RES; - this->info_.input_curr_ = - static_cast((raw[5] << 8) | raw[4]) / CAP_RES; - this->info_.target_power_ = - static_cast((raw[7] << 8) | raw[6]) / CAP_RES; - + switch (rx.index) { + case OUTPUT: { + this->info_.output_power_ = + static_cast((raw[0] << 8) | raw[1]) / 100; + this->info_.cap_volt_ = static_cast((raw[2] << 8) | raw[3]) / 100; + this->info_.output_curr_ = + static_cast((raw[4] << 8) | raw[5]) / 100; + break; + } + case INSTRUCT: { + this->info_.cap_instruct_ = static_cast((raw[0] << 8) | raw[1]); + break; + } + case POWER_LIMIT: { + this->info_.target_power_ = + static_cast((raw[0] << 8) | raw[1]) / 100; + break; + } + case OUTPUT_VOLT: { + this->info_.cap_volt_max_ = + static_cast((raw[0] << 8) | raw[1]) / 100; + break; + } + } + this->info_.online_ = true; /* 更新电容状态和百分比 */ this->info_.percentage_ = this->GetPercentage(); } -bool Cap::Control() { - uint16_t pwr_lim = static_cast(this->out_.power_limit_ * CAP_RES); - +bool Cap::Control(CanID can_id_) { Can::Pack tx_buff; - - tx_buff.index = DEV_CAP_CTRL_ID_BASE; - - tx_buff.data[0] = (pwr_lim >> 8) & 0xFF; - tx_buff.data[1] = pwr_lim & 0xFF; - - return Can::SendStdPack(this->param_.can, tx_buff); + tx_buff.index = can_id_; + switch (tx_buff.index) { + case OUTPUT_VOLT: { + uint16_t cap_volt = 2300; + tx_buff.data[0] = (cap_volt >> 8) & 0xFF; + tx_buff.data[1] = cap_volt & 0xFF; + Can::SendStdPack(this->param_.can, tx_buff); + break; + } + case OUTPUT: { + Can::SendStdRemotePack(this->param_.can, tx_buff); + break; + } + case INSTRUCT: { + tx_buff.data[0] = (instruct_ >> 8) & 0XFF; + tx_buff.data[1] = instruct_ & 0XFF; + Can::SendStdPack(this->param_.can, tx_buff); + break; + } + case POWER_LIMIT: { + uint16_t pwr_lim = static_cast(this->out_.power_limit_ * 100); + tx_buff.data[0] = (pwr_lim >> 8) & 0xFF; + tx_buff.data[1] = pwr_lim & 0xFF; + Can::SendStdPack(this->param_.can, tx_buff); + break; + } + } + return true; } bool Cap::Offline() { this->info_.cap_volt_ = 0; - this->info_.input_curr_ = 0; - this->info_.input_volt_ = 0; + this->info_.output_curr_ = 0; this->info_.target_power_ = 0; this->info_.online_ = 0; - + this->info_.cap_instruct_ = 0; + this->info_.cap_volt_max_ = 0; return true; } float Cap::GetPercentage() { - const float C_MAX = this->info_.input_volt_ * this->info_.input_volt_; + const float C_MAX = V_MAX * V_MAX; const float C_CAP = this->info_.cap_volt_ * this->info_.cap_volt_; - const float C_MIN = param_.cutoff_volt * param_.cutoff_volt; + const float C_MIN = V_MIN * V_MIN; float percentage = (C_CAP - C_MIN) / (C_MAX - C_MIN); clampf(&percentage, 0.0f, 1.0f); return percentage; diff --git a/src/device/cap/dev_cap.hpp b/src/device/cap/dev_cap.hpp index 65fc8d8b..850a1182 100644 --- a/src/device/cap/dev_cap.hpp +++ b/src/device/cap/dev_cap.hpp @@ -5,6 +5,7 @@ #include "bsp_can.h" #include "comp_ui.hpp" #include "dev_can.hpp" +#include "dev_motor.hpp" #include "dev_referee.hpp" #define DEV_CAP_FB_ID_BASE (0x211) @@ -14,11 +15,13 @@ namespace Device { class Cap { public: typedef struct { - float input_volt_; float cap_volt_; - float input_curr_; - float target_power_; + float output_curr_; + float output_power_; float percentage_; + float target_power_; + uint8_t cap_instruct_; + float cap_volt_max_; bool online_; } Info; @@ -26,20 +29,60 @@ class Cap { float power_limit_; } Output; + typedef enum { + INSTRUCT = 0X600, + POWER_LIMIT = 0X601, + OUTPUT_VOLT = 0X602, + OUTPUT_CUR = 0X603, + STATE = 0X610, + INPUT = 0X611, + OUTPUT = 0X612, + TP_TIME = 0X613, + } CanID; + typedef struct { + uint16_t ready : 1; + uint16_t operate : 1; + uint16_t alarm : 1; + uint16_t powerswitch : 1; + uint16_t loadswitch : 1; + uint16_t const_vlot : 1; + uint16_t const_cur : 1; + uint16_t const_power : 1; + uint16_t retain : 7; + uint16_t err : 1; + } ModuleState; + typedef enum { + NORMAL, + INPUT_UNDERVOLT, + INPUT_OVERVOLT, + INPUT_OVERCUR, + INPUT_OVERPOWER, + PROTECT_OVERTP, + PROTECT_LOWTP, + OUTPUT_OVERVOLT, + OUTPUT_OVERCUR, + OUTPUT_OVERPOWER, + ZERO_OVERDRIFT, + REVERSE_ERRO, + FAILURE_CONTROL, + FAILURE_COMMUNICATION, + FAILURE_ERR, + } CapState; + typedef struct { bsp_can_t can; - uint32_t index; - float cutoff_volt; } Param; Cap(Param& param); bool Update(); - bool Control(); + bool Control(CanID can_id_); bool Offline(); + bool InstructUpdata(); + void Decode(Can::Pack& rx); float GetPercentage(); @@ -49,10 +92,16 @@ class Cap { static void DrawUIDynamic(Cap* cap); private: + System::Semaphore can_recv_ = System::Semaphore(true); + System::Semaphore power_limit_update_ = System::Semaphore(true); Param param_; uint32_t last_online_time_ = 0; + uint16_t instruct_ = 0; + + Device::BaseMotor::Feedback feedback_; + System::Queue control_feedback_ = System::Queue(1); System::Thread thread_; diff --git a/src/device/motor/dev_mit_motor.cpp b/src/device/motor/dev_mit_motor.cpp index 9731793b..2b7d7baa 100644 --- a/src/device/motor/dev_mit_motor.cpp +++ b/src/device/motor/dev_mit_motor.cpp @@ -15,16 +15,14 @@ using namespace Device; -static const uint8_t RELAX_CMD[8] = {0X7F, 0XFF, 0X7F, 0XF0, - 0X00, 0X00, 0X07, 0XFF}; +// 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}; -/* -static const uint8_t RESET_CMD[8] = {0XFF, 0XFF, 0XFF, 0XFF, +static const uint8_t RELAX_CMD[8] = {0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFD}; -*/ static std::array initd = {false}; @@ -33,7 +31,7 @@ std::array *, BSP_CAN_NUM> MitMotor::mit_tp_; 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] == motor->param_.id) { + if ((rx.data[0] & 0x0f) == motor->param_.id) { motor->recv_.Overwrite(rx); } @@ -71,9 +69,41 @@ bool MitMotor::Update() { return true; } +void MitMotor::SetMit(float out) { + clampf(&out, -1.0f, 1.0f); + if (this->feedback_.temp > 75.0f) { + Relax(); + OMLOG_WARNING("motor %s high temperature detected", name_); + return; + } + int p_int = float_to_uint(0.0f, P_MIN, P_MAX, 16); + int v_int = float_to_uint(0.0f, V_MIN, V_MAX, 12); + int kp_int = float_to_uint(0.0f, KP_MIN, KP_MAX, 12); + 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); + } else { + t_int = float_to_uint(out * T_MAX, T_MIN, T_MAX, 12); + } + Can::Pack tx_buff; + + tx_buff.index = this->param_.id; + tx_buff.data[0] = p_int >> 8; + tx_buff.data[1] = p_int & 0xFF; + tx_buff.data[2] = v_int >> 4; + tx_buff.data[3] = ((v_int & 0xF) << 4) | (kp_int >> 8); + tx_buff.data[4] = kp_int & 0xFF; + tx_buff.data[5] = kd_int >> 4; + tx_buff.data[6] = ((kd_int & 0xF) << 4) | (t_int >> 8); + 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]) { + if (this->param_.id != (rx.data[0] & 0x0f)) { return; } @@ -125,7 +155,7 @@ void MitMotor::SetPos(float pos) { pos_sp = -pos_sp; } - clampf(&pos_sp, -param_.max_error, param_.max_error); + // clampf(&pos_sp, -param_.max_error, param_.max_error); pos_sp += this->raw_pos_; @@ -138,7 +168,7 @@ void MitMotor::SetPos(float pos) { } int p_int = float_to_uint(pos_sp, P_MIN, P_MAX, 16); - int v_int = float_to_uint(this->param_.def_speed, V_MIN, V_MAX, 12); + int v_int = float_to_uint(0.0f, V_MIN, V_MAX, 12); int kp_int = float_to_uint(this->param_.kp, KP_MIN, KP_MAX, 12); int kd_int = float_to_uint(this->param_.kd, KD_MIN, KD_MAX, 12); int t_int = float_to_uint(this->current_, T_MIN, T_MAX, 12); @@ -157,6 +187,7 @@ void MitMotor::SetPos(float pos) { tx_buff.data[7] = t_int & 0xff; Can::SendStdPack(this->param_.can, tx_buff); + this->Update(); } void MitMotor::Relax() { @@ -169,12 +200,16 @@ void MitMotor::Relax() { Can::SendStdPack(this->param_.can, tx_buff); } -void MitMotor::Enable() { +bool MitMotor::Enable() { Can::Pack tx_buff; - tx_buff.index = param_.id; + tx_buff.index = this->param_.id; memcpy(tx_buff.data, ENABLE_CMD, sizeof(ENABLE_CMD)); - Can::SendStdPack(this->param_.can, tx_buff); + if (Can::SendStdPack(this->param_.can, tx_buff)) { + return true; + } else { + return false; + }; } diff --git a/src/device/motor/dev_mit_motor.hpp b/src/device/motor/dev_mit_motor.hpp index 6381f164..0aa6ffb3 100644 --- a/src/device/motor/dev_mit_motor.hpp +++ b/src/device/motor/dev_mit_motor.hpp @@ -11,10 +11,9 @@ class MitMotor : public BaseMotor { typedef struct { float kp; float kd; - float def_speed; + uint32_t feedback_id; uint32_t id; bsp_can_t can; - float max_error; bool reverse; } Param; @@ -32,7 +31,8 @@ class MitMotor : public BaseMotor { void SetPos(float pos); - void Enable(); + void SetMit(float out); + bool Enable(); private: Param param_; diff --git a/src/module/can_usart/can_usart.cpp b/src/module/can_usart/can_usart.cpp index 8a887aad..ff0cee46 100644 --- a/src/module/can_usart/can_usart.cpp +++ b/src/module/can_usart/can_usart.cpp @@ -61,7 +61,7 @@ CantoUsart::CantoUsart() auto rx_callback = [](Device::Can::Pack &rx, CantoUsart *can_uart) { can_uart->uart_trans_buff_.start_frame = START; can_uart->uart_trans_buff_.id = rx.index; - can_uart->uart_trans_buff_.type = CAN_FORMAT_STD; + can_uart->uart_trans_buff_.type = CAN_FORMAT_STD_DATA; memcpy(&can_uart->uart_trans_buff_.data, rx.data, sizeof(rx.data)); can_uart->uart_trans_buff_.end_frame = END; can_uart->tx_ready_.Post(); diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp index ffad075c..5a261aff 100644 --- a/src/module/chassis/mod_chassis.cpp +++ b/src/module/chassis/mod_chassis.cpp @@ -11,16 +11,18 @@ #include "mod_chassis.hpp" +#include + #include #include "bsp_time.h" -#define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */ -#define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */ +#define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */ +#define ROTOR_WZ_MAX 1.0f /* 小陀螺旋转位移上界 */ #define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */ -#define MOTOR_MAX_ROTATIONAL_SPEED 7000.0f /* 电机的最大转速 */ +#define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */ #if POWER_LIMIT_WITH_CAP /* 保证电容电量宏定义在正确范围内 */ @@ -95,7 +97,7 @@ Chassis::Chassis(Param& param, float control_freq) auto cmd_sub = Message::Subscriber("cmd_chassis"); - auto yaw_sub = Message::Subscriber("gimbal_data"); + auto yaw_sub = Message::Subscriber("chassis_yaw"); auto cap_sub = Message::Subscriber("cap_info"); @@ -105,7 +107,7 @@ Chassis::Chassis(Param& param, float control_freq) /* 读取控制指令、电容、裁判系统、电机反馈 */ cmd_sub.DumpData(chassis->cmd_); raw_ref_sub.DumpData(chassis->raw_ref_); - yaw_sub.DumpData(chassis->gimbal_data_); + yaw_sub.DumpData(chassis->yaw_); cap_sub.DumpData(chassis->cap_); /* 更新反馈值 */ @@ -134,9 +136,44 @@ void Chassis::UpdateFeedback() { /* 将CAN中的反馈数据写入到feedback中 */ for (size_t i = 0; i < this->mixer_.len_; i++) { this->motor_[i]->Update(); + 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[len]; + 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(); @@ -145,6 +182,7 @@ 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_) { @@ -163,7 +201,7 @@ void Chassis::Control() { case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 */ case Chassis::ROTOR: { - float beta = this->gimbal_data_.yaw_; + 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; @@ -185,7 +223,7 @@ void Chassis::Control() { case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台 */ this->move_vec_.wz = - this->follow_pid_.Calculate(0.0f, this->gimbal_data_.yaw_, this->dt_); + this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_); break; case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转 @@ -211,22 +249,39 @@ void Chassis::Control() { case Chassis::ROTOR: case Chassis::INDENPENDENT: /* 独立模式,受PID控制 */ { float percentage = 0.0f; - if (cap_.online_) { - percentage = cap_.percentage_; - } else if (ref_.status == Device::Referee::RUNNING) { - percentage = this->ref_.chassis_pwr_buff / 60.0f; + if (ref_.status == Device::Referee::RUNNING) { + if (ref_.chassis_pwr_buff > 30) { + percentage = 1.0f; + + } else { + percentage = this->ref_.chassis_pwr_buff / 30.0f; + } } 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( + out_.motor_out[i] = this->actuator_[i]->Calculate( this->setpoint_.motor_rotational_speed[i] * - MOTOR_MAX_ROTATIONAL_SPEED, + max_motor_rotational_speed_, this->motor_[i]->GetSpeed(), this->dt_); - this->motor_[i]->Control(out_.motor3508_out[i]); + } + for (unsigned i = 0; i < this->mixer_.len_; i++) { + if (cap_.online_) { + LimitChassisOutPower(max_power_limit, out_.motor_out, motor_feedback_, + this->mixer_.len_); + this->motor_[i]->Control(out_.motor_out[i]); + + } else { + LimitChassisOutPower(ref_.chassis_power_limit, out_.motor_out, + motor_feedback_, this->mixer_.len_); + this->motor_[i]->Control(out_.motor_out[i]); + } } break; @@ -253,9 +308,7 @@ void Chassis::PraseRef() { template float Chassis::CalcWz(const float LO, const float HI) { - float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * - (static_cast(bsp_time_get_ms())))) + - LO; + float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO; clampf(&wz_vary, LO, HI); return wz_vary; } @@ -381,5 +434,4 @@ void Chassis::DrawUIDynamic( Device::Referee::AddUI(chassis->rectange_); } } - template class Module::Chassis; diff --git a/src/module/chassis/mod_chassis.hpp b/src/module/chassis/mod_chassis.hpp index ecb110dd..896e13e8 100644 --- a/src/module/chassis/mod_chassis.hpp +++ b/src/module/chassis/mod_chassis.hpp @@ -43,13 +43,13 @@ class Chassis { SET_MODE_INDENPENDENT, } ChassisEvent; - typedef struct { - float yaw_; - Component::Type::Eulr mech_zero; - } GimbalData; - /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 */ typedef struct Param { + float toque_coefficient_; + float speed_2_coefficient_; + float out_2_coefficient_; + float constant_; + Component::Mixer::Mode type = Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */ @@ -60,6 +60,7 @@ class Chassis { std::array actuator_param{}; std::array motor_param; + float (*get_speed)(float); } Param; typedef struct { @@ -77,7 +78,9 @@ class Chassis { void SetMode(Mode mode); - void PackOutput(); + bool LimitChassisOutPower(float power_limit, float *motor_out, float *speed, + uint32_t len); + uint16_t MAXSPEEDGET(float power_limit); void PraseRef(); @@ -90,8 +93,12 @@ class Chassis { private: Param param_; + float max_motor_rotational_speed_ = 0.0f; + float dt_ = 0.0f; + float chassis_current_; + uint64_t last_wakeup_ = 0; uint64_t now_ = 0; @@ -118,18 +125,19 @@ class Chassis { float *motor_rotational_speed; /* 电机转速的动态数组，单位：RPM */ } setpoint_; - /* 反馈控制用的PID */ + float motor_feedback_[4]; struct { - float motor3508_out[4]; + float motor_out[4]; } out_; + Component::PID follow_pid_; /* 跟随云台用的PID */ System::Thread thread_; System::Semaphore ctrl_lock_; - GimbalData gimbal_data_; + float yaw_; Device::Referee::Data raw_ref_; Component::CMD::ChassisCMD cmd_; diff --git a/src/module/helm_chassis/mod_helm_chassis.cpp b/src/module/helm_chassis/mod_helm_chassis.cpp index 63d7fc68..c412d4b0 100644 --- a/src/module/helm_chassis/mod_helm_chassis.cpp +++ b/src/module/helm_chassis/mod_helm_chassis.cpp @@ -1,3 +1,6 @@ +#include "mod_helm_chassis.hpp" + +using namespace Module; /** * @file chassis.c * @author Qu Shen (503578404@qq.com) @@ -9,8 +12,6 @@ * */ -#include "mod_helm_chassis.hpp" - #include #include @@ -24,9 +25,10 @@ #include #include "bsp_time.h" +#include "mod_helm_chassis.hpp" -#define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */ -#define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */ +#define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */ +#define ROTOR_WZ_MAX 1.0f /* 小陀螺旋转位移上界 */ #define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */ @@ -51,13 +53,16 @@ static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f; #endif -#define MOTOR_MAX_ROTATIONAL_SPEED 7000.0f /* 电机的最大转速 */ +#define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */ using namespace Module; template HelmChassis::HelmChassis(Param& param, float control_freq) - : param_(param), mode_(HelmChassis::RELAX), ctrl_lock_(true) { + : param_(param), + mode_(HelmChassis::RELAX), + follow_pid_(param.follow_pid_param, control_freq), + ctrl_lock_(true) { memset(&(this->cmd_), 0, sizeof(this->cmd_)); for (int i = 0; i < 4; i++) { this->pos_actr_.at(i) = @@ -84,8 +89,11 @@ HelmChassis::HelmChassis(Param& param, float control_freq) case SET_MODE_RELAX: chassis->SetMode(RELAX); break; - case SET_MODE_FOLLOW: - chassis->SetMode(FOLLOW_GIMBAL); + case SET_MODE_6020_FOLLOW: + chassis->SetMode(CHASSIS_6020_FOLLOW_GIMBAL); + break; + case SET_MODE_CHASSIS_FOLLOW: + chassis->SetMode(CHASSIS_FOLLOW_GIMBAL); break; case SET_MODE_ROTOR: chassis->SetMode(ROTOR); @@ -147,16 +155,62 @@ void HelmChassis::PraseRef() { this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt; this->ref_.status = this->raw_ref_.status; } - template void HelmChassis::UpdateFeedback() { /* 将CAN中的反馈数据写入到feedback中 */ for (size_t i = 0; i < 4; i++) { this->pos_motor_[i]->Update(); + this->pos_motor_feedback_[i] = this->pos_motor_[i]->GetSpeed(); this->speed_motor_[i]->Update(); + this->speed_motor_feedback_[i] = this->speed_motor_[i]->GetSpeed(); } } +template +bool HelmChassis::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_3508_power[len]; + for (size_t i = 0; i < len; i++) { + motor_3508_power[i] = + this->param_.toque_coefficient_3508 * fabsf(motor_out[i]) * + fabsf(speed_rpm[i]) + + this->param_.speed_2_coefficient_3508 * speed_rpm[i] * speed_rpm[i] + + this->param_.out_2_coefficient_3508 * motor_out[i] * motor_out[i]; + sum_motor_power += motor_3508_power[i]; + } + sum_motor_power += this->param_.constant_3508; + 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 +uint16_t HelmChassis::MAXSPEEDGET(float power_limit) { + uint16_t speed = 0; + if (power_limit <= 50.0f) { + speed = 5000; + } else if (power_limit <= 60.0f) { + speed = 5500; + } else if (power_limit <= 70.0f) { + speed = 6000; + } else if (power_limit <= 80.0f) { + speed = 6500; + } else if (power_limit <= 100.0f) { + speed = 7000; + } else { + speed = 7500; + } + return speed; +} template void HelmChassis::Control() { this->now_ = bsp_time_get(); @@ -164,7 +218,9 @@ void HelmChassis::Control() { this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); this->last_wakeup_ = this->now_; - + // max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit); + max_motor_rotational_speed_ = + this->MAXSPEEDGET(this->ref_.chassis_power_limit); /* 计算vx、vy */ switch (this->mode_) { case HelmChassis::BREAK: /* 刹车/放松模式电机停止 */ @@ -174,7 +230,7 @@ void HelmChassis::Control() { break; /* 独立模式控制向量与运动向量相等 */ case HelmChassis::INDENPENDENT: - case HelmChassis::FOLLOW_GIMBAL: { + case HelmChassis::CHASSIS_6020_FOLLOW_GIMBAL: { float tmp = sqrtf(cmd_.x * cmd_.x + cmd_.y * cmd_.y) * 1.41421f / 2.0f; clampf(&tmp, -1.0f, 1.0f); @@ -188,6 +244,7 @@ void HelmChassis::Control() { } break; } + case HelmChassis::CHASSIS_FOLLOW_GIMBAL: case HelmChassis::ROTOR: { float beta = this->yaw_; float cos_beta = cosf(beta); @@ -209,10 +266,15 @@ void HelmChassis::Control() { this->move_vec_.wz = this->cmd_.z; this->main_direct_ -= this->move_vec_.wz * WZ_MAX_OMEGA * dt_; break; - case HelmChassis::FOLLOW_GIMBAL: + case HelmChassis::CHASSIS_6020_FOLLOW_GIMBAL: /* 跟随模式每个轮子的方向与云台相同 */ this->move_vec_.wz = 0; this->main_direct_ = -yaw_; + break; + case HelmChassis::CHASSIS_FOLLOW_GIMBAL: + this->move_vec_.wz = + this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_) * 0.25f; + break; case HelmChassis::ROTOR: /* 陀螺模式底盘以一定速度旋转 */ @@ -237,24 +299,27 @@ void HelmChassis::Control() { speed = 0.0f; } break; - case HelmChassis::FOLLOW_GIMBAL: + + case HelmChassis::CHASSIS_6020_FOLLOW_GIMBAL: case HelmChassis::INDENPENDENT: /* 独立模式,受PID控制 */ for (auto& angle : setpoint_.wheel_pos) { angle = main_direct_ + direct_offset_; } for (auto& speed : setpoint_.motor_rotational_speed) { - speed = MOTOR_MAX_ROTATIONAL_SPEED * move_vec_.vy; + speed = max_motor_rotational_speed_ * move_vec_.vy; } break; + case HelmChassis::CHASSIS_FOLLOW_GIMBAL: case HelmChassis::ROTOR: { float x = 0, y = 0, wheel_pos = 0; for (int i = 0; i < 4; i++) { - wheel_pos = -i * M_PI / 2.0f + M_PI / 4.0f; + wheel_pos = -i * M_PI / 2.0f + M_PI / 4.0f * 3.0f; x = sinf(wheel_pos) * move_vec_.wz + move_vec_.vx; y = cosf(wheel_pos) * move_vec_.wz + move_vec_.vy; setpoint_.wheel_pos[i] = -(atan2(y, x) - M_PI / 2.0f); - setpoint_.motor_rotational_speed[i] = - MOTOR_MAX_ROTATIONAL_SPEED * sqrtf(x * x + y * y) * 1.41421f / 2.0f; + setpoint_.motor_rotational_speed[i] = max_motor_rotational_speed_ * + sqrtf(x * x + y * y) * 1.41421f / + 2.0f; } break; } @@ -274,41 +339,52 @@ void HelmChassis::Control() { /* 输出计算 */ for (int i = 0; i < 4; i++) { if (motor_reverse_[i]) { - speed_motor_out_[i] = + out_.speed_motor_out[i] = speed_actr_[i]->Calculate(-setpoint_.motor_rotational_speed[i], speed_motor_[i]->GetSpeed(), dt_); - pos_motor_out_[i] = pos_actr_[i]->Calculate( + out_.motor6020_out[i] = pos_actr_[i]->Calculate( setpoint_.wheel_pos[i] + M_PI + this->param_.mech_zero[i], pos_motor_[i]->GetSpeed(), pos_motor_[i]->GetAngle(), dt_); } else { - speed_motor_out_[i] = + out_.speed_motor_out[i] = speed_actr_[i]->Calculate(setpoint_.motor_rotational_speed[i], speed_motor_[i]->GetSpeed(), dt_); - pos_motor_out_[i] = pos_actr_[i]->Calculate( + out_.motor6020_out[i] = pos_actr_[i]->Calculate( setpoint_.wheel_pos[i] + this->param_.mech_zero[i], pos_motor_[i]->GetSpeed(), pos_motor_[i]->GetAngle(), dt_); } } float percentage = 0.0f; - if (cap_.online_) { - percentage = cap_.percentage_; - } else if (ref_.status == Device::Referee::RUNNING) { - percentage = this->ref_.chassis_pwr_buff / 60.0f; + if (ref_.status == Device::Referee::RUNNING) { + if (ref_.chassis_pwr_buff > 30) { + percentage = 1.0f; + } else { + percentage = this->ref_.chassis_pwr_buff / 30.0f; + } } 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_; + // sum_6020_out_ = + // Calculate6020Power(out_.motor6020_out, motor_feedback_.pos_speed, 4); for (int i = 0; i < 4; i++) { - speed_motor_[i]->Control(speed_motor_out_[i] * percentage); - } - - clampf(&percentage, 0.5f, 1.0f); - - for (int i = 0; i < 4; i++) { - pos_motor_[i]->Control(pos_motor_out_[i] * percentage); + if (cap_.online_) { + LimitChassisOutPower(max_power_limit, out_.speed_motor_out, + speed_motor_feedback_, 4); + this->speed_motor_[i]->Control(out_.speed_motor_out[i]); + this->pos_motor_[i]->Control(out_.motor6020_out[i]); + } else { + LimitChassisOutPower(max_power_limit, out_.speed_motor_out, + speed_motor_feedback_, 4); + this->speed_motor_[i]->Control(out_.speed_motor_out[i]); + this->pos_motor_[i]->Control(out_.motor6020_out[i]); + } } } @@ -359,7 +435,8 @@ void HelmChassis::DrawUIStatic( /* 更新底盘模式选择框 */ switch (chassis->mode_) { - case FOLLOW_GIMBAL: + case CHASSIS_6020_FOLLOW_GIMBAL: + case CHASSIS_FOLLOW_GIMBAL: box_pos_left = REF_UI_MODE_OFFSET_2_LEFT; box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT; break; @@ -407,7 +484,8 @@ void HelmChassis::DrawUIDynamic( /* 更新底盘模式选择框 */ switch (chassis->mode_) { - case FOLLOW_GIMBAL: + case CHASSIS_6020_FOLLOW_GIMBAL: + case CHASSIS_FOLLOW_GIMBAL: box_pos_left = REF_UI_MODE_OFFSET_2_LEFT; box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT; break; diff --git a/src/module/helm_chassis/mod_helm_chassis.hpp b/src/module/helm_chassis/mod_helm_chassis.hpp index aa0212eb..b120d573 100644 --- a/src/module/helm_chassis/mod_helm_chassis.hpp +++ b/src/module/helm_chassis/mod_helm_chassis.hpp @@ -37,24 +37,31 @@ class HelmChassis { typedef enum { RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ - FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ + CHASSIS_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ + CHASSIS_6020_FOLLOW_GIMBAL, /*6020电机直接跟随车头方向*/ ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ } Mode; typedef enum { SET_MODE_RELAX, - SET_MODE_FOLLOW, + SET_MODE_CHASSIS_FOLLOW, + SET_MODE_6020_FOLLOW, SET_MODE_ROTOR, SET_MODE_INDENPENDENT, } ChassisEvent; /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 */ - typedef struct { + typedef struct Param { + float toque_coefficient_3508; + float speed_2_coefficient_3508; + float out_2_coefficient_3508; + float constant_3508; + Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */ const std::vector EVENT_MAP; - std::array pos_param; - std::array speed_param; + std::array pos_param{}; + std::array speed_param{}; std::array mech_zero; std::array pos_motor_param; @@ -76,6 +83,14 @@ class HelmChassis { void Control(); + bool LimitChassisOutPower(float power_limit, float *motor3508_out, + float *speed_3508, uint32_t len); + bool LimitChassisOutput(float power_limit, float *motor_out, float *speed, + uint32_t len); + // float Calculate6020Power(float *motor_out, float *speed, uint32_t len); + + uint16_t MAXSPEEDGET(float power_limit); + void SetMode(Mode mode); static void DrawUIStatic(HelmChassis *chassis); @@ -92,13 +107,18 @@ class HelmChassis { uint64_t last_wakeup_ = 0; uint64_t now_ = 0; - float yaw_; + float pos_motor_sum_out_; + + float max_motor_rotational_speed_; Device::Cap::Info cap_; Device::Referee::Data raw_ref_; + float speed_motor_feedback_[4]; + float pos_motor_feedback_[4]; + RefForChassis ref_; Mode mode_ = RELAX; @@ -122,10 +142,16 @@ class HelmChassis { float motor_rotational_speed[4]; /* 电机转速的动态数组，单位：RPM */ std::array wheel_pos; } setpoint_; - + struct { + float speed_motor_out[4]; + float motor6020_out[4]; + } out_; Component::Type::CycleValue main_direct_ = 0.0f; + float direct_offset_ = 0.0f; + Component::PID follow_pid_; + System::Thread thread_; System::Semaphore ctrl_lock_; diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index 838cb9ab..7df68f5d 100644 --- a/src/module/launcher/mod_launcher.cpp +++ b/src/module/launcher/mod_launcher.cpp @@ -114,6 +114,7 @@ void Launcher::UpdateFeedback() { 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++) { diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp index 05808e9a..b569c249 100644 --- a/src/module/launcher/mod_launcher.hpp +++ b/src/module/launcher/mod_launcher.hpp @@ -122,6 +122,7 @@ class Launcher { Launcher(Param ¶m, float control_freq); + std::array speed; void UpdateFeedback(); void Control(); diff --git a/src/module/omni_chassis/Kconfig b/src/module/omni_chassis/Kconfig new file mode 100644 index 00000000..e69de29b diff --git a/src/module/omni_chassis/info.cmake b/src/module/omni_chassis/info.cmake new file mode 100644 index 00000000..35463fe0 --- /dev/null +++ b/src/module/omni_chassis/info.cmake @@ -0,0 +1,6 @@ +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() \ No newline at end of file diff --git a/src/module/omni_chassis/mod_omni_chassis.cpp b/src/module/omni_chassis/mod_omni_chassis.cpp new file mode 100644 index 00000000..6f11c638 --- /dev/null +++ b/src/module/omni_chassis/mod_omni_chassis.cpp @@ -0,0 +1,522 @@ +/** + * @file chassis.c + * @author Qu Shen (503578404@qq.com) + * @brief 底盘模组 + * @version 1.0.0 + * @date 2021-04-15 + * + * @copyright Copyright (c) 2021 + * + */ + +#include "mod_omni_chassis.hpp" + +#include + +#include + +#include "bsp_time.h" + +#define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */ +#define ROTOR_WZ_MAX 1.0f /* 小陀螺旋转位移上界 */ + +#define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */ + +#define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */ + +#if POWER_LIMIT_WITH_CAP +/* 保证电容电量宏定义在正确范围内 */ +#if ((CAP_PERCENT_NO_LIM < 0) || (CAP_PERCENT_NO_LIM > 100) || \ + (CAP_PERCENT_WORK < 0) || (CAP_PERCENT_WORK > 100)) +#error "Cap percentage should be in the range from 0 to 100." +#endif + +/* 保证电容功率宏定义在正确范围内 */ +#if ((CAP_MAX_LOAD < 60) || (CAP_MAX_LOAD > 200)) +#error "The capacitor power should be in in the range from 60 to 200." +#endif + +static const float kCAP_PERCENTAGE_NO_LIM = (float)CAP_PERCENT_NO_LIM / 100.0f; +static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f; + +#endif + +using namespace Module; + +template +OmniChassis::OmniChassis(Param& param, float control_freq) + : param_(param), + mode_(OmniChassis::RELAX), + mixer_(param.type), + follow_pid_(param.follow_pid_param, control_freq), + ctrl_lock_(true) { + memset(&(this->cmd_), 0, sizeof(this->cmd_)); + + for (uint8_t i = 0; i < this->mixer_.len_; i++) { + this->actuator_.at(i) = + new Component::SpeedActuator(param.actuator_param.at(i), control_freq); + + this->motor_.at(i) = + new Motor(param.motor_param.at(i), + (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) { + chassis->ctrl_lock_.Wait(UINT32_MAX); + + switch (event) { + case SET_MODE_RELAX: + chassis->SetMode(RELAX); + break; + case SET_MODE_INTERSECT: + chassis->SetMode(FOLLOW_GIMBAL_INTERSECT); + chassis->param_.type = Component::Mixer::OMNIPLUS; + + break; + case SET_MODE_CROSS: + chassis->SetMode(FOLLOW_GIMBAL_CROSS); + break; + case SET_MODE_ROTOR: + chassis->SetMode(ROTOR); + break; + case SET_MODE_INDENPENDENT: + chassis->SetMode(INDENPENDENT); + break; + default: + break; + } + + chassis->ctrl_lock_.Post(); + }; + + Component::CMD::RegisterEvent( + event_callback, this, this->param_.EVENT_MAP); + + auto chassis_thread = [](OmniChassis* 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(); + + while (1) { + /* 读取控制指令、电容、裁判系统、电机反馈 */ + cmd_sub.DumpData(chassis->cmd_); + raw_ref_sub.DumpData(chassis->raw_ref_); + yaw_sub.DumpData(chassis->yaw_); + cap_sub.DumpData(chassis->cap_); + + /* 更新反馈值 */ + chassis->PraseRef(); + + chassis->ctrl_lock_.Wait(UINT32_MAX); + chassis->UpdateFeedback(); + chassis->Control(); + chassis->ctrl_lock_.Post(); + + /* 运行结束，等待下一次唤醒 */ + chassis->thread_.SleepUntil(2, last_online_time); + } + }; + + this->thread_.Create(chassis_thread, this, "chassis_thread", 512, + System::Thread::MEDIUM); + + System::Timer::Create(this->DrawUIStatic, this, 2100); + + System::Timer::Create(this->DrawUIDynamic, this, 200); +} + +template +void OmniChassis::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(); + } +} +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; + } else { + speed = 7500; + } + return speed; +} +template +bool OmniChassis::LimitChassisOutput(float power_limit, + float* motor_out, + float* speed_rpm, + uint32_t len) { + if (power_limit < 0.0f) { + return 0; + } + + float motor_3508_power[4]; + float sum_motor_out = 0.0f; + float power_scal = 0.0f; + for (size_t i = 0; i < len; i++) { + motor_3508_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] + + this->param_.constant_; + sum_motor_out += motor_3508_power[i]; + } + if (sum_motor_out >= power_limit) { + power_scal = power_limit / sum_motor_out; + for (size_t i = 0; i < len; i++) { + motor_3508_power[i] *= power_scal; + float b = this->param_.toque_coefficient_ * fabsf(speed_rpm[i]); + float c = + this->param_.speed_2_coefficient_ * speed_rpm[i] * speed_rpm[i] - + motor_3508_power[i] + this->param_.constant_; + if (motor_out[i] >= 0) { + float out = + (-b + sqrtf(b * b - 4 * this->param_.out_2_coefficient_ * c)) / 2 * + this->param_.out_2_coefficient_; + clampf(&out, 0.0f, 1.0f); + motor_out[i] = out; + } else { + float out = fabsf( + (-b - sqrtf(b * b - 4 * this->param_.out_2_coefficient_ * c)) / 2 * + this->param_.out_2_coefficient_); + clampf(&out, 0.0f, 1.0f); + motor_out[i] = -out; + } + } + } + return 0; +} +template +bool OmniChassis::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]; + for (size_t i = 0; i < len; i++) { + motor_3508_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 += this->param_.constant_; + power_ = sum_motor_power; + power_1_ = power_limit; + 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 OmniChassis::Control() { + this->now_ = bsp_time_get(); + + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); + + this->last_wakeup_ = this->now_; + + max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit); + /* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */ + /* 计算vx、vy */ + switch (this->mode_) { + case OmniChassis::BREAK: /* 刹车模式电机停止 */ + this->move_vec_.vx = 0.0f; + this->move_vec_.vy = 0.0f; + break; + + case OmniChassis::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: { + 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; + break; + } + default: + break; + } + + /* 计算wz */ + switch (this->mode_) { + case OmniChassis::RELAX: + case OmniChassis::BREAK: + case OmniChassis::INDENPENDENT: /* 独立模式wz为0 */ + this->move_vec_.wz = this->cmd_.z; + break; + + case OmniChassis::FOLLOW_GIMBAL_INTERSECT: + this->move_vec_.wz = + this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_); + break; + case OmniChassis::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; + } + default: + XB_ASSERT(false); + return; + } + + /* move_vec -> motor_rpm_set. 通过运动向量计算轮子转速目标值 */ + this->mixer_.Apply(this->move_vec_, this->setpoint_.motor_rotational_speed); + + /* 根据轮子转速目标值，利用PID计算电机输出值 */ + + /* 根据底盘模式计算输出值 */ + switch (this->mode_) { + case OmniChassis::BREAK: + case OmniChassis::FOLLOW_GIMBAL_INTERSECT: + case OmniChassis::FOLLOW_GIMBAL_CROSS: + case OmniChassis::ROTOR: + case OmniChassis::INDENPENDENT: /* 独立模式,受PID控制 */ { + float percentage = 0.0f; + if (ref_.status == Device::Referee::RUNNING) { + if (ref_.chassis_pwr_buff > 30) { + percentage = 1.0f; + } else { + percentage = this->ref_.chassis_pwr_buff / 30.0f; + } + } 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_, + this->motor_[i]->GetSpeed(), this->dt_); + } + 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_); + this->motor_[i]->Control(out_.motor3508_out[i]); + } else { + LimitChassisOutPower(ref_.chassis_power_limit, out_.motor3508_out, + motor_feedback_.motor_speed, this->mixer_.len_); + + this->motor_[i]->Control(out_.motor3508_out[i]); + } + } + break; + } + case OmniChassis::RELAX: /* 放松模式,不输出 */ + for (size_t i = 0; i < this->mixer_.len_; i++) { + this->motor_[i]->Relax(); + } + break; + default: + XB_ASSERT(false); + return; + } +} + +template +void OmniChassis::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 OmniChassis::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) { + if (mode == this->mode_) { + return; /* 模式未改变直接返回 */ + } + + if (mode == OmniChassis::ROTOR && this->mode_ != OmniChassis::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) { + this->param_.type = Component::Mixer::OMNICROSS; + } + if (mode == OmniChassis::FOLLOW_GIMBAL_INTERSECT && + this->mode_ != OmniChassis::FOLLOW_GIMBAL_INTERSECT) { + this->param_.type = Component::Mixer::OMNIPLUS; + } + /* 切换模式后重置PID和滤波器 */ + for (size_t i = 0; i < this->mixer_.len_; i++) { + this->actuator_[i]->Reset(); + } + this->mode_ = mode; +} + +template +void OmniChassis::DrawUIStatic( + OmniChassis* 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"); + Device::Referee::AddUI(chassis->string_); + + float box_pos_left = 0.0f, box_pos_right = 0.0f; + + /* 更新底盘模式选择框 */ + switch (chassis->mode_) { + case FOLLOW_GIMBAL_INTERSECT: + case FOLLOW_GIMBAL_CROSS: + 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_); + } +} + +template +void OmniChassis::DrawUIDynamic( + OmniChassis* chassis) { + float box_pos_left = 0.0f, box_pos_right = 0.0f; + + /* 更新底盘模式选择框 */ + switch (chassis->mode_) { + case FOLLOW_GIMBAL_INTERSECT: + case FOLLOW_GIMBAL_CROSS: + 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( + "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_); + } +} +template class Module::OmniChassis; diff --git a/src/module/omni_chassis/mod_omni_chassis.hpp b/src/module/omni_chassis/mod_omni_chassis.hpp new file mode 100644 index 00000000..38ded65e --- /dev/null +++ b/src/module/omni_chassis/mod_omni_chassis.hpp @@ -0,0 +1,156 @@ +/** + * @file chassis.h + * @author Qu Shen (503578404@qq.com) + * @brief 底盘模组 + * @version 1.0.0 + * @date 2021-04-15 + * + * @copyright Copyright (c) 2021 + * + */ + +#pragma once + +#include + +#include "comp_actuator.hpp" +#include "comp_cmd.hpp" +#include "comp_filter.hpp" +#include "comp_mixer.hpp" +#include "comp_pid.hpp" +#include "dev_cap.hpp" +#include "dev_motor.hpp" +#include "dev_referee.hpp" +#include "dev_rm_motor.hpp" +namespace Module { +template +class OmniChassis { + public: + /* 底盘运行模式 */ + typedef enum { + RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ + BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ + FOLLOW_GIMBAL_INTERSECT, /* 通过闭环控制使车头方向跟随云台 */ + FOLLOW_GIMBAL_CROSS, /* 通过闭环控制使车头方向跟随云台 */ + ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ + INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ + } Mode; + + typedef enum { + SET_MODE_RELAX, + SET_MODE_INTERSECT, + SET_MODE_CROSS, + SET_MODE_ROTOR, + SET_MODE_INDENPENDENT, + } ChassisEvent; + /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 + */ + typedef struct Param { + float toque_coefficient_; + float speed_2_coefficient_; + float out_2_coefficient_; + float constant_; + Component::Mixer::Mode type = + Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */ + + Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */ + + const std::vector EVENT_MAP; + + std::array actuator_param{}; + + std::array motor_param; + + } Param; + + typedef struct { + Device::Referee::Status status; + float chassis_power_limit; + float chassis_pwr_buff; + float chassis_watt; + } RefForChassis; + + OmniChassis(Param ¶m, float control_freq); + + void UpdateFeedback(); + + void Control(); + + void SetMode(Mode mode); + + bool LimitChassisOutPower(float power_limit, float *motor_out, float *speed, + uint32_t len); + bool LimitChassisOutput(float power_limit, float *motor_out, float *speed, + uint32_t len); + + void PraseRef(); + uint16_t MAXSPEEDGET(float power_limit); + static void DrawUIStatic(OmniChassis *chassis); + + static void DrawUIDynamic(OmniChassis *chassis); + + float CalcWz(const float LO, const float HI); + + private: + Param param_; + + float max_motor_rotational_speed_ = 0.0f; + + float power_1_; + float power_; + float dt_ = 0.0f; + + float chassis_current_; + + uint64_t last_wakeup_ = 0; + + uint64_t now_ = 0; + + RefForChassis ref_; + + Mode mode_ = RELAX; + + Device::Cap::Info cap_; + + std::array actuator_; + + std::array motor_; + + /* 底盘设计 */ + Component::Mixer mixer_; + + Component::Type::MoveVector move_vec_; /* 底盘实际的运动向量 */ + + float wz_dir_mult_; /* 小陀螺模式旋转方向乘数 */ + + /* PID计算的目标值 */ + struct { + float *motor_rotational_speed; /* 电机转速的动态数组，单位：RPM */ + } setpoint_; + struct { + float motor3508_out[4]; /*转矩电流范围-1--1*/ + } out_; + /* 反馈控制用的PID */ + + Component::PID follow_pid_; /* 跟随云台用的PID */ + + System::Thread thread_; + + System::Semaphore ctrl_lock_; + + float yaw_; + Device::Referee::Data raw_ref_; + + Device::BaseMotor::Feedback motor_feedback_; + + Component::CMD::ChassisCMD cmd_; + + Component::UI::String string_; + + Component::UI::Line line_; + + Component::UI::Rectangle rectange_; +}; + +typedef OmniChassis RMChassis; +} // namespace Module diff --git a/src/module/omni_gimbal/Kconfig b/src/module/omni_gimbal/Kconfig new file mode 100644 index 00000000..e69de29b diff --git a/src/module/omni_gimbal/info.cmake b/src/module/omni_gimbal/info.cmake new file mode 100644 index 00000000..35463fe0 --- /dev/null +++ b/src/module/omni_gimbal/info.cmake @@ -0,0 +1,6 @@ +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() \ No newline at end of file diff --git a/src/module/omni_gimbal/mod_omni_gimbal.cpp b/src/module/omni_gimbal/mod_omni_gimbal.cpp new file mode 100644 index 00000000..b9377c62 --- /dev/null +++ b/src/module/omni_gimbal/mod_omni_gimbal.cpp @@ -0,0 +1,328 @@ +#include "mod_omni_gimbal.hpp" + +#include +#include +#include + +#include "bsp_time.h" + +using namespace Module; + +#define GIMBAL_MAX_SPEED (M_2PI * 1.5f) + +template +DMGimbal::DMGimbal( + Param& param, float control_freq) + : param_(param), + st_(param.st), + yaw_actuator_(this->param_.yaw_actr, control_freq), + pit_actuator_(this->param_.pit_actr, control_freq), + yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"), + pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"), + ctrl_lock_(true) { + auto event_callback = [](GimbalEvent event, DMGimbal* gimbal) { + gimbal->ctrl_lock_.Wait(UINT32_MAX); + + switch (event) { + case SET_MODE_RELAX: + case SET_MODE_ABSOLUTE: + gimbal->SetMode(static_cast(event)); + break; + + case START_AUTO_AIM: + Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI); + break; + case STOP_AUTO_AIM: + Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC); + break; + } + gimbal->ctrl_lock_.Post(); + }; + + Component::CMD::RegisterEvent(event_callback, this, + this->param_.EVENT_MAP); + + auto gimbal_thread = [](DMGimbal* gimbal) { + auto eulr_sub = Message::Subscriber("imu_eulr"); + + auto gyro_sub = Message::Subscriber("imu_gyro"); + + auto cmd_sub = Message::Subscriber("cmd_gimbal"); + + uint32_t last_online_time = bsp_time_get_ms(); + + while (1) { + /* 读取控制指令、姿态、IMU、电机反馈 */ + eulr_sub.DumpData(gimbal->eulr_); /* imu */ + gyro_sub.DumpData(gimbal->gyro_); /* imu */ + cmd_sub.DumpData(gimbal->cmd_); /* cmd */ + + gimbal->ctrl_lock_.Wait(UINT32_MAX); + gimbal->Control(); + gimbal->UpdateFeedback(); + gimbal->ctrl_lock_.Post(); + + gimbal->yaw_tp_.Publish(gimbal->yaw_); + + /* 运行结束，等待下一次唤醒 */ + gimbal->thread_.SleepUntil(2, last_online_time); + } + }; + + this->thread_.Create(gimbal_thread, this, "gimbal_thread", 512, + System::Thread::MEDIUM); + + System::Timer::Create(this->DrawUIStatic, this, 2000); + + System::Timer::Create(this->DrawUIDynamic, this, 60); +} + +template +void DMGimbal::UpdateFeedback() { + this->pit_motor_.Update(); + this->yaw_motor_.Update(); + switch (this->mode_) { + case RELAX: + case ABSOLUTE: + this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + this->raw_angles_ = this->pit_motor_.GetAngle(); + break; + } +} + +template +void DMGimbal::Control() { + this->now_ = bsp_time_get(); + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); + + this->last_wakeup_ = this->now_; + + float gimbal_pit_cmd = 0.0f; + float gimbal_yaw_cmd = 0.0f; + + /* yaw坐标正方向与遥控器操作逻辑相反 */ + 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; + + } else { + gimbal_yaw_cmd = Component::Type::CycleValue(this->cmd_.eulr.yaw) - + this->setpoint_.eulr_.yaw; + gimbal_pit_cmd = Component::Type::CycleValue(this->cmd_.eulr.pit) - + this->setpoint_.eulr_.pit; + } + + /* 处理yaw控制命令，软件限位 */ + /* 某个轴max=min时不进行限位,配置文件默认不写 */ + if (param_.limit.yaw_max != param_.limit.yaw_min) { + const float ENCODER_DELTA_MAX_YAW = + this->param_.limit.yaw_max - this->yaw_motor_.GetAngle(); + const float ENCODER_DELTA_MIN_YAW = + this->param_.limit.yaw_min - this->yaw_motor_.GetAngle(); + const float YAW_ERR = this->setpoint_.eulr_.yaw - eulr_.yaw; + const float DELTA_MAX_YAW = ENCODER_DELTA_MAX_YAW - YAW_ERR; + const float DELTA_MIN_YAW = ENCODER_DELTA_MIN_YAW - YAW_ERR; + clampf(&(gimbal_yaw_cmd), DELTA_MIN_YAW, DELTA_MAX_YAW); + } + this->setpoint_.eulr_.yaw += gimbal_yaw_cmd; + + /* 处理pitch控制命令，软件限位 */ + if (param_.limit.pitch_max != param_.limit.pitch_min) { + const float ENCODER_DELTA_MAX_PIT = + this->param_.limit.pitch_max - this->pit_motor_.GetAngle(); + const float ENCODER_DELTA_MIN_PIT = + this->param_.limit.pitch_min - this->pit_motor_.GetAngle(); + const float PIT_ERR = this->setpoint_.eulr_.pit - eulr_.pit; + const float DELTA_MAX_PIT = ENCODER_DELTA_MAX_PIT - PIT_ERR; + const float DELTA_MIN_PIT = ENCODER_DELTA_MIN_PIT - PIT_ERR; + clampf(&(gimbal_pit_cmd), DELTA_MIN_PIT, DELTA_MAX_PIT); + } + this->setpoint_.eulr_.pit += gimbal_pit_cmd; + + /* 控制相关逻辑 */ + float yaw_out = 0; + float pit_out = 0; + switch (this->mode_) { + case RELAX: + this->yaw_motor_.Relax(); + this->pit_motor_.Relax(); + break; + case ABSOLUTE: + + /* Yaw轴角速度环参数计算 */ + yaw_out = this->yaw_actuator_.Calculate( + this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); + + pit_out = this->pit_actuator_.Calculate( + this->setpoint_.eulr_.pit, this->gyro_.x, this->eulr_.pit, this->dt_); + + this->yaw_motor_.Control(yaw_out); + this->pit_motor_.SetMit(pit_out); + + break; + } +} + +template +void DMGimbal::SetMode( + Mode mode) { + if (mode == this->mode_) { + return; + } + + /* 切换模式后重置PID和滤波器 */ + // this->pit_actuator_.Reset(); + this->yaw_actuator_.Reset(); + this->pit_motor_.Enable(); + memcpy(&(this->setpoint_.eulr_), &(this->eulr_), + sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ + if (this->mode_ == RELAX) { + if (mode == ABSOLUTE) { + this->setpoint_.eulr_.yaw = this->eulr_.yaw; + } + } + this->mode_ = mode; +} + +template +void DMGimbal::DrawUIStatic(DMGimbal* gimbal) { + gimbal->string_.Draw("GM", 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_LINE2_H), + "GMBL RELX ABSL RLTV"); + Device::Referee::AddUI(gimbal->string_); + + float box_pos_left = 0.0f, box_pos_right = 0.0f; + + /* 更新云台模式选择框 */ + switch (gimbal->mode_) { + case RELAX: + box_pos_left = REF_UI_MODE_OFFSET_2_LEFT; + box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT; + break; + case ABSOLUTE: + if (gimbal->cmd_.mode == Component::CMD::GIMBAL_ABSOLUTE_CTRL) { + box_pos_left = REF_UI_MODE_OFFSET_3_LEFT; + box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT; + } else { + box_pos_left = REF_UI_MODE_OFFSET_4_LEFT; + box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT; + } + break; + default: + box_pos_left = 0.0f; + box_pos_right = 0.0f; + break; + } + if (box_pos_left != 0.0f && box_pos_right != 0.0f) { + gimbal->rectangle_.Draw( + "GS", Component::UI::UI_GRAPHIC_OP_ADD, + Component::UI::UI_GRAPHIC_LAYER_GIMBAL, 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_LINE2_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_LINE2_H + + REF_UI_BOX_BOT_OFFSET)); + Device::Referee::AddUI(gimbal->rectangle_); + } + gimbal->line_.Draw( + "g", Component::UI::UI_GRAPHIC_OP_ADD, + Component::UI::UI_GRAPHIC_LAYER_CONST, Component::UI::UI_GREEN, + UI_DEFAULT_WIDTH * 3, + static_cast(Device::Referee::UIGetWidth() * 0.4f), + static_cast(Device::Referee::UIGetHeight() * 0.2f), + static_cast(Device::Referee::UIGetWidth() * 0.4f), + static_cast(Device::Referee::UIGetHeight() * 0.2f + 50.f)); + Device::Referee::AddUI(gimbal->line_); + gimbal->line_.Draw( + "GA", Component::UI::UI_GRAPHIC_OP_ADD, + Component::UI::UI_GRAPHIC_LAYER_GIMBAL, Component::UI::UI_GREEN, + UI_DEFAULT_WIDTH * 12, + static_cast(Device::Referee::UIGetWidth() * 0.4f), + static_cast(Device::Referee::UIGetHeight() * 0.2f), + static_cast(Device::Referee::UIGetWidth() * 0.4f + + -sinf(gimbal->yaw_) * 44), + static_cast(Device::Referee::UIGetHeight() * 0.2f + + cosf(gimbal->yaw_) * 44)); + Device::Referee::AddUI(gimbal->line_); +} + +template +void DMGimbal::DrawUIDynamic(DMGimbal* gimbal) { + float box_pos_left = 0.0f, box_pos_right = 0.0f; + + /* 更新云台模式选择框 */ + switch (gimbal->mode_) { + case RELAX: + box_pos_left = REF_UI_MODE_OFFSET_2_LEFT; + box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT; + break; + case ABSOLUTE: + if (gimbal->cmd_.mode == Component::CMD::GIMBAL_ABSOLUTE_CTRL) { + box_pos_left = REF_UI_MODE_OFFSET_3_LEFT; + box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT; + } else { + box_pos_left = REF_UI_MODE_OFFSET_4_LEFT; + box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT; + } + break; + default: + box_pos_left = 0.0f; + box_pos_right = 0.0f; + break; + } + if (box_pos_left != 0.0f && box_pos_right != 0.0f) { + gimbal->rectangle_.Draw( + "GS", Component::UI::UI_GRAPHIC_OP_REWRITE, + Component::UI::UI_GRAPHIC_LAYER_GIMBAL, 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_LINE2_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_LINE2_H + + REF_UI_BOX_BOT_OFFSET)); + Device::Referee::AddUI(gimbal->rectangle_); + } + gimbal->line_.Draw( + "GA", Component::UI::UI_GRAPHIC_OP_REWRITE, + Component::UI::UI_GRAPHIC_LAYER_GIMBAL, Component::UI::UI_GREEN, + UI_DEFAULT_WIDTH * 12, + static_cast(Device::Referee::UIGetWidth() * 0.4f), + static_cast(Device::Referee::UIGetHeight() * 0.2f), + static_cast(Device::Referee::UIGetWidth() * 0.4f + + -sinf(gimbal->yaw_) * 44), + static_cast(Device::Referee::UIGetHeight() * 0.2f + + cosf(gimbal->yaw_) * 44)); + Device::Referee::AddUI(gimbal->line_); +} +template class Module::DMGimbal; diff --git a/src/module/omni_gimbal/mod_omni_gimbal.hpp b/src/module/omni_gimbal/mod_omni_gimbal.hpp new file mode 100644 index 00000000..5564355f --- /dev/null +++ b/src/module/omni_gimbal/mod_omni_gimbal.hpp @@ -0,0 +1,131 @@ +#pragma once + +#include +#include +#include + +#include "comp_actuator.hpp" +#include "comp_cf.hpp" +#include "comp_cmd.hpp" +#include "comp_filter.hpp" +#include "comp_pid.hpp" +#include "dev_ahrs.hpp" +#include "dev_bmi088.hpp" +#include "dev_mit_motor.hpp" +#include "dev_referee.hpp" +#include "dev_rm_motor.hpp" + +namespace Module { +template +class DMGimbal { + public: + /* 云台运行模式 */ + typedef enum { + RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ + ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + } Mode; + + enum { + GIMBAL_CTRL_YAW_OMEGA_IDX = 0, /* Yaw轴控制的角速度环控制器的索引值 */ + GIMBAL_CTRL_YAW_ANGLE_IDX, /* Yaw轴控制的角度环控制器的索引值 */ + GIMBAL_CTRL_PIT_OMEGA_IDX, /* Pitch轴控制的角速度环控制器的索引值 */ + GIMBAL_CTRL_PIT_ANGLE_IDX, /* Pitch轴控制的角度环控制器的索引值 */ + GIMBAL_CTRL_NUM, /* 总共的控制器数量 */ + }; + + typedef enum { + SET_MODE_RELAX, + SET_MODE_ABSOLUTE, + START_AUTO_AIM, + STOP_AUTO_AIM, + } GimbalEvent; + + typedef struct { + Component::SecOrderFunction::Param ff; /* PITCH前馈 */ + Component::SecOrderFunction::Param st; /* YAW自整定参数 */ + + Component::PosActuator::Param yaw_actr; + Component::PosActuator::Param pit_actr; + + Device::RMMotor::Param yaw_motor; + Device::MitMotor::Param pit_motor; + + Component::Type::Eulr mech_zero; + + float patrol_range; + float patrol_omega; + float patrol_hight; + + struct { + Component::Type::CycleValue pitch_max; + Component::Type::CycleValue pitch_min; + Component::Type::CycleValue yaw_max; + Component::Type::CycleValue yaw_min; + } limit; + + const std::vector EVENT_MAP; + + } Param; + + DMGimbal(Param ¶m, float control_freq); + + void UpdateFeedback(); + + void Control(); + + void SetMode(Mode mode); + + static void DrawUIStatic(DMGimbal *gimbal); + + static void DrawUIDynamic(DMGimbal *gimbal); + + private: + uint64_t last_wakeup_ = 0; + + uint64_t now_ = 0; + + float raw_angles_; + float angles_; + float dt_ = 0.0f; + + uint32_t autopatrol_start_time_ = 0; + + Param param_; + + DMGimbal::Mode mode_ = RELAX; /* 云台模式 */ + + struct { + Component::Type::Eulr eulr_; /* 表示云台姿态的欧拉角 */ + } setpoint_; + + Component::SecOrderFunction st_; /* YAW自整定参数 */ + + Component::PosActuator yaw_actuator_; + Component::PosActuator pit_actuator_; + + Device::RMMotor yaw_motor_; + Device::MitMotor pit_motor_; + + System::Thread thread_; + + System::Semaphore ctrl_lock_; + + Message::Topic yaw_tp_ = Message::Topic("chassis_yaw"); + + float yaw_; + + Component::UI::String string_; + + Component::UI::Rectangle rectangle_; + + Component::UI::Line line_; + + Component::Type::Eulr eulr_; + Component::Type::Vector3 gyro_; + Component::CMD::GimbalCMD cmd_; +}; +typedef DMGimbal + Gimbal; +} // namespace Module diff --git a/src/robot/balance_infantry/robot.cpp b/src/robot/balance_infantry/robot.cpp index 89f634bd..83378aaf 100644 --- a/src/robot/balance_infantry/robot.cpp +++ b/src/robot/balance_infantry/robot.cpp @@ -285,34 +285,26 @@ Robot::Infantry::Param param = { Device::MitMotor::Param{ .kp = 50.0f, .kd = 0.05f, - .def_speed = 0.0f, .id = 1, .can = BSP_CAN_2, - .max_error = 0.1f, }, Device::MitMotor::Param{ .kp = 50.0f, .kd = 0.05f, - .def_speed = 0.0f, .id = 2, .can = BSP_CAN_2, - .max_error = 0.1f, }, Device::MitMotor::Param{ .kp = 50.0f, .kd = 0.05f, - .def_speed = 0.0f, .id = 1, .can = BSP_CAN_1, - .max_error = 0.1f, }, Device::MitMotor::Param{ .kp = 50.0f, .kd = 0.05f, - .def_speed = 0.0f, .id = 4, .can = BSP_CAN_1, - .max_error = 0.1f, }, }, }, @@ -600,8 +592,6 @@ Robot::Infantry::Param param = { .cap = { .can = BSP_CAN_2, - .index = DEV_CAP_FB_ID_BASE, - .cutoff_volt = 13.0f, }, .can_imu = { diff --git a/src/robot/helm_infantry/robot.cpp b/src/robot/helm_infantry/robot.cpp index bf3b5bcb..5625dc87 100644 --- a/src/robot/helm_infantry/robot.cpp +++ b/src/robot/helm_infantry/robot.cpp @@ -11,6 +11,23 @@ Robot::HelmInfantry::Param param = { .chassis = { + .toque_coefficient_3508 = 0.044876589f, + .speed_2_coefficient_3508 = 1.563578147703433e-19f, + .out_2_coefficient_3508 = 54.67448580571495f, + .constant_3508 = 6.564590719535691f, + + .follow_pid_param = + { + .k = 5.0f, + .p = 0.5f, + .i = 0.5f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 5.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + .EVENT_MAP = {Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, @@ -20,7 +37,7 @@ Robot::HelmInfantry::Param param = Module::RMHelmChassis::SET_MODE_RELAX}, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_MID, - Module::RMHelmChassis::SET_MODE_FOLLOW}, + Module::RMHelmChassis::SET_MODE_CHASSIS_FOLLOW}, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_BOT, Module::RMHelmChassis::SET_MODE_ROTOR}, @@ -29,7 +46,11 @@ Robot::HelmInfantry::Param param = Module::RMHelmChassis::SET_MODE_ROTOR}, Component::CMD::EventMapItem{ Device::DR16::KEY_B, - Module::RMHelmChassis::SET_MODE_FOLLOW}}, + Module::RMHelmChassis::SET_MODE_CHASSIS_FOLLOW}, + Component::CMD::EventMapItem{ + Device::DR16::KEY_F, + Module::RMHelmChassis::SET_MODE_6020_FOLLOW} + }, .pos_param = { @@ -152,11 +173,11 @@ Robot::HelmInfantry::Param param = .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 2.0f, .i = 0.0f, .d = 0.0f, - .i_limit = 0.3f, - .out_limit = 0.7f, + .i_limit = 1.0f, + .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -170,11 +191,11 @@ Robot::HelmInfantry::Param param = .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 2.0f, .i = 0.0f, .d = 0.0f, - .i_limit = 0.3f, - .out_limit = 0.7f, + .i_limit = 1.0f, + .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -187,11 +208,11 @@ Robot::HelmInfantry::Param param = .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 2.0f, .i = 0.0f, .d = 0.0f, - .i_limit = 0.3f, - .out_limit = 0.7f, + .i_limit = 1.0f, + .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -204,11 +225,11 @@ Robot::HelmInfantry::Param param = .speed = { .k = 0.0001f, - .p = 1.0f, + .p = 2.0f, .i = 0.0f, .d = 0.0f, - .i_limit = 0.3f, - .out_limit = 0.7f, + .i_limit = 1.0f, + .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -219,7 +240,7 @@ Robot::HelmInfantry::Param param = }, }, - .mech_zero = {2.65762205, 2.33241792, 1.836174984, 2.5862916}, + .mech_zero = { 1.15201962, 3.88327241, 3.41387439, 0.99171859}, .pos_motor_param = { @@ -255,13 +276,14 @@ Robot::HelmInfantry::Param param = .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 = true, }, Device::RMMotor::Param{ .id_feedback = 0x203, @@ -269,7 +291,6 @@ Robot::HelmInfantry::Param param = .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, .reverse = true, - }, Device::RMMotor::Param{ .id_feedback = 0x205, @@ -277,7 +298,6 @@ Robot::HelmInfantry::Param param = .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, .reverse = true, - }, }, }, @@ -387,15 +407,15 @@ Robot::HelmInfantry::Param param = .mech_zero = { - .yaw = 4.16475773f, - .pit = 4.8f, + .yaw = 6.26477766f, + .pit = M_2PI - 1.0f, .rol = 0.0f, }, .limit = { - .pitch_max = M_2PI - 4.03667068f, - .pitch_min = M_2PI - 5.27919483f, + .pitch_max = M_2PI - 0.60f, + .pitch_min = M_2PI - 1.4f }, .EVENT_MAP = {Component::CMD::EventMapItem{ @@ -406,10 +426,10 @@ Robot::HelmInfantry::Param param = Module::Gimbal::SET_MODE_ABSOLUTE}, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Gimbal::SET_MODE_ABSOLUTE}, + Module::Gimbal::START_AUTO_AIM}, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Gimbal::SET_MODE_ABSOLUTE}, + Module::Gimbal::START_AUTO_AIM}, Component::CMD::EventMapItem{ Device::DR16::KEY_R_PRESS, Module::Gimbal::START_AUTO_AIM}, @@ -425,7 +445,7 @@ Robot::HelmInfantry::Param param = .cover_open_duty = 0.125f, .cover_close_duty = 0.075f, .model = Module::Launcher::LAUNCHER_MODEL_17MM, - .default_bullet_speed = 15.f, + .default_bullet_speed = 30.f, .min_launch_delay = static_cast(1000.0f / 20.0f), .trig_actr = { @@ -461,9 +481,9 @@ Robot::HelmInfantry::Param param = .fric_actr = { Component::SpeedActuator::Param{ .speed = { - .k = 0.0003f, + .k = 0.00025f, .p = 1.0f, - .i = 0.8f, + .i = 0.0f, .d = 0.0f, .i_limit = 0.2f, .out_limit = 1.0f, @@ -477,9 +497,9 @@ Robot::HelmInfantry::Param param = }, Component::SpeedActuator::Param{ .speed = { - .k = 0.0003f, + .k = 0.00025f, .p = 1.0f, - .i = 0.8f, + .i = 0.0f, .d = 0.0f, .i_limit = 0.2f, .out_limit = 1.0f, @@ -499,18 +519,19 @@ Robot::HelmInfantry::Param param = .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M2006, .can = BSP_CAN_1, + .reverse = true, } }, .fric_motor = { Device::RMMotor::Param{ - .id_feedback = 0x207, + .id_feedback = 0x206, .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, }, Device::RMMotor::Param{ - .id_feedback = 0x206, + .id_feedback = 0x207, .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, @@ -567,8 +588,6 @@ Robot::HelmInfantry::Param param = }, .cap = { .can = BSP_CAN_1, - .index = DEV_CAP_FB_ID_BASE, - .cutoff_volt = 10.0f, }, }; /* clang-format on */ diff --git a/src/robot/helm_infantry/robot.hpp b/src/robot/helm_infantry/robot.hpp index 0399eff5..201b620c 100644 --- a/src/robot/helm_infantry/robot.hpp +++ b/src/robot/helm_infantry/robot.hpp @@ -27,14 +27,14 @@ class HelmInfantry { Component::CMD cmd_; + Device::Referee referee_; + Device::Can can_; Device::AI ai_; Device::AHRS ahrs_; Device::BMI088 bmi088_; - Device::Can can_; Device::Cap cap_; Device::DR16 dr16_; Device::RGB led_; - Device::Referee referee_; Module::RMHelmChassis chassis_; Module::Gimbal gimbal_; diff --git a/src/robot/hero/robot.cpp b/src/robot/hero/robot.cpp index e9625ae1..346614da 100644 --- a/src/robot/hero/robot.cpp +++ b/src/robot/hero/robot.cpp @@ -10,8 +10,11 @@ /* clang-format off */ Robot::Hero::Param param = { .chassis={ + .toque_coefficient_ = 0.032717046f, + .speed_2_coefficient_ = 2.1878478350476053e-07f, + .out_2_coefficient_ = 74.32893136030049f, + .constant_ = 1.9021463852937033f, .type = Component::Mixer::MECANUM, - .follow_pid_param = { .k = 0.5f, .p = 1.0f, @@ -144,6 +147,24 @@ Robot::Hero::Param param = { .can = BSP_CAN_1, }, }, + + .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; + }, }, @@ -248,8 +269,8 @@ Robot::Hero::Param param = { }, .limit = { - .pitch_max = 2.7f, - .pitch_min = 2.0f, + .pitch_max = M_2PI - 3.51895213f, + .pitch_min = M_2PI - 4.46541834f, }, .EVENT_MAP = { @@ -259,15 +280,15 @@ Robot::Hero::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::ABSOLUTE }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::START_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::START_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::KEY_R_PRESS, @@ -325,8 +346,8 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.001f, - .p = 1.0f, - .i = 0.2, + .p = 1.2f, + .i = 0.0f, .d = 0.0f, .i_limit = 0.3f, .out_limit = 1.0f, @@ -341,8 +362,8 @@ Robot::Hero::Param param = { Component::SpeedActuator::Param{ .speed = { .k = 0.001f, - .p = 1.0f, - .i = 0.2f, + .p = 1.2f, + .i = 0.0f, .d = 0.0f, .i_limit = 0.3f, .out_limit = 1.0f, @@ -430,8 +451,6 @@ Robot::Hero::Param param = { .cap = { .can = BSP_CAN_1, - .index = DEV_CAP_FB_ID_BASE, - .cutoff_volt = 10.0f, }, }; /* clang-format on */ diff --git a/src/robot/infantry/robot.cpp b/src/robot/infantry/robot.cpp index f531df7d..c94d77e5 100644 --- a/src/robot/infantry/robot.cpp +++ b/src/robot/infantry/robot.cpp @@ -9,6 +9,10 @@ /* clang-format off */ Robot::Infantry::Param param = { .chassis={ + .toque_coefficient_ = 0.0327120418848f, + .speed_2_coefficient_ = 0.0f, + .out_2_coefficient_ = 0.0f, + .constant_ = 0.0f, .type = Component::Mixer::MECANUM, .follow_pid_param = { @@ -33,7 +37,7 @@ Robot::Infantry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_MID, - Module::RMChassis::SET_MODE_FOLLOW + Module::RMChassis::SET_MODE_INDENPENDENT }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_BOT, @@ -143,6 +147,23 @@ Robot::Infantry::Param param = { .can = BSP_CAN_1, }, }, + .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; + }, }, .gimbal = { @@ -231,21 +252,22 @@ Robot::Infantry::Param param = { }, .pit_motor = { - .id_feedback = 0x20A, - .id_control = GM6020_CTRL_ID_EXTAND, + .id_feedback = 0x206, + .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_2, + .reverse = true , }, .mech_zero = { .yaw = 1.3f, - .pit = 4.0f, + .pit =M_2PI - 5.37046671f, .rol = 0.0f, }, .limit = { - .pitch_max = 3.8f, - .pitch_min = 3.0f, + .pitch_max =M_2PI - 5.1f, + .pitch_min =M_2PI - 5.8f, }, .EVENT_MAP = { @@ -354,8 +376,8 @@ Robot::Infantry::Param param = { .trig_motor = { Device::RMMotor::Param{ - .id_feedback = 0x207, - .id_control = M3508_M2006_CTRL_ID_EXTAND, + .id_feedback = 0x201, + .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M2006, .can = BSP_CAN_2, } @@ -363,14 +385,14 @@ Robot::Infantry::Param param = { .fric_motor = { 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, }, Device::RMMotor::Param{ - .id_feedback = 0x206, - .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, }, @@ -426,8 +448,6 @@ Robot::Infantry::Param param = { .cap = { .can = BSP_CAN_1, - .index = DEV_CAP_FB_ID_BASE, - .cutoff_volt = 13.0f, }, }; /* clang-format on */ diff --git a/src/robot/infantry/robot.hpp b/src/robot/infantry/robot.hpp index 6c8ba729..b04806a6 100644 --- a/src/robot/infantry/robot.hpp +++ b/src/robot/infantry/robot.hpp @@ -29,10 +29,10 @@ class Infantry { Device::AHRS ahrs_; Device::BMI088 bmi088_; Device::Can can_; + Device::Referee referee_; Device::Cap cap_; Device::DR16 dr16_; Device::RGB led_; - Device::Referee referee_; Module::RMChassis chassis_; Module::Gimbal gimbal_; diff --git a/src/robot/omni_infantry/Kconfig b/src/robot/omni_infantry/Kconfig new file mode 100644 index 00000000..e69de29b diff --git a/src/robot/omni_infantry/robot.cpp b/src/robot/omni_infantry/robot.cpp new file mode 100644 index 00000000..976cd2a9 --- /dev/null +++ b/src/robot/omni_infantry/robot.cpp @@ -0,0 +1,441 @@ +#include "robot.hpp" + +#include + +#include "dev_rm_motor.hpp" +#include "mod_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, + + .follow_pid_param = { + .k = 0.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, + }, + + .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_INTERSECT + }, + 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_INTERSECT + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_E, + Module::RMChassis::SET_MODE_CROSS + } + }, + + .actuator_param = { + Component::SpeedActuator::Param{ + .speed = { + .k = 0.00015f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .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.00018f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .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.00015f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .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.00015f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + }, + + .motor_param = { + Device::RMMotor::Param{ + .id_feedback = 0x203, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_1, + }, + Device::RMMotor::Param{ + .id_feedback = 0x204, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_1, + }, + Device::RMMotor::Param{ + .id_feedback = 0x201, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_1, + }, + Device::RMMotor::Param{ + .id_feedback = 0x202, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_1, + }, + }, + }, + + .gimbal= { + .ff = { + /* GIMBAL_CTRL_PIT_FEEDFORWARD */ + .a = 0.0439f, + .b = -0.0896f, + .c = 0.077f, + .max = 0.1f, + .min = -0.2f, + }, /* ff */ + + .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.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 = 10.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.0f, + .p = 0.0f, + .i = 0.f, + .d = 0.f, + .i_limit = 0.f, + .out_limit = 0.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 30.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 = 0x205, + .id_control = GM6020_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_GM6020, + .can = BSP_CAN_1, + }, + .pit_motor ={ + .kp = 10.0f, + .kd = 1.0f, + .feedback_id = 0, + .id = 1, + .can = BSP_CAN_2, + .reverse = true, + }, + .mech_zero = { + .yaw = 1.3f + M_PI / 2.0f, + .pit = 0.0f, + .rol = 0.0f, + }, + + .limit = { + .pitch_max = M_2PI - 0.38f, + .pitch_min = M_2PI - 2.27f, + }, + + .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::SET_MODE_ABSOLUTE + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_MID, + Module::Gimbal::SET_MODE_ABSOLUTE + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_BOT, + Module::Gimbal::SET_MODE_ABSOLUTE + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_R_PRESS, + Module::Gimbal::START_AUTO_AIM + }, + Component::CMD::EventMapItem{ + Device::DR16::KEY_R_RELEASE, + Module::Gimbal::STOP_AUTO_AIM + } + }, + + }, + + .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 = 15.f, + .min_launch_delay = static_cast(1000.0f / 20.0f), + + .trig_actr = { + Component::PosActuator::Param{ + .speed = { + .k = 5.0f, + .p = 1.0f, + .i = 0.15f, + .d = 0.0f, + .i_limit = 0.1f, + .out_limit = 1.5f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + .k = 5.0f, + .p = 1.0f, + .i = 0.0f, + .d = 0.010f, + .i_limit = 0.0f, + .out_limit = 0.70f, + .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.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, + }, + 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 = { + Device::RMMotor::Param{ + .id_feedback = 0x208, + .id_control = M3508_M2006_CTRL_ID_EXTAND, + .model = Device::RMMotor::MOTOR_M2006, + .can = BSP_CAN_2, + } + }, + + .fric_motor = { + Device::RMMotor::Param{ + .id_feedback = 0x206, + .id_control = M3508_M2006_CTRL_ID_EXTAND, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_2, + }, + Device::RMMotor::Param{ + .id_feedback = 0x205, + .id_control = M3508_M2006_CTRL_ID_EXTAND, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_2, + }, + }, + + .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::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 */ + + .bmi088_rot = { + .rot_mat = { + { +1, +0, +0}, + { +0, +1, +0}, + { +0, +0, +1}, + }, + }, + + .cap = { + .can = BSP_CAN_1, + }, +}; +/* clang-format on */ + +void robot_init() { + System::Start(param, 500.0f); +} diff --git a/src/robot/omni_infantry/robot.hpp b/src/robot/omni_infantry/robot.hpp new file mode 100644 index 00000000..97f0f8df --- /dev/null +++ b/src/robot/omni_infantry/robot.hpp @@ -0,0 +1,48 @@ +#include "comp_cmd.hpp" +#include "dev_ahrs.hpp" +#include "dev_ai.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_launcher.hpp" +#include "mod_omni_chassis.hpp" +#include "mod_omni_gimbal.hpp" + +void robot_init(); +namespace Robot { +class OmniInfantry { + public: + typedef struct Param { + Module::RMChassis::Param chassis; + Module::Gimbal::Param gimbal; + Module::Launcher::Param launcher; + Device::BMI088::Rotation bmi088_rot{}; + Device::Cap::Param cap{}; + } Param; + + Component::CMD cmd_; + + Device::AI ai_; + Device::AHRS ahrs_; + Device::BMI088 bmi088_; + Device::Can can_; + Device::Referee referee_; + Device::Cap cap_; + Device::DR16 dr16_; + Device::RGB led_; + + Module::RMChassis chassis_; + Module::Gimbal gimbal_; + Module::Launcher launcher_; + + OmniInfantry(Param& param, float control_freq) + : bmi088_(param.bmi088_rot), + cap_(param.cap), + chassis_(param.chassis, control_freq), + gimbal_(param.gimbal, control_freq), + launcher_(param.launcher, control_freq) {} +}; +} // namespace Robot diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index ac36e47a..2da4db9f 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -9,6 +9,10 @@ /* clang-format off */ Robot::Sentry::Param param = { .chassis={ + .toque_coefficient_ = 0.0327120418848f, + .speed_2_coefficient_ = 2.300974248103511e-07f, + .out_2_coefficient_ = 2.1455244766462685e-29f, + .constant_ = 0.23958431845825284f, .type = Component::Mixer::OMNICROSS, .follow_pid_param = { @@ -139,6 +143,24 @@ Robot::Sentry::Param param = { .can = BSP_CAN_1, }, }, + + .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; + }, }, .gimbal = { @@ -540,8 +562,6 @@ Robot::Sentry::Param param = { .cap = { .can = BSP_CAN_1, - .index = DEV_CAP_FB_ID_BASE, - .cutoff_volt = 15.0f, }, }; /* clang-format on */