diff --git a/2.Firmware/Core-STM32F4-fw/.idea/REF-STM32F4.iml b/2.Firmware/Core-STM32F4-fw/.idea/REF-STM32F4.iml index f08604b..6d70257 100644 --- a/2.Firmware/Core-STM32F4-fw/.idea/REF-STM32F4.iml +++ b/2.Firmware/Core-STM32F4-fw/.idea/REF-STM32F4.iml @@ -1,2 +1,2 @@ - + \ No newline at end of file diff --git a/2.Firmware/Core-STM32F4-fw/.idea/modules.xml b/2.Firmware/Core-STM32F4-fw/.idea/modules.xml index 4f6e096..b319820 100644 --- a/2.Firmware/Core-STM32F4-fw/.idea/modules.xml +++ b/2.Firmware/Core-STM32F4-fw/.idea/modules.xml @@ -1,8 +1,8 @@ - - - - - - - + + + + + + + \ No newline at end of file diff --git a/2.Firmware/Core-STM32F4-fw/Bsp/communication/communication.cpp b/2.Firmware/Core-STM32F4-fw/Bsp/communication/communication.cpp index d033599..f1ebc67 100644 --- a/2.Firmware/Core-STM32F4-fw/Bsp/communication/communication.cpp +++ b/2.Firmware/Core-STM32F4-fw/Bsp/communication/communication.cpp @@ -1,95 +1,89 @@ - -/* Includes ------------------------------------------------------------------*/ - -#include "communication.hpp" -#include "common_inc.h" - -/* Private defines -----------------------------------------------------------*/ -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -/* Private constant data -----------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ -volatile bool endpointListValid = false; -/* Private function prototypes -----------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ -// @brief Sends a line on the specified output. - -osThreadId_t commTaskHandle; -const osThreadAttr_t commTask_attributes = { - .name = "commTask", - .stack_size = 10000 * 4, - .priority = (osPriority_t) osPriorityNormal, -}; - -void InitCommunication(void) -{ - printf("\r\nHello, REF v%.1f Started!\r\n", CONFIG_FW_VERSION); - - // Start command handling thread - commTaskHandle = osThreadNew(CommunicationTask, NULL, &commTask_attributes); - - while (!endpointListValid) - osDelay(1); -} - -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -osThreadId_t usbIrqTaskHandle; - -void UsbDeferredInterruptTask(void* ctx) -{ - (void) ctx; // unused parameter - - for (;;) - { - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreAcquire(sem_usb_irq, osWaitForever); - if (semaphore_status == osOK) - { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } - } -} - -// Thread to handle deffered processing of USB interrupt, and -// read commands out of the UART DMA circular buffer -void CommunicationTask(void* ctx) -{ - (void) ctx; // unused parameter - - CommitProtocol(); - - // Allow main init to continue - endpointListValid = true; - - StartUartServer(); - StartUsbServer(); - StartCanServer(CAN1); - StartCanServer(CAN2); - - for (;;) - { - osDelay(1000); // nothing to do - } -} - -extern "C" { -int _write(int file, const char* data, int len); -} - -// @brief This is what printf calls internally -int _write(int file, const char* data, int len) -{ -#ifdef DEBUG_VIA_USB_SERIAL - usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); - uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); -#endif - - uart5StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); - - return len; -} + +/* Includes ------------------------------------------------------------------*/ + +#include "communication.hpp" +#include "common_inc.h" + +/* Private defines -----------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +volatile bool endpointListValid = false; +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ +// @brief Sends a line on the specified output. + +osThreadId_t commTaskHandle; +const osThreadAttr_t commTask_attributes = { + .name = "commTask", + .stack_size = 45000, + .priority = (osPriority_t) osPriorityNormal, +}; + +void InitCommunication(void) +{ + // Start command handling thread + commTaskHandle = osThreadNew(CommunicationTask, nullptr, &commTask_attributes); + + while (!endpointListValid) + osDelay(1); +} + +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; +osThreadId_t usbIrqTaskHandle; + +void UsbDeferredInterruptTask(void* ctx) +{ + (void) ctx; // unused parameter + + for (;;) + { + // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) + osStatus semaphore_status = osSemaphoreAcquire(sem_usb_irq, osWaitForever); + if (semaphore_status == osOK) + { + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } +} + +// Thread to handle deffered processing of USB interrupt, and +// read commands out of the UART DMA circular buffer +void CommunicationTask(void* ctx) +{ + (void) ctx; // unused parameter + + CommitProtocol(); + + // Allow main init to continue + endpointListValid = true; + + StartUartServer(); + StartUsbServer(); + StartCanServer(CAN1); + StartCanServer(CAN2); + + for (;;) + { + osDelay(1000); // nothing to do + } +} + +extern "C" { +int _write(int file, const char* data, int len); +} + +// @brief This is what printf calls internally +int _write(int file, const char* data, int len) +{ + usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); + uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); + + return len; +} diff --git a/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_can.cpp b/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_can.cpp index 78bdaa0..b302124 100644 --- a/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_can.cpp +++ b/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_can.cpp @@ -1,252 +1,250 @@ -/* -* -* Zero-config node ID negotiation -* ------------------------------- -* -* A heartbeat message is a message with a 8 byte unique serial number as payload. -* A regular message is any message that is not a heartbeat message. -* -* All nodes MUST obey these four rules: -* -* a) At a given point in time, a node MUST consider a node ID taken (by others) -* if any of the following is true: -* - the node received a (not self-emitted) heartbeat message with that node ID -* within the last second -* - the node attempted and failed at sending a heartbeat message with that -* node ID within the last second (failed in the sense of not ACK'd) -* -* b) At a given point in time, a node MUST NOT consider a node ID self-assigned -* if, within the last second, it did not succeed in sending a heartbeat -* message with that node ID. -* -* c) At a given point in time, a node MUST NOT send any heartbeat message with -* a node ID that is taken. -* -* d) At a given point in time, a node MUST NOT send any regular message with -* a node ID that is not self-assigned. -* -* Hardware allocation -* ------------------- -* RX FIFO0: -* - filter bank 0: heartbeat messages -*/ - -#include "common_inc.h" -#include -#include - -// defined in can.c -extern CAN_HandleTypeDef hcan1; -extern CAN_HandleTypeDef hcan2; - -CAN_context can1Ctx; -CAN_context can2Ctx; -static CAN_context* ctxs = nullptr; -static CAN_RxHeaderTypeDef headerRx; -static uint8_t data[8]; - - -struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan) -{ - if (hcan->Instance == CAN1) - return &can1Ctx; - else if (hcan->Instance == CAN2) - return &can2Ctx; - else - return nullptr; -} - -bool StartCanServer(CAN_TypeDef* hcan) -{ - if (hcan == CAN1) - { - ctxs = &can1Ctx; - ctxs->handle = &hcan1; - } else if (hcan == CAN2) - { - ctxs = &can2Ctx; - ctxs->handle = &hcan2; - } else - return false; // fail if none of the above checks matched - - HAL_StatusTypeDef status; - - ctxs->node_id = 0; - ctxs->serial_number = serialNumber; - osSemaphoreDef(sem_send_heartbeat); - ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat)); - - //// Set up filter - CAN_FilterTypeDef sFilterConfig = { - .FilterIdHigh = 0x0000, - .FilterIdLow = 0x0000, - .FilterMaskIdHigh = 0x0000, - .FilterMaskIdLow = 0x0000, - .FilterFIFOAssignment = CAN_RX_FIFO0, - .FilterBank = 0, - .FilterMode = CAN_FILTERMODE_IDMASK, - .FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters - .FilterActivation = ENABLE, - .SlaveStartFilterBank = 0 - }; - status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig); - if (status != HAL_OK) - return false; - - status = HAL_CAN_Start(ctxs->handle); - if (status != HAL_OK) - return false; - - status = HAL_CAN_ActivateNotification(ctxs->handle, - CAN_IT_TX_MAILBOX_EMPTY | - CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | - /* we probably only want this */ - CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL | - CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN | - CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK | - CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE | - CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE | - CAN_IT_ERROR); - if (status != HAL_OK) - return false; - - return true; -} - -void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx) -{ -// CAN_context* ctx = get_can_ctx(hcan); -// if (!ctx) return; -// ctx->tx_msg_cnt++; - - if (hcan->Instance == CAN1) - osSemaphoreRelease(sem_can1_tx); - else if (hcan->Instance == CAN2) - osSemaphoreRelease(sem_can2_tx); -} - -void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx) -{ - if (!get_can_ctx(hcan)) - return; - get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++; -} - -void tx_error(CAN_context* ctx, uint8_t mailbox_idx) -{ -} - -void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan) -{ tx_complete_callback(hcan, 0); } - -void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan) -{ tx_complete_callback(hcan, 1); } - -void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan) -{ tx_complete_callback(hcan, 2); } - -void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan) -{ tx_aborted_callback(hcan, 0); } - -void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan) -{ tx_aborted_callback(hcan, 1); } - -void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan) -{ tx_aborted_callback(hcan, 2); } - -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) -{ - CAN_context* ctx = get_can_ctx(hcan); - if (!ctx) return; - ctx->received_msg_cnt++; - - HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data); - if (status != HAL_OK) - { - ctx->unexpected_errors++; - return; - } - - OnCanMessage(ctx, &headerRx, data); -} - -void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan) -{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; } - -void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan) -{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; } - -void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan) -{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; } - -void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan) -{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; } - -void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan) -{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; } - -void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan) -{ - //__asm volatile ("bkpt"); - CAN_context* ctx = get_can_ctx(hcan); - if (!ctx) return; - volatile uint32_t original_error = hcan->ErrorCode; - (void) original_error; - - // handle transmit errors in all three mailboxes - if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0) - { - SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ); - hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0; - } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0) - { - tx_error(ctx, 0); - hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; - hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; - hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0; - } - - if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1) - { - SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ); - hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1; - } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1) - { - tx_error(ctx, 1); - hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; - hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; - hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1; - } - - if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2) - { - SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ); - hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2; - } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2) - { - tx_error(ctx, 2); - hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; - hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; - hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2; - } - - if (hcan->ErrorCode) - ctx->unexpected_errors++; -} - -void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader) -{ - osStatus semaphore_status; - if (canCtx->handle->Instance == CAN1) - semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever); - else if (canCtx->handle->Instance == CAN2) - semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever); - else - return; - - if (semaphore_status == osOK) - { - HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox); - } -} - +/* +* +* Zero-config node ID negotiation +* ------------------------------- +* +* A heartbeat message is a message with a 8 byte unique serial number as payload. +* A regular message is any message that is not a heartbeat message. +* +* All nodes MUST obey these four rules: +* +* a) At a given point in time, a node MUST consider a node ID taken (by others) +* if any of the following is true: +* - the node received a (not self-emitted) heartbeat message with that node ID +* within the last second +* - the node attempted and failed at sending a heartbeat message with that +* node ID within the last second (failed in the sense of not ACK'd) +* +* b) At a given point in time, a node MUST NOT consider a node ID self-assigned +* if, within the last second, it did not succeed in sending a heartbeat +* message with that node ID. +* +* c) At a given point in time, a node MUST NOT send any heartbeat message with +* a node ID that is taken. +* +* d) At a given point in time, a node MUST NOT send any regular message with +* a node ID that is not self-assigned. +* +* Hardware allocation +* ------------------- +* RX FIFO0: +* - filter bank 0: heartbeat messages +*/ + +#include "common_inc.h" +#include +#include + +// defined in can.c +extern CAN_HandleTypeDef hcan1; +extern CAN_HandleTypeDef hcan2; + +CAN_context can1Ctx; +CAN_context can2Ctx; +static CAN_context* ctxs = nullptr; +static CAN_RxHeaderTypeDef headerRx; +static uint8_t data[8]; + + +struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan) +{ + if (hcan->Instance == CAN1) + return &can1Ctx; + else if (hcan->Instance == CAN2) + return &can2Ctx; + else + return nullptr; +} + +bool StartCanServer(CAN_TypeDef* hcan) +{ + if (hcan == CAN1) + { + ctxs = &can1Ctx; + ctxs->handle = &hcan1; + } else if (hcan == CAN2) + { + ctxs = &can2Ctx; + ctxs->handle = &hcan2; + } else + return false; // fail if none of the above checks matched + + HAL_StatusTypeDef status; + + ctxs->node_id = 0; + ctxs->serial_number = serialNumber; + osSemaphoreDef(sem_send_heartbeat); + ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat)); + + //// Set up filter + CAN_FilterTypeDef sFilterConfig = { + .FilterIdHigh = 0x0000, + .FilterIdLow = 0x0000, + .FilterMaskIdHigh = 0x0000, + .FilterMaskIdLow = 0x0000, + .FilterFIFOAssignment = CAN_RX_FIFO0, + .FilterBank = 0, + .FilterMode = CAN_FILTERMODE_IDMASK, + .FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters + .FilterActivation = ENABLE, + .SlaveStartFilterBank = 0 + }; + status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig); + if (status != HAL_OK) + return false; + + status = HAL_CAN_Start(ctxs->handle); + if (status != HAL_OK) + return false; + + status = HAL_CAN_ActivateNotification(ctxs->handle, + CAN_IT_TX_MAILBOX_EMPTY | + CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | + /* we probably only want this */ + CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL | + CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN | + CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK | + CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE | + CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE | + CAN_IT_ERROR); + if (status != HAL_OK) + return false; + + return true; +} + +void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx) +{ +// CAN_context* ctx = get_can_ctx(hcan); +// if (!ctx) return; +// ctx->tx_msg_cnt++; + + if (hcan->Instance == CAN1) + osSemaphoreRelease(sem_can1_tx); + else if (hcan->Instance == CAN2) + osSemaphoreRelease(sem_can2_tx); +} + +void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx) +{ + if (!get_can_ctx(hcan)) + return; + get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++; +} + +void tx_error(CAN_context* ctx, uint8_t mailbox_idx) +{ +} + +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan) +{ tx_complete_callback(hcan, 0); } + +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan) +{ tx_complete_callback(hcan, 1); } + +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan) +{ tx_complete_callback(hcan, 2); } + +void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan) +{ tx_aborted_callback(hcan, 0); } + +void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan) +{ tx_aborted_callback(hcan, 1); } + +void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan) +{ tx_aborted_callback(hcan, 2); } + +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) +{ + CAN_context* ctx = get_can_ctx(hcan); + if (!ctx) return; + ctx->received_msg_cnt++; + + HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data); + if (status != HAL_OK) + { + ctx->unexpected_errors++; + return; + } + + OnCanMessage(ctx, &headerRx, data); +} + +void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan) +{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; } + +void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan) +{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; } + +void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan) +{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; } + +void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan) +{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; } + +void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan) +{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; } + +void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan) +{ + //__asm volatile ("bkpt"); + CAN_context* ctx = get_can_ctx(hcan); + if (!ctx) return; + volatile uint32_t original_error = hcan->ErrorCode; + (void) original_error; + + // handle transmit errors in all three mailboxes + if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0) + { + SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ); + hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0; + } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0) + { + tx_error(ctx, 0); + hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; + hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; + hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0; + } + + if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1) + { + SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ); + hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1; + } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1) + { + tx_error(ctx, 1); + hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; + hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; + hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1; + } + + if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2) + { + SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ); + hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2; + } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2) + { + tx_error(ctx, 2); + hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; + hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; + hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2; + } + + if (hcan->ErrorCode) + ctx->unexpected_errors++; +} + +void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader) +{ + osStatus semaphore_status; + if (canCtx->handle->Instance == CAN1) + semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever); + else if (canCtx->handle->Instance == CAN2) + semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever); + else + return; + + if (semaphore_status == osOK) + HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox); +} + diff --git a/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_uart.cpp b/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_uart.cpp index 99f4d3f..639e6e1 100644 --- a/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_uart.cpp +++ b/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_uart.cpp @@ -1,206 +1,206 @@ -#include "common_inc.h" -#include "interface_uart.hpp" -#include "ascii_processor.hpp" -#include "fibre/protocol.hpp" -#include "usart.h" - -#define UART_TX_BUFFER_SIZE 64 -#define UART_RX_BUFFER_SIZE 64 - -// DMA open loop continous circular buffer -// 1ms delay periodic, chase DMA ptr around -static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE]; -static uint32_t dma_last_rcv_idx[2]; - -// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -// static thread_local uint32_t deadline_ms = 0; - -osThreadId_t uartServerTaskHandle; - - -class UART4Sender : public StreamSink -{ -public: - UART4Sender() - { - channelType = CHANNEL_TYPE_UART4; - } - - int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override - { - // Loop to ensure all bytes get sent - while (length) - { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - if (processed_bytes) - *processed_bytes += chunk; - } - return 0; - } - - size_t get_free_space() override - { return SIZE_MAX; } - -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_stream_output; - -class UART5Sender : public StreamSink -{ -public: - UART5Sender() - { - channelType = CHANNEL_TYPE_UART5; - } - - int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override - { - // Loop to ensure all bytes get sent - while (length) - { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - if (processed_bytes) - *processed_bytes += chunk; - } - return 0; - } - - size_t get_free_space() override - { return SIZE_MAX; } - -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart5_stream_output; - -StreamSink* uart4StreamOutputPtr = &uart4_stream_output; -StreamBasedPacketSink uart4_packet_output(uart4_stream_output); -BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); -StreamToPacketSegmenter uart4_stream_input(uart4_channel); - -StreamSink* uart5StreamOutputPtr = &uart5_stream_output; -StreamBasedPacketSink uart5_packet_output(uart5_stream_output); -BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output); -StreamToPacketSegmenter uart5_stream_input(uart5_channel); - -static void UartServerTask(void* ctx) -{ - (void) ctx; - - for (;;) - { - // Check for UART errors and restart recieve DMA transfer if required - if (huart4.ErrorCode != HAL_UART_ERROR_NONE) - { - HAL_UART_AbortReceive(&huart4); - HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0])); - } - // Fetch the circular buffer "write pointer", where it would write next - uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < dma_last_rcv_idx[0]) - { - uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0], - UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], - nullptr); // TODO: use process_all - ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0], - UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output); - dma_last_rcv_idx[0] = 0; - } - if (new_rcv_idx > dma_last_rcv_idx[0]) - { - uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0], - new_rcv_idx - dma_last_rcv_idx[0], - nullptr); // TODO: use process_all - ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0], - new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output); - dma_last_rcv_idx[0] = new_rcv_idx; - } - - - // Check for UART errors and restart recieve DMA transfer if required - if (huart5.ErrorCode != HAL_UART_ERROR_NONE) - { - HAL_UART_AbortReceive(&huart5); - HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1])); - } - // Fetch the circular buffer "write pointer", where it would write next - new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR; - - // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < dma_last_rcv_idx[1]) - { - uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1], - UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], - nullptr); // TODO: use process_all - ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1], - UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output); - dma_last_rcv_idx[1] = 0; - } - if (new_rcv_idx > dma_last_rcv_idx[1]) - { - uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1], - new_rcv_idx - dma_last_rcv_idx[1], - nullptr); // TODO: use process_all - ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1], - new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output); - dma_last_rcv_idx[1] = new_rcv_idx; - } - - - osDelay(1); - }; -} - -const osThreadAttr_t uartServerTask_attributes = { - .name = "UartServerTask", - .stack_size = 1000 * 4, - .priority = (osPriority_t) osPriorityNormal, -}; - -void StartUartServer() -{ - // DMA is set up to recieve in a circular buffer forever. - // We dont use interrupts to fetch the data, instead we periodically read - // data out of the circular buffer into a parse buffer, controlled by a state machine - HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0])); - dma_last_rcv_idx[0] = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1])); - dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR; - - // Start UART communication thread - uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes); -} - -void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) -{ - if (huart->Instance == UART4) - osSemaphoreRelease(sem_uart4_dma); - else if (huart->Instance == UART5) - osSemaphoreRelease(sem_uart5_dma); -} +#include "common_inc.h" +#include "interface_uart.hpp" +#include "ascii_processor.hpp" +#include "fibre/protocol.hpp" +#include "usart.h" + +#define UART_TX_BUFFER_SIZE 64 +#define UART_RX_BUFFER_SIZE 64 + +// DMA open loop continous circular buffer +// 1ms delay periodic, chase DMA ptr around +static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE]; +static uint32_t dma_last_rcv_idx[2]; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +// static thread_local uint32_t deadline_ms = 0; + +osThreadId_t uartServerTaskHandle; + + +class UART4Sender : public StreamSink +{ +public: + UART4Sender() + { + channelType = CHANNEL_TYPE_UART4; + } + + int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override + { + // Loop to ensure all bytes get sent + while (length) + { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + if (processed_bytes) + *processed_bytes += chunk; + } + return 0; + } + + size_t get_free_space() override + { return SIZE_MAX; } + +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart4_stream_output; + +class UART5Sender : public StreamSink +{ +public: + UART5Sender() + { + channelType = CHANNEL_TYPE_UART5; + } + + int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override + { + // Loop to ensure all bytes get sent + while (length) + { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + if (processed_bytes) + *processed_bytes += chunk; + } + return 0; + } + + size_t get_free_space() override + { return SIZE_MAX; } + +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart5_stream_output; + +StreamSink* uart4StreamOutputPtr = &uart4_stream_output; +StreamBasedPacketSink uart4_packet_output(uart4_stream_output); +BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); +StreamToPacketSegmenter uart4_stream_input(uart4_channel); + +StreamSink* uart5StreamOutputPtr = &uart5_stream_output; +StreamBasedPacketSink uart5_packet_output(uart5_stream_output); +BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output); +StreamToPacketSegmenter uart5_stream_input(uart5_channel); + +static void UartServerTask(void* ctx) +{ + (void) ctx; + + for (;;) + { + // Check for UART errors and restart recieve DMA transfer if required + if (huart4.ErrorCode != HAL_UART_ERROR_NONE) + { + HAL_UART_AbortReceive(&huart4); + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0])); + } + // Fetch the circular buffer "write pointer", where it would write next + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < dma_last_rcv_idx[0]) + { + uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0], + UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], + nullptr); // TODO: use process_all + ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0], + UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output); + dma_last_rcv_idx[0] = 0; + } + if (new_rcv_idx > dma_last_rcv_idx[0]) + { + uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0], + new_rcv_idx - dma_last_rcv_idx[0], + nullptr); // TODO: use process_all + ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0], + new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output); + dma_last_rcv_idx[0] = new_rcv_idx; + } + + + // Check for UART errors and restart recieve DMA transfer if required + if (huart5.ErrorCode != HAL_UART_ERROR_NONE) + { + HAL_UART_AbortReceive(&huart5); + HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1])); + } + // Fetch the circular buffer "write pointer", where it would write next + new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR; + + // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < dma_last_rcv_idx[1]) + { + uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1], + UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], + nullptr); // TODO: use process_all + ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1], + UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output); + dma_last_rcv_idx[1] = 0; + } + if (new_rcv_idx > dma_last_rcv_idx[1]) + { + uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1], + new_rcv_idx - dma_last_rcv_idx[1], + nullptr); // TODO: use process_all + ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1], + new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output); + dma_last_rcv_idx[1] = new_rcv_idx; + } + + + osDelay(1); + }; +} + +const osThreadAttr_t uartServerTask_attributes = { + .name = "UartServerTask", + .stack_size = 2000, + .priority = (osPriority_t) osPriorityNormal, +}; + +void StartUartServer() +{ + // DMA is set up to receive in a circular buffer forever. + // We don't use interrupts to fetch the data, instead we periodically read + // data out of the circular buffer into a parse buffer, controlled by a state machine + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0])); + dma_last_rcv_idx[0] = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1])); + dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR; + + // Start UART communication thread + uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes); +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) +{ + if (huart->Instance == UART4) + osSemaphoreRelease(sem_uart4_dma); + else if (huart->Instance == UART5) + osSemaphoreRelease(sem_uart5_dma); +} diff --git a/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_usb.cpp b/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_usb.cpp index e6cd59a..8af5226 100644 --- a/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_usb.cpp +++ b/2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_usb.cpp @@ -1,190 +1,190 @@ -#include "common_inc.h" -#include "ascii_processor.hpp" -#include "usbd_cdc.h" -#include "usbd_cdc_if.h" -#include "usb_device.h" -#include "interface_usb.hpp" - -osThreadId_t usbServerTaskHandle; -USBStats_t usb_stats_ = {0}; - -class USBSender : public PacketSink -{ -public: - USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx) - : endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) - {} - - int process_packet(const uint8_t *buffer, size_t length) override - { - // cannot send partial packets - if (length > USB_TX_DATA_SIZE) - return -1; - // wait for USB interface to become ready - if (osSemaphoreAcquire(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) - { - // If the host resets the device it might be that the TX-complete handler is never called - // and the sem_usb_tx_ semaphore is never released. To handle this we just override the - // TX buffer if this wait times out. The implication is that the channel is no longer lossless. - // TODO: handle endpoint reset properly - usb_stats_.tx_overrun_cnt++; - } - - // transmit packet - uint8_t status = CDC_Transmit_FS(const_cast(buffer), length, endpoint_pair_); - if (status != USBD_OK) - { - osSemaphoreRelease(sem_usb_tx_); - return -1; - } - usb_stats_.tx_cnt++; - - return 0; - } - -private: - uint8_t endpoint_pair_; - const osSemaphoreId &sem_usb_tx_; -}; - -// Note we could have independent semaphores here to allow concurrent transmission -USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx); -USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx); - -class TreatPacketSinkAsStreamSink : public StreamSink -{ -public: - TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output) - { - channelType = CHANNEL_TYPE_USB; - } - - int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes) - { - // Loop to ensure all bytes get sent - while (length) - { - size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - if (output_.process_packet(buffer, length) != 0) - return -1; - buffer += chunk; - length -= chunk; - if (processed_bytes) - *processed_bytes += chunk; - } - return 0; - } - - size_t get_free_space() - { return SIZE_MAX; } - - - -private: - PacketSink &output_; -} usb_stream_output(usb_packet_output_cdc); - -// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed) -// TODO: less spaghetti code -StreamSink *usbStreamOutputPtr = &usb_stream_output; - -BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native); - - -struct USBInterface -{ - uint8_t *rx_buf = nullptr; - uint32_t rx_len = 0; - bool data_pending = false; - uint8_t out_ep; - uint8_t in_ep; - USBSender &usb_sender; -}; - -// Note: statics make this less modular. -// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread -static USBInterface CDC_interface = { - .rx_buf = nullptr, - .rx_len = 0, - .data_pending = false, - .out_ep = CDC_OUT_EP, - .in_ep = CDC_IN_EP, - .usb_sender = usb_packet_output_cdc, -}; -static USBInterface ODrive_interface = { - .rx_buf = nullptr, - .rx_len = 0, - .data_pending = false, - .out_ep = ODRIVE_OUT_EP, - .in_ep = ODRIVE_IN_EP, - .usb_sender = usb_packet_output_native, -}; - -static void UsbServerTask(void *ctx) -{ - (void) ctx; - - for (;;) - { - // const uint32_t usb_check_timeout = 1; // ms - osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever); - if (sem_stat == osOK) - { - usb_stats_.rx_cnt++; - - // CDC Interface - if (CDC_interface.data_pending) - { - CDC_interface.data_pending = false; - - ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output); - USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet - } - - // Native Interface - if (ODrive_interface.data_pending) - { - ODrive_interface.data_pending = false; - usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len); - USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet - } - } - } -} - -// Called from CDC_Receive_FS callback function, this allows the communication -// thread to handle the incoming data -void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) -{ - USBInterface *usb_iface; - if (endpoint_pair == CDC_interface.out_ep) - { - usb_iface = &CDC_interface; - } else if (endpoint_pair == ODrive_interface.out_ep) - { - usb_iface = &ODrive_interface; - } else - { - return; - } - - // We don't allow the next USB packet until the previous one has been processed completely. - // Therefore it's safe to write to these vars directly since we know previous processing is complete. - usb_iface->rx_buf = buf; - usb_iface->rx_len = len; - usb_iface->data_pending = true; - osSemaphoreRelease(sem_usb_rx); -} - - -const osThreadAttr_t usbServerTask_attributes = { - .name = "UsbServerTask", - .stack_size = 512 * 4, - .priority = (osPriority_t) osPriorityNormal, -}; - -void StartUsbServer() -{ - // Start USB communication thread - usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes); -} +#include "common_inc.h" +#include "ascii_processor.hpp" +#include "usbd_cdc.h" +#include "usbd_cdc_if.h" +#include "usb_device.h" +#include "interface_usb.hpp" + +osThreadId_t usbServerTaskHandle; +USBStats_t usb_stats_ = {0}; + +class USBSender : public PacketSink +{ +public: + USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx) + : endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) + {} + + int process_packet(const uint8_t *buffer, size_t length) override + { + // cannot send partial packets + if (length > USB_TX_DATA_SIZE) + return -1; + // wait for USB interface to become ready + if (osSemaphoreAcquire(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) + { + // If the host resets the device it might be that the TX-complete handler is never called + // and the sem_usb_tx_ semaphore is never released. To handle this we just override the + // TX buffer if this wait times out. The implication is that the channel is no longer lossless. + // TODO: handle endpoint reset properly + usb_stats_.tx_overrun_cnt++; + } + + // transmit packet + uint8_t status = CDC_Transmit_FS(const_cast(buffer), length, endpoint_pair_); + if (status != USBD_OK) + { + osSemaphoreRelease(sem_usb_tx_); + return -1; + } + usb_stats_.tx_cnt++; + + return 0; + } + +private: + uint8_t endpoint_pair_; + const osSemaphoreId &sem_usb_tx_; +}; + +// Note we could have independent semaphores here to allow concurrent transmission +USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx); +USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx); + +class TreatPacketSinkAsStreamSink : public StreamSink +{ +public: + TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output) + { + channelType = CHANNEL_TYPE_USB; + } + + int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes) + { + // Loop to ensure all bytes get sent + while (length) + { + size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; + if (output_.process_packet(buffer, length) != 0) + return -1; + buffer += chunk; + length -= chunk; + if (processed_bytes) + *processed_bytes += chunk; + } + return 0; + } + + size_t get_free_space() + { return SIZE_MAX; } + + + +private: + PacketSink &output_; +} usb_stream_output(usb_packet_output_cdc); + +// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed) +// TODO: less spaghetti code +StreamSink *usbStreamOutputPtr = &usb_stream_output; + +BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native); + + +struct USBInterface +{ + uint8_t *rx_buf = nullptr; + uint32_t rx_len = 0; + bool data_pending = false; + uint8_t out_ep; + uint8_t in_ep; + USBSender &usb_sender; +}; + +// Note: statics make this less modular. +// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread +static USBInterface CDC_interface = { + .rx_buf = nullptr, + .rx_len = 0, + .data_pending = false, + .out_ep = CDC_OUT_EP, + .in_ep = CDC_IN_EP, + .usb_sender = usb_packet_output_cdc, +}; +static USBInterface ODrive_interface = { + .rx_buf = nullptr, + .rx_len = 0, + .data_pending = false, + .out_ep = ODRIVE_OUT_EP, + .in_ep = ODRIVE_IN_EP, + .usb_sender = usb_packet_output_native, +}; + +static void UsbServerTask(void *ctx) +{ + (void) ctx; + + for (;;) + { + // const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever); + if (sem_stat == osOK) + { + usb_stats_.rx_cnt++; + + // CDC Interface + if (CDC_interface.data_pending) + { + CDC_interface.data_pending = false; + + ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output); + USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet + } + + // Native Interface + if (ODrive_interface.data_pending) + { + ODrive_interface.data_pending = false; + usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len); + USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet + } + } + } +} + +// Called from CDC_Receive_FS callback function, this allows the communication +// thread to handle the incoming data +void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) +{ + USBInterface *usb_iface; + if (endpoint_pair == CDC_interface.out_ep) + { + usb_iface = &CDC_interface; + } else if (endpoint_pair == ODrive_interface.out_ep) + { + usb_iface = &ODrive_interface; + } else + { + return; + } + + // We don't allow the next USB packet until the previous one has been processed completely. + // Therefore it's safe to write to these vars directly since we know previous processing is complete. + usb_iface->rx_buf = buf; + usb_iface->rx_len = len; + usb_iface->data_pending = true; + osSemaphoreRelease(sem_usb_rx); +} + + +const osThreadAttr_t usbServerTask_attributes = { + .name = "UsbServerTask", + .stack_size = 2000, + .priority = (osPriority_t) osPriorityNormal, +}; + +void StartUsbServer() +{ + // Start USB communication thread + usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes); +} diff --git a/2.Firmware/Core-STM32F4-fw/CMakeLists.txt b/2.Firmware/Core-STM32F4-fw/CMakeLists.txt index baa8873..ab7043a 100644 --- a/2.Firmware/Core-STM32F4-fw/CMakeLists.txt +++ b/2.Firmware/Core-STM32F4-fw/CMakeLists.txt @@ -1,114 +1,113 @@ -#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! -set(CMAKE_SYSTEM_NAME Generic) -set(CMAKE_SYSTEM_VERSION 1) -cmake_minimum_required(VERSION 3.19) - -# specify cross compilers and tools -set(CMAKE_C_COMPILER arm-none-eabi-gcc) -set(CMAKE_CXX_COMPILER arm-none-eabi-g++) -set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) -set(CMAKE_AR arm-none-eabi-ar) -set(CMAKE_OBJCOPY arm-none-eabi-objcopy) -set(CMAKE_OBJDUMP arm-none-eabi-objdump) -set(SIZE arm-none-eabi-size) -set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) - -# project settings -project(Core-STM32F4-fw C CXX ASM) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_C_STANDARD 11) - -# for use printf & scanf with float -set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float") - -#Uncomment for hardware floating point -add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) -add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) -add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) - -#Uncomment for software floating point -#add_compile_options(-mfloat-abi=soft) - -add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) -add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) - -# uncomment to mitigate c++17 absolute addresses warnings -#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") - -if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") - message(STATUS "Maximum optimization for speed") - add_compile_options(-Ofast) -elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") - message(STATUS "Maximum optimization for speed, debug info included") - add_compile_options(-Ofast -g) -elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") - message(STATUS "Maximum optimization for size") - add_compile_options(-Os) -else () - message(STATUS "Minimal optimization, debug info included") - add_compile_options(-Og -g) -endif () - -include_directories( - Core/Inc - Drivers/STM32F4xx_HAL_Driver/Inc - Drivers/STM32F4xx_HAL_Driver/Inc/Legacy - Drivers/CMSIS/Device/ST/STM32F4xx/Include - Drivers/CMSIS/Include - Middlewares/Third_Party/FreeRTOS/Source/include - Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 - Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F - Middlewares/ST/STM32_USB_Device_Library/Core/Inc - Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc - USB_DEVICE/App - USB_DEVICE/Target - 3rdParty/fibre/cpp/include - 3rdParty/u8g2 - 3rdParty/u8g2/cpp - Bsp - Bsp/imu - Bsp/imu/filters - Bsp/communication - Bsp/memory - Bsp/utils - Bsp/gpio - Bsp/utils/software_i2c - Bsp/utils/arm_math - Robot - UserApp -) - -add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP) -add_definitions(-DDEBUG_VIA_USB_SERIAL) - -file(GLOB_RECURSE SOURCES - "startup/*.*" - "Drivers/*.*" - "Core/*.*" - "UserApp/*.*" - "3rdParty/*.*" - "Middlewares/*.*" - "USB_DEVICE/*.*" - "Robot/*.*" - "Bsp/*.*" - ) - -set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld) - -add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map) -add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) -add_link_options(-T ${LINKER_SCRIPT}) - -link_directories("Drivers/CMSIS/Lib") -link_libraries("arm_cortexM4lf_math.a") - -add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}) - -set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) -set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin) - -add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD - COMMAND ${CMAKE_OBJCOPY} -Oihex $ ${HEX_FILE} - COMMAND ${CMAKE_OBJCOPY} -Obinary $ ${BIN_FILE} - COMMENT "Building ${HEX_FILE} -Building ${BIN_FILE}") +#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_VERSION 1) +cmake_minimum_required(VERSION 3.19) + +# specify cross compilers and tools +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) +set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) +set(CMAKE_AR arm-none-eabi-ar) +set(CMAKE_OBJCOPY arm-none-eabi-objcopy) +set(CMAKE_OBJDUMP arm-none-eabi-objdump) +set(SIZE arm-none-eabi-size) +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# project settings +project(Core-STM32F4-fw C CXX ASM) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +# for use printf & scanf with float +set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float") + +#Uncomment for hardware floating point +add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) +add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) +add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) + +#Uncomment for software floating point +#add_compile_options(-mfloat-abi=soft) + +add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) +add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) + +# uncomment to mitigate c++17 absolute addresses warnings +#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") + +if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") + message(STATUS "Maximum optimization for speed") + add_compile_options(-Ofast) +elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + message(STATUS "Maximum optimization for speed, debug info included") + add_compile_options(-Ofast -g) +elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") + message(STATUS "Maximum optimization for size") + add_compile_options(-Os) +else () + message(STATUS "Minimal optimization, debug info included") + add_compile_options(-Og -g) +endif () + +include_directories( + Core/Inc + Drivers/STM32F4xx_HAL_Driver/Inc + Drivers/STM32F4xx_HAL_Driver/Inc/Legacy + Drivers/CMSIS/Device/ST/STM32F4xx/Include + Drivers/CMSIS/Include + Middlewares/Third_Party/FreeRTOS/Source/include + Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 + Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F + Middlewares/ST/STM32_USB_Device_Library/Core/Inc + Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc + USB_DEVICE/App + USB_DEVICE/Target + 3rdParty/fibre/cpp/include + 3rdParty/u8g2 + 3rdParty/u8g2/cpp + Bsp + Bsp/imu + Bsp/imu/filters + Bsp/communication + Bsp/memory + Bsp/utils + Bsp/gpio + Bsp/utils/software_i2c + Bsp/utils/arm_math + Robot + UserApp +) + +add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP) + +file(GLOB_RECURSE SOURCES + "startup/*.*" + "Drivers/*.*" + "Core/*.*" + "UserApp/*.*" + "3rdParty/*.*" + "Middlewares/*.*" + "USB_DEVICE/*.*" + "Robot/*.*" + "Bsp/*.*" + ) + +set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld) + +add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map) +add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) +add_link_options(-T ${LINKER_SCRIPT}) + +link_directories("Drivers/CMSIS/Lib") +link_libraries("arm_cortexM4lf_math.a") + +add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}) + +set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) +set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin) + +add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD + COMMAND ${CMAKE_OBJCOPY} -Oihex $ ${HEX_FILE} + COMMAND ${CMAKE_OBJCOPY} -Obinary $ ${BIN_FILE} + COMMENT "Building ${HEX_FILE} +Building ${BIN_FILE}") diff --git a/2.Firmware/Core-STM32F4-fw/CMakeLists_template.txt b/2.Firmware/Core-STM32F4-fw/CMakeLists_template.txt index 07ea271..ab7043a 100644 --- a/2.Firmware/Core-STM32F4-fw/CMakeLists_template.txt +++ b/2.Firmware/Core-STM32F4-fw/CMakeLists_template.txt @@ -1,114 +1,113 @@ -#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! -set(CMAKE_SYSTEM_NAME Generic) -set(CMAKE_SYSTEM_VERSION 1) -cmake_minimum_required(VERSION 3.19) - -# specify cross compilers and tools -set(CMAKE_C_COMPILER arm-none-eabi-gcc) -set(CMAKE_CXX_COMPILER arm-none-eabi-g++) -set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) -set(CMAKE_AR arm-none-eabi-ar) -set(CMAKE_OBJCOPY arm-none-eabi-objcopy) -set(CMAKE_OBJDUMP arm-none-eabi-objdump) -set(SIZE arm-none-eabi-size) -set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) - -# project settings -project(Core-STM32F4-fw C CXX ASM) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_C_STANDARD 11) - -# for use printf & scanf with float -set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float") - -#Uncomment for hardware floating point -add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) -add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) -add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) - -#Uncomment for software floating point -#add_compile_options(-mfloat-abi=soft) - -add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) -add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) - -# uncomment to mitigate c++17 absolute addresses warnings -#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") - -if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") - message(STATUS "Maximum optimization for speed") - add_compile_options(-Ofast) -elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") - message(STATUS "Maximum optimization for speed, debug info included") - add_compile_options(-Ofast -g) -elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") - message(STATUS "Maximum optimization for size") - add_compile_options(-Os) -else () - message(STATUS "Minimal optimization, debug info included") - add_compile_options(-Og -g) -endif () - -include_directories( - Core/Inc - Drivers/STM32F4xx_HAL_Driver/Inc - Drivers/STM32F4xx_HAL_Driver/Inc/Legacy - Drivers/CMSIS/Device/ST/STM32F4xx/Include - Drivers/CMSIS/Include - Middlewares/Third_Party/FreeRTOS/Source/include - Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 - Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F - Middlewares/ST/STM32_USB_Device_Library/Core/Inc - Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc - USB_DEVICE/App - USB_DEVICE/Target - 3rdParty/fibre/cpp/include - 3rdParty/u8g2 - 3rdParty/u8g2/cpp - Bsp - Bsp/imu - Bsp/imu/filters - Bsp/communication - Bsp/memory - Bsp/utils - Bsp/gpio - Bsp/utils/software_i2c - Bsp/utils/arm_math - Robot - UserApp -) - -add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP) -# add_definitions(-DDEBUG_VIA_USB_SERIAL) - -file(GLOB_RECURSE SOURCES - "startup/*.*" - "Drivers/*.*" - "Core/*.*" - "UserApp/*.*" - "3rdParty/*.*" - "Middlewares/*.*" - "USB_DEVICE/*.*" - "Robot/*.*" - "Bsp/*.*" - ) - -set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld) - -add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map) -add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) -add_link_options(-T ${LINKER_SCRIPT}) - -link_directories("Drivers/CMSIS/Lib") -link_libraries("arm_cortexM4lf_math.a") - -add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}) - -set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) -set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin) - -add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD - COMMAND ${CMAKE_OBJCOPY} -Oihex $ ${HEX_FILE} - COMMAND ${CMAKE_OBJCOPY} -Obinary $ ${BIN_FILE} - COMMENT "Building ${HEX_FILE} -Building ${BIN_FILE}") +#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_VERSION 1) +cmake_minimum_required(VERSION 3.19) + +# specify cross compilers and tools +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) +set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) +set(CMAKE_AR arm-none-eabi-ar) +set(CMAKE_OBJCOPY arm-none-eabi-objcopy) +set(CMAKE_OBJDUMP arm-none-eabi-objdump) +set(SIZE arm-none-eabi-size) +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# project settings +project(Core-STM32F4-fw C CXX ASM) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +# for use printf & scanf with float +set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float") + +#Uncomment for hardware floating point +add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) +add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) +add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) + +#Uncomment for software floating point +#add_compile_options(-mfloat-abi=soft) + +add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) +add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) + +# uncomment to mitigate c++17 absolute addresses warnings +#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") + +if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") + message(STATUS "Maximum optimization for speed") + add_compile_options(-Ofast) +elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + message(STATUS "Maximum optimization for speed, debug info included") + add_compile_options(-Ofast -g) +elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") + message(STATUS "Maximum optimization for size") + add_compile_options(-Os) +else () + message(STATUS "Minimal optimization, debug info included") + add_compile_options(-Og -g) +endif () + +include_directories( + Core/Inc + Drivers/STM32F4xx_HAL_Driver/Inc + Drivers/STM32F4xx_HAL_Driver/Inc/Legacy + Drivers/CMSIS/Device/ST/STM32F4xx/Include + Drivers/CMSIS/Include + Middlewares/Third_Party/FreeRTOS/Source/include + Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 + Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F + Middlewares/ST/STM32_USB_Device_Library/Core/Inc + Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc + USB_DEVICE/App + USB_DEVICE/Target + 3rdParty/fibre/cpp/include + 3rdParty/u8g2 + 3rdParty/u8g2/cpp + Bsp + Bsp/imu + Bsp/imu/filters + Bsp/communication + Bsp/memory + Bsp/utils + Bsp/gpio + Bsp/utils/software_i2c + Bsp/utils/arm_math + Robot + UserApp +) + +add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP) + +file(GLOB_RECURSE SOURCES + "startup/*.*" + "Drivers/*.*" + "Core/*.*" + "UserApp/*.*" + "3rdParty/*.*" + "Middlewares/*.*" + "USB_DEVICE/*.*" + "Robot/*.*" + "Bsp/*.*" + ) + +set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld) + +add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map) +add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) +add_link_options(-T ${LINKER_SCRIPT}) + +link_directories("Drivers/CMSIS/Lib") +link_libraries("arm_cortexM4lf_math.a") + +add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}) + +set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) +set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin) + +add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD + COMMAND ${CMAKE_OBJCOPY} -Oihex $ ${HEX_FILE} + COMMAND ${CMAKE_OBJCOPY} -Obinary $ ${BIN_FILE} + COMMENT "Building ${HEX_FILE} +Building ${BIN_FILE}") diff --git a/2.Firmware/Core-STM32F4-fw/Core/Src/freertos.c b/2.Firmware/Core-STM32F4-fw/Core/Src/freertos.c index 2afe0af..b1d76ec 100644 --- a/2.Firmware/Core-STM32F4-fw/Core/Src/freertos.c +++ b/2.Firmware/Core-STM32F4-fw/Core/Src/freertos.c @@ -1,175 +1,175 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * File Name : freertos.c - * Description : Code for freertos applications - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2021 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under Ultimate Liberty license - * SLA0044, the "License"; You may not use this file except in compliance with - * the License. You may obtain a copy of the License at: - * www.st.com/SLA0044 - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Includes ------------------------------------------------------------------*/ -#include "FreeRTOS.h" -#include "task.h" -#include "main.h" -#include "cmsis_os.h" - -/* Private includes ----------------------------------------------------------*/ -/* USER CODE BEGIN Includes */ -#include "common_inc.h" -#include "communication.hpp" -/* USER CODE END Includes */ - -/* Private typedef -----------------------------------------------------------*/ -/* USER CODE BEGIN PTD */ - -/* USER CODE END PTD */ - -/* Private define ------------------------------------------------------------*/ -/* USER CODE BEGIN PD */ - -/* USER CODE END PD */ - -/* Private macro -------------------------------------------------------------*/ -/* USER CODE BEGIN PM */ - -/* USER CODE END PM */ - -/* Private variables ---------------------------------------------------------*/ -/* USER CODE BEGIN Variables */ - -// List of semaphores -osSemaphoreId sem_usb_irq; -osSemaphoreId sem_uart4_dma; -osSemaphoreId sem_uart5_dma; -osSemaphoreId sem_usb_rx; -osSemaphoreId sem_usb_tx; -osSemaphoreId sem_can1_tx; -osSemaphoreId sem_can2_tx; - -/* USER CODE END Variables */ -/* Definitions for defaultTask */ -osThreadId_t defaultTaskHandle; -const osThreadAttr_t defaultTask_attributes = { - .name = "defaultTask", - .stack_size = 500 * 4, - .priority = (osPriority_t) osPriorityNormal, -}; - -/* Private function prototypes -----------------------------------------------*/ -/* USER CODE BEGIN FunctionPrototypes */ - - -/* USER CODE END FunctionPrototypes */ - -void StartDefaultTask(void *argument); - -extern void MX_USB_DEVICE_Init(void); -void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ - -/** - * @brief FreeRTOS initialization - * @param None - * @retval None - */ -void MX_FREERTOS_Init(void) { - /* USER CODE BEGIN Init */ - - /* USER CODE END Init */ - - /* USER CODE BEGIN RTOS_MUTEX */ - /* add mutexes, ... */ - /* USER CODE END RTOS_MUTEX */ - - /* USER CODE BEGIN RTOS_SEMAPHORES */ - // Init usb irq binary semaphore, and start with no tokens by removing the starting one. - osSemaphoreDef(sem_usb_irq); - sem_usb_irq = osSemaphoreNew(1, 0, osSemaphore(sem_usb_irq)); - - // Create a semaphore for UART DMA and remove a token - osSemaphoreDef(sem_uart4_dma); - sem_uart4_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart4_dma)); - osSemaphoreDef(sem_uart5_dma); - sem_uart5_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart5_dma)); - - // Create a semaphore for USB RX, and start with no tokens by removing the starting one. - osSemaphoreDef(sem_usb_rx); - sem_usb_rx = osSemaphoreNew(1, 0, osSemaphore(sem_usb_rx)); - - // Create a semaphore for USB TX - osSemaphoreDef(sem_usb_tx); - sem_usb_tx = osSemaphoreNew(1, 1, osSemaphore(sem_usb_tx)); - - // Create a semaphore for CAN TX - osSemaphoreDef(sem_can1_tx); - sem_can1_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can1_tx)); - osSemaphoreDef(sem_can2_tx); - sem_can2_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can2_tx)); - - /* USER CODE END RTOS_SEMAPHORES */ - - /* USER CODE BEGIN RTOS_TIMERS */ - - /* USER CODE END RTOS_TIMERS */ - - /* USER CODE BEGIN RTOS_QUEUES */ - // This Task must run before MX_USB_DEVICE_Init(), so have to put it here. - const osThreadAttr_t usbIrqTask_attributes = { - .name = "usbIrqTask", - .stack_size = 128 * 4, - .priority = (osPriority_t) osPriorityAboveNormal, - }; - usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes); - - /* USER CODE END RTOS_QUEUES */ - - /* Create the thread(s) */ - /* creation of defaultTask */ - defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); - - /* USER CODE BEGIN RTOS_THREADS */ - /* add threads, ... */ - /* USER CODE END RTOS_THREADS */ - - /* USER CODE BEGIN RTOS_EVENTS */ - /* add events, ... */ - /* USER CODE END RTOS_EVENTS */ - -} - -/* USER CODE BEGIN Header_StartDefaultTask */ -/** - * @brief Function implementing the defaultTask thread. - * @param argument: Not used - * @retval None - */ -/* USER CODE END Header_StartDefaultTask */ -void StartDefaultTask(void *argument) -{ - /* init code for USB_DEVICE */ - MX_USB_DEVICE_Init(); - /* USER CODE BEGIN StartDefaultTask */ - - // Invoke cpp-version main(). - Main(); - - vTaskDelete(defaultTaskHandle); - /* USER CODE END StartDefaultTask */ -} - -/* Private application code --------------------------------------------------*/ -/* USER CODE BEGIN Application */ - -/* USER CODE END Application */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * File Name : freertos.c + * Description : Code for freertos applications + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2021 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under Ultimate Liberty license + * SLA0044, the "License"; You may not use this file except in compliance with + * the License. You may obtain a copy of the License at: + * www.st.com/SLA0044 + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "FreeRTOS.h" +#include "task.h" +#include "main.h" +#include "cmsis_os.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include "common_inc.h" +#include "communication.hpp" +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN Variables */ + +// List of semaphores +osSemaphoreId sem_usb_irq; +osSemaphoreId sem_uart4_dma; +osSemaphoreId sem_uart5_dma; +osSemaphoreId sem_usb_rx; +osSemaphoreId sem_usb_tx; +osSemaphoreId sem_can1_tx; +osSemaphoreId sem_can2_tx; + +/* USER CODE END Variables */ +/* Definitions for defaultTask */ +osThreadId_t defaultTaskHandle; +const osThreadAttr_t defaultTask_attributes = { + .name = "defaultTask", + .stack_size = 2000, + .priority = (osPriority_t) osPriorityNormal, +}; + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN FunctionPrototypes */ + + +/* USER CODE END FunctionPrototypes */ + +void StartDefaultTask(void *argument); + +extern void MX_USB_DEVICE_Init(void); +void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ + +/** + * @brief FreeRTOS initialization + * @param None + * @retval None + */ +void MX_FREERTOS_Init(void) { + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* USER CODE BEGIN RTOS_MUTEX */ + /* add mutexes, ... */ + /* USER CODE END RTOS_MUTEX */ + + /* USER CODE BEGIN RTOS_SEMAPHORES */ + // Init usb irq binary semaphore, and start with no tokens by removing the starting one. + osSemaphoreDef(sem_usb_irq); + sem_usb_irq = osSemaphoreNew(1, 0, osSemaphore(sem_usb_irq)); + + // Create a semaphore for UART DMA and remove a token + osSemaphoreDef(sem_uart4_dma); + sem_uart4_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart4_dma)); + osSemaphoreDef(sem_uart5_dma); + sem_uart5_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart5_dma)); + + // Create a semaphore for USB RX, and start with no tokens by removing the starting one. + osSemaphoreDef(sem_usb_rx); + sem_usb_rx = osSemaphoreNew(1, 0, osSemaphore(sem_usb_rx)); + + // Create a semaphore for USB TX + osSemaphoreDef(sem_usb_tx); + sem_usb_tx = osSemaphoreNew(1, 1, osSemaphore(sem_usb_tx)); + + // Create a semaphore for CAN TX + osSemaphoreDef(sem_can1_tx); + sem_can1_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can1_tx)); + osSemaphoreDef(sem_can2_tx); + sem_can2_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can2_tx)); + + /* USER CODE END RTOS_SEMAPHORES */ + + /* USER CODE BEGIN RTOS_TIMERS */ + + /* USER CODE END RTOS_TIMERS */ + + /* USER CODE BEGIN RTOS_QUEUES */ + // This Task must run before MX_USB_DEVICE_Init(), so have to put it here. + const osThreadAttr_t usbIrqTask_attributes = { + .name = "usbIrqTask", + .stack_size = 500, + .priority = (osPriority_t) osPriorityAboveNormal, + }; + usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes); + + /* USER CODE END RTOS_QUEUES */ + + /* Create the thread(s) */ + /* creation of defaultTask */ + defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); + + /* USER CODE BEGIN RTOS_THREADS */ + /* add threads, ... */ + /* USER CODE END RTOS_THREADS */ + + /* USER CODE BEGIN RTOS_EVENTS */ + /* add events, ... */ + /* USER CODE END RTOS_EVENTS */ + +} + +/* USER CODE BEGIN Header_StartDefaultTask */ +/** + * @brief Function implementing the defaultTask thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_StartDefaultTask */ +void StartDefaultTask(void *argument) +{ + /* init code for USB_DEVICE */ + MX_USB_DEVICE_Init(); + /* USER CODE BEGIN StartDefaultTask */ + + // Invoke cpp-version main(). + Main(); + + vTaskDelete(defaultTaskHandle); + /* USER CODE END StartDefaultTask */ +} + +/* Private application code --------------------------------------------------*/ +/* USER CODE BEGIN Application */ + +/* USER CODE END Application */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_it.c b/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_it.c index cca8c02..c56b8f6 100644 --- a/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_it.c +++ b/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_it.c @@ -1,549 +1,558 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * @file stm32f4xx_it.c - * @brief Interrupt Service Routines. - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2021 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under BSD 3-Clause license, - * the "License"; You may not use this file except in compliance with the - * License. You may obtain a copy of the License at: - * opensource.org/licenses/BSD-3-Clause - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Includes ------------------------------------------------------------------*/ -#include "main.h" -#include "stm32f4xx_it.h" -/* Private includes ----------------------------------------------------------*/ -/* USER CODE BEGIN Includes */ -/* USER CODE END Includes */ - -/* Private typedef -----------------------------------------------------------*/ -/* USER CODE BEGIN TD */ - -/* USER CODE END TD */ - -/* Private define ------------------------------------------------------------*/ -/* USER CODE BEGIN PD */ - -/* USER CODE END PD */ - -/* Private macro -------------------------------------------------------------*/ -/* USER CODE BEGIN PM */ - -/* USER CODE END PM */ - -/* Private variables ---------------------------------------------------------*/ -/* USER CODE BEGIN PV */ - -/* USER CODE END PV */ - -/* Private function prototypes -----------------------------------------------*/ -/* USER CODE BEGIN PFP */ - -/* USER CODE END PFP */ - -/* Private user code ---------------------------------------------------------*/ -/* USER CODE BEGIN 0 */ - -/* USER CODE END 0 */ - -/* External variables --------------------------------------------------------*/ -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern DMA_HandleTypeDef hdma_adc1; -extern ADC_HandleTypeDef hadc1; -extern CAN_HandleTypeDef hcan1; -extern CAN_HandleTypeDef hcan2; -extern SPI_HandleTypeDef hspi3; -extern TIM_HandleTypeDef htim2; -extern TIM_HandleTypeDef htim3; -extern TIM_HandleTypeDef htim7; -extern TIM_HandleTypeDef htim10; -extern TIM_HandleTypeDef htim11; -extern TIM_HandleTypeDef htim13; -extern TIM_HandleTypeDef htim14; -extern DMA_HandleTypeDef hdma_uart4_rx; -extern DMA_HandleTypeDef hdma_uart4_tx; -extern DMA_HandleTypeDef hdma_uart5_rx; -extern DMA_HandleTypeDef hdma_uart5_tx; -extern UART_HandleTypeDef huart4; -extern UART_HandleTypeDef huart5; -extern TIM_HandleTypeDef htim6; - -/* USER CODE BEGIN EV */ - -/* USER CODE END EV */ - -/******************************************************************************/ -/* Cortex-M4 Processor Interruption and Exception Handlers */ -/******************************************************************************/ -/** - * @brief This function handles Non maskable interrupt. - */ -void NMI_Handler(void) -{ - /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ - - /* USER CODE END NonMaskableInt_IRQn 0 */ - /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - while (1) - { - } - /* USER CODE END NonMaskableInt_IRQn 1 */ -} - -/** - * @brief This function handles Hard fault interrupt. - */ -void HardFault_Handler(void) -{ - /* USER CODE BEGIN HardFault_IRQn 0 */ - - /* USER CODE END HardFault_IRQn 0 */ - while (1) - { - /* USER CODE BEGIN W1_HardFault_IRQn 0 */ - /* USER CODE END W1_HardFault_IRQn 0 */ - } -} - -/** - * @brief This function handles Memory management fault. - */ -void MemManage_Handler(void) -{ - /* USER CODE BEGIN MemoryManagement_IRQn 0 */ - - /* USER CODE END MemoryManagement_IRQn 0 */ - while (1) - { - /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */ - /* USER CODE END W1_MemoryManagement_IRQn 0 */ - } -} - -/** - * @brief This function handles Pre-fetch fault, memory access fault. - */ -void BusFault_Handler(void) -{ - /* USER CODE BEGIN BusFault_IRQn 0 */ - - /* USER CODE END BusFault_IRQn 0 */ - while (1) - { - /* USER CODE BEGIN W1_BusFault_IRQn 0 */ - /* USER CODE END W1_BusFault_IRQn 0 */ - } -} - -/** - * @brief This function handles Undefined instruction or illegal state. - */ -void UsageFault_Handler(void) -{ - /* USER CODE BEGIN UsageFault_IRQn 0 */ - - /* USER CODE END UsageFault_IRQn 0 */ - while (1) - { - /* USER CODE BEGIN W1_UsageFault_IRQn 0 */ - /* USER CODE END W1_UsageFault_IRQn 0 */ - } -} - -/** - * @brief This function handles Debug monitor. - */ -void DebugMon_Handler(void) -{ - /* USER CODE BEGIN DebugMonitor_IRQn 0 */ - - /* USER CODE END DebugMonitor_IRQn 0 */ - /* USER CODE BEGIN DebugMonitor_IRQn 1 */ - - /* USER CODE END DebugMonitor_IRQn 1 */ -} - -/******************************************************************************/ -/* STM32F4xx Peripheral Interrupt Handlers */ -/* Add here the Interrupt Handlers for the used peripherals. */ -/* For the available peripheral interrupt handler names, */ -/* please refer to the startup file (startup_stm32f4xx.s). */ -/******************************************************************************/ - -/** - * @brief This function handles DMA1 stream0 global interrupt. - */ -void DMA1_Stream0_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream0_IRQn 0 */ - - /* USER CODE END DMA1_Stream0_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_uart5_rx); - /* USER CODE BEGIN DMA1_Stream0_IRQn 1 */ - - /* USER CODE END DMA1_Stream0_IRQn 1 */ -} - -/** - * @brief This function handles DMA1 stream2 global interrupt. - */ -void DMA1_Stream2_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */ - - /* USER CODE END DMA1_Stream2_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_uart4_rx); - /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */ - - /* USER CODE END DMA1_Stream2_IRQn 1 */ -} - -/** - * @brief This function handles DMA1 stream4 global interrupt. - */ -void DMA1_Stream4_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream4_IRQn 0 */ - - /* USER CODE END DMA1_Stream4_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_uart4_tx); - /* USER CODE BEGIN DMA1_Stream4_IRQn 1 */ - - /* USER CODE END DMA1_Stream4_IRQn 1 */ -} - -/** - * @brief This function handles ADC1, ADC2 and ADC3 global interrupts. - */ -void ADC_IRQHandler(void) -{ - /* USER CODE BEGIN ADC_IRQn 0 */ - - /* USER CODE END ADC_IRQn 0 */ - HAL_ADC_IRQHandler(&hadc1); - /* USER CODE BEGIN ADC_IRQn 1 */ - - /* USER CODE END ADC_IRQn 1 */ -} - -/** - * @brief This function handles CAN1 TX interrupts. - */ -void CAN1_TX_IRQHandler(void) -{ - /* USER CODE BEGIN CAN1_TX_IRQn 0 */ - - /* USER CODE END CAN1_TX_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan1); - /* USER CODE BEGIN CAN1_TX_IRQn 1 */ - - /* USER CODE END CAN1_TX_IRQn 1 */ -} - -/** - * @brief This function handles CAN1 RX0 interrupts. - */ -void CAN1_RX0_IRQHandler(void) -{ - /* USER CODE BEGIN CAN1_RX0_IRQn 0 */ - - /* USER CODE END CAN1_RX0_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan1); - /* USER CODE BEGIN CAN1_RX0_IRQn 1 */ - - /* USER CODE END CAN1_RX0_IRQn 1 */ -} - -/** - * @brief This function handles CAN1 RX1 interrupt. - */ -void CAN1_RX1_IRQHandler(void) -{ - /* USER CODE BEGIN CAN1_RX1_IRQn 0 */ - - /* USER CODE END CAN1_RX1_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan1); - /* USER CODE BEGIN CAN1_RX1_IRQn 1 */ - - /* USER CODE END CAN1_RX1_IRQn 1 */ -} - -/** - * @brief This function handles CAN1 SCE interrupt. - */ -void CAN1_SCE_IRQHandler(void) -{ - /* USER CODE BEGIN CAN1_SCE_IRQn 0 */ - - /* USER CODE END CAN1_SCE_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan1); - /* USER CODE BEGIN CAN1_SCE_IRQn 1 */ - - /* USER CODE END CAN1_SCE_IRQn 1 */ -} - -/** - * @brief This function handles TIM1 update interrupt and TIM10 global interrupt. - */ -void TIM1_UP_TIM10_IRQHandler(void) -{ - /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 0 */ - - /* USER CODE END TIM1_UP_TIM10_IRQn 0 */ - HAL_TIM_IRQHandler(&htim10); - /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 1 */ - - /* USER CODE END TIM1_UP_TIM10_IRQn 1 */ -} - -/** - * @brief This function handles TIM1 trigger and commutation interrupts and TIM11 global interrupt. - */ -void TIM1_TRG_COM_TIM11_IRQHandler(void) -{ - /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 0 */ - - /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 0 */ - HAL_TIM_IRQHandler(&htim11); - /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 1 */ - - /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 1 */ -} - -/** - * @brief This function handles TIM2 global interrupt. - */ -void TIM2_IRQHandler(void) -{ - /* USER CODE BEGIN TIM2_IRQn 0 */ - - /* USER CODE END TIM2_IRQn 0 */ - HAL_TIM_IRQHandler(&htim2); - /* USER CODE BEGIN TIM2_IRQn 1 */ - - /* USER CODE END TIM2_IRQn 1 */ -} - -/** - * @brief This function handles TIM3 global interrupt. - */ -void TIM3_IRQHandler(void) -{ - /* USER CODE BEGIN TIM3_IRQn 0 */ - - /* USER CODE END TIM3_IRQn 0 */ - HAL_TIM_IRQHandler(&htim3); - /* USER CODE BEGIN TIM3_IRQn 1 */ - - /* USER CODE END TIM3_IRQn 1 */ -} - -/** - * @brief This function handles TIM8 update interrupt and TIM13 global interrupt. - */ -void TIM8_UP_TIM13_IRQHandler(void) -{ - /* USER CODE BEGIN TIM8_UP_TIM13_IRQn 0 */ - - /* USER CODE END TIM8_UP_TIM13_IRQn 0 */ - HAL_TIM_IRQHandler(&htim13); - /* USER CODE BEGIN TIM8_UP_TIM13_IRQn 1 */ - - /* USER CODE END TIM8_UP_TIM13_IRQn 1 */ -} - -/** - * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt. - */ -void TIM8_TRG_COM_TIM14_IRQHandler(void) -{ - /* USER CODE BEGIN TIM8_TRG_COM_TIM14_IRQn 0 */ - - /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 0 */ - HAL_TIM_IRQHandler(&htim14); - /* USER CODE BEGIN TIM8_TRG_COM_TIM14_IRQn 1 */ - - /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 1 */ -} - -/** - * @brief This function handles DMA1 stream7 global interrupt. - */ -void DMA1_Stream7_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream7_IRQn 0 */ - - /* USER CODE END DMA1_Stream7_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_uart5_tx); - /* USER CODE BEGIN DMA1_Stream7_IRQn 1 */ - - /* USER CODE END DMA1_Stream7_IRQn 1 */ -} - -/** - * @brief This function handles SPI3 global interrupt. - */ -void SPI3_IRQHandler(void) -{ - /* USER CODE BEGIN SPI3_IRQn 0 */ - - /* USER CODE END SPI3_IRQn 0 */ - HAL_SPI_IRQHandler(&hspi3); - /* USER CODE BEGIN SPI3_IRQn 1 */ - - /* USER CODE END SPI3_IRQn 1 */ -} - -/** - * @brief This function handles UART4 global interrupt. - */ -void UART4_IRQHandler(void) -{ - /* USER CODE BEGIN UART4_IRQn 0 */ - - /* USER CODE END UART4_IRQn 0 */ - HAL_UART_IRQHandler(&huart4); - /* USER CODE BEGIN UART4_IRQn 1 */ - - /* USER CODE END UART4_IRQn 1 */ -} - -/** - * @brief This function handles UART5 global interrupt. - */ -void UART5_IRQHandler(void) -{ - /* USER CODE BEGIN UART5_IRQn 0 */ - - /* USER CODE END UART5_IRQn 0 */ - HAL_UART_IRQHandler(&huart5); - /* USER CODE BEGIN UART5_IRQn 1 */ - - /* USER CODE END UART5_IRQn 1 */ -} - -/** - * @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts. - */ -void TIM6_DAC_IRQHandler(void) -{ - /* USER CODE BEGIN TIM6_DAC_IRQn 0 */ - - /* USER CODE END TIM6_DAC_IRQn 0 */ - HAL_TIM_IRQHandler(&htim6); - /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ - - /* USER CODE END TIM6_DAC_IRQn 1 */ -} - -/** - * @brief This function handles TIM7 global interrupt. - */ -void TIM7_IRQHandler(void) -{ - /* USER CODE BEGIN TIM7_IRQn 0 */ - - /* USER CODE END TIM7_IRQn 0 */ - HAL_TIM_IRQHandler(&htim7); - /* USER CODE BEGIN TIM7_IRQn 1 */ - - /* USER CODE END TIM7_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream0 global interrupt. - */ -void DMA2_Stream0_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream0_IRQn 0 */ - - /* USER CODE END DMA2_Stream0_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_adc1); - /* USER CODE BEGIN DMA2_Stream0_IRQn 1 */ - - /* USER CODE END DMA2_Stream0_IRQn 1 */ -} - -/** - * @brief This function handles CAN2 TX interrupts. - */ -void CAN2_TX_IRQHandler(void) -{ - /* USER CODE BEGIN CAN2_TX_IRQn 0 */ - - /* USER CODE END CAN2_TX_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan2); - /* USER CODE BEGIN CAN2_TX_IRQn 1 */ - - /* USER CODE END CAN2_TX_IRQn 1 */ -} - -/** - * @brief This function handles CAN2 RX0 interrupts. - */ -void CAN2_RX0_IRQHandler(void) -{ - /* USER CODE BEGIN CAN2_RX0_IRQn 0 */ - - /* USER CODE END CAN2_RX0_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan2); - /* USER CODE BEGIN CAN2_RX0_IRQn 1 */ - - /* USER CODE END CAN2_RX0_IRQn 1 */ -} - -/** - * @brief This function handles CAN2 RX1 interrupt. - */ -void CAN2_RX1_IRQHandler(void) -{ - /* USER CODE BEGIN CAN2_RX1_IRQn 0 */ - - /* USER CODE END CAN2_RX1_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan2); - /* USER CODE BEGIN CAN2_RX1_IRQn 1 */ - - /* USER CODE END CAN2_RX1_IRQn 1 */ -} - -/** - * @brief This function handles CAN2 SCE interrupt. - */ -void CAN2_SCE_IRQHandler(void) -{ - /* USER CODE BEGIN CAN2_SCE_IRQn 0 */ - - /* USER CODE END CAN2_SCE_IRQn 0 */ - HAL_CAN_IRQHandler(&hcan2); - /* USER CODE BEGIN CAN2_SCE_IRQn 1 */ - - /* USER CODE END CAN2_SCE_IRQn 1 */ -} - -/** - * @brief This function handles USB On The Go FS global interrupt. - */ -void OTG_FS_IRQHandler(void) -{ - /* USER CODE BEGIN OTG_FS_IRQn 0 */ - - /* USER CODE END OTG_FS_IRQn 0 */ - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - /* USER CODE BEGIN OTG_FS_IRQn 1 */ - - /* USER CODE END OTG_FS_IRQn 1 */ -} - -/* USER CODE BEGIN 1 */ - -/* USER CODE END 1 */ -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f4xx_it.c + * @brief Interrupt Service Routines. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2021 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +#include "stm32f4xx_it.h" +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN TD */ + +/* USER CODE END TD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/* External variables --------------------------------------------------------*/ +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; +extern DMA_HandleTypeDef hdma_adc1; +extern ADC_HandleTypeDef hadc1; +extern CAN_HandleTypeDef hcan1; +extern CAN_HandleTypeDef hcan2; +extern SPI_HandleTypeDef hspi3; +extern TIM_HandleTypeDef htim2; +extern TIM_HandleTypeDef htim3; +extern TIM_HandleTypeDef htim7; +extern TIM_HandleTypeDef htim10; +extern TIM_HandleTypeDef htim11; +extern TIM_HandleTypeDef htim13; +extern TIM_HandleTypeDef htim14; +extern DMA_HandleTypeDef hdma_uart4_rx; +extern DMA_HandleTypeDef hdma_uart4_tx; +extern DMA_HandleTypeDef hdma_uart5_rx; +extern DMA_HandleTypeDef hdma_uart5_tx; +extern UART_HandleTypeDef huart4; +extern UART_HandleTypeDef huart5; +extern TIM_HandleTypeDef htim6; + +/* USER CODE BEGIN EV */ + +/* USER CODE END EV */ + +/******************************************************************************/ +/* Cortex-M4 Processor Interruption and Exception Handlers */ +/******************************************************************************/ +/** + * @brief This function handles Non maskable interrupt. + */ +void NMI_Handler(void) +{ + /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ + + /* USER CODE END NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ + while (1) + { + } + /* USER CODE END NonMaskableInt_IRQn 1 */ +} + +/** + * @brief This function handles Hard fault interrupt. + */ +void HardFault_Handler(void) +{ + /* USER CODE BEGIN HardFault_IRQn 0 */ + + /* USER CODE END HardFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + /* USER CODE END W1_HardFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Memory management fault. + */ +void MemManage_Handler(void) +{ + /* USER CODE BEGIN MemoryManagement_IRQn 0 */ + + /* USER CODE END MemoryManagement_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */ + /* USER CODE END W1_MemoryManagement_IRQn 0 */ + } +} + +/** + * @brief This function handles Pre-fetch fault, memory access fault. + */ +void BusFault_Handler(void) +{ + /* USER CODE BEGIN BusFault_IRQn 0 */ + + /* USER CODE END BusFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_BusFault_IRQn 0 */ + /* USER CODE END W1_BusFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Undefined instruction or illegal state. + */ +void UsageFault_Handler(void) +{ + /* USER CODE BEGIN UsageFault_IRQn 0 */ + + /* USER CODE END UsageFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_UsageFault_IRQn 0 */ + /* USER CODE END W1_UsageFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Debug monitor. + */ +void DebugMon_Handler(void) +{ + /* USER CODE BEGIN DebugMonitor_IRQn 0 */ + + /* USER CODE END DebugMonitor_IRQn 0 */ + /* USER CODE BEGIN DebugMonitor_IRQn 1 */ + + /* USER CODE END DebugMonitor_IRQn 1 */ +} + +/******************************************************************************/ +/* STM32F4xx Peripheral Interrupt Handlers */ +/* Add here the Interrupt Handlers for the used peripherals. */ +/* For the available peripheral interrupt handler names, */ +/* please refer to the startup file (startup_stm32f4xx.s). */ +/******************************************************************************/ + +/** + * @brief This function handles DMA1 stream0 global interrupt. + */ +void DMA1_Stream0_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream0_IRQn 0 */ + + /* USER CODE END DMA1_Stream0_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_uart5_rx); + /* USER CODE BEGIN DMA1_Stream0_IRQn 1 */ + + /* USER CODE END DMA1_Stream0_IRQn 1 */ +} + +/** + * @brief This function handles DMA1 stream2 global interrupt. + */ +void DMA1_Stream2_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */ + + /* USER CODE END DMA1_Stream2_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_uart4_rx); + /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */ + + /* USER CODE END DMA1_Stream2_IRQn 1 */ +} + +/** + * @brief This function handles DMA1 stream4 global interrupt. + */ +void DMA1_Stream4_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream4_IRQn 0 */ + + /* USER CODE END DMA1_Stream4_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_uart4_tx); + /* USER CODE BEGIN DMA1_Stream4_IRQn 1 */ + + /* USER CODE END DMA1_Stream4_IRQn 1 */ +} + +/** + * @brief This function handles ADC1, ADC2 and ADC3 global interrupts. + */ +void ADC_IRQHandler(void) +{ + /* USER CODE BEGIN ADC_IRQn 0 */ + + /* USER CODE END ADC_IRQn 0 */ + HAL_ADC_IRQHandler(&hadc1); + /* USER CODE BEGIN ADC_IRQn 1 */ + + /* USER CODE END ADC_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 TX interrupts. + */ +void CAN1_TX_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_TX_IRQn 0 */ + + /* USER CODE END CAN1_TX_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_TX_IRQn 1 */ + + /* USER CODE END CAN1_TX_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 RX0 interrupts. + */ +void CAN1_RX0_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_RX0_IRQn 0 */ + if (( READ_REG(hcan1.Instance->IER) & CAN_IT_RX_FIFO0_MSG_PENDING) != 0U) + { + /* Check if message is still pending */ + if ((hcan1.Instance->RF0R & CAN_RF0R_FMP0) != 0U) + { + /* Call weak (surcharged) callback */ + HAL_CAN_RxFifo0MsgPendingCallback(&hcan1); + } + } + return; + /* USER CODE END CAN1_RX0_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_RX0_IRQn 1 */ + + /* USER CODE END CAN1_RX0_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 RX1 interrupt. + */ +void CAN1_RX1_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_RX1_IRQn 0 */ + + /* USER CODE END CAN1_RX1_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_RX1_IRQn 1 */ + + /* USER CODE END CAN1_RX1_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 SCE interrupt. + */ +void CAN1_SCE_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_SCE_IRQn 0 */ + + /* USER CODE END CAN1_SCE_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_SCE_IRQn 1 */ + + /* USER CODE END CAN1_SCE_IRQn 1 */ +} + +/** + * @brief This function handles TIM1 update interrupt and TIM10 global interrupt. + */ +void TIM1_UP_TIM10_IRQHandler(void) +{ + /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 0 */ + + /* USER CODE END TIM1_UP_TIM10_IRQn 0 */ + HAL_TIM_IRQHandler(&htim10); + /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 1 */ + + /* USER CODE END TIM1_UP_TIM10_IRQn 1 */ +} + +/** + * @brief This function handles TIM1 trigger and commutation interrupts and TIM11 global interrupt. + */ +void TIM1_TRG_COM_TIM11_IRQHandler(void) +{ + /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 0 */ + + /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 0 */ + HAL_TIM_IRQHandler(&htim11); + /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 1 */ + + /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 1 */ +} + +/** + * @brief This function handles TIM2 global interrupt. + */ +void TIM2_IRQHandler(void) +{ + /* USER CODE BEGIN TIM2_IRQn 0 */ + + /* USER CODE END TIM2_IRQn 0 */ + HAL_TIM_IRQHandler(&htim2); + /* USER CODE BEGIN TIM2_IRQn 1 */ + + /* USER CODE END TIM2_IRQn 1 */ +} + +/** + * @brief This function handles TIM3 global interrupt. + */ +void TIM3_IRQHandler(void) +{ + /* USER CODE BEGIN TIM3_IRQn 0 */ + + /* USER CODE END TIM3_IRQn 0 */ + HAL_TIM_IRQHandler(&htim3); + /* USER CODE BEGIN TIM3_IRQn 1 */ + + /* USER CODE END TIM3_IRQn 1 */ +} + +/** + * @brief This function handles TIM8 update interrupt and TIM13 global interrupt. + */ +void TIM8_UP_TIM13_IRQHandler(void) +{ + /* USER CODE BEGIN TIM8_UP_TIM13_IRQn 0 */ + + /* USER CODE END TIM8_UP_TIM13_IRQn 0 */ + HAL_TIM_IRQHandler(&htim13); + /* USER CODE BEGIN TIM8_UP_TIM13_IRQn 1 */ + + /* USER CODE END TIM8_UP_TIM13_IRQn 1 */ +} + +/** + * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt. + */ +void TIM8_TRG_COM_TIM14_IRQHandler(void) +{ + /* USER CODE BEGIN TIM8_TRG_COM_TIM14_IRQn 0 */ + + /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 0 */ + HAL_TIM_IRQHandler(&htim14); + /* USER CODE BEGIN TIM8_TRG_COM_TIM14_IRQn 1 */ + + /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 1 */ +} + +/** + * @brief This function handles DMA1 stream7 global interrupt. + */ +void DMA1_Stream7_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream7_IRQn 0 */ + + /* USER CODE END DMA1_Stream7_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_uart5_tx); + /* USER CODE BEGIN DMA1_Stream7_IRQn 1 */ + + /* USER CODE END DMA1_Stream7_IRQn 1 */ +} + +/** + * @brief This function handles SPI3 global interrupt. + */ +void SPI3_IRQHandler(void) +{ + /* USER CODE BEGIN SPI3_IRQn 0 */ + + /* USER CODE END SPI3_IRQn 0 */ + HAL_SPI_IRQHandler(&hspi3); + /* USER CODE BEGIN SPI3_IRQn 1 */ + + /* USER CODE END SPI3_IRQn 1 */ +} + +/** + * @brief This function handles UART4 global interrupt. + */ +void UART4_IRQHandler(void) +{ + /* USER CODE BEGIN UART4_IRQn 0 */ + + /* USER CODE END UART4_IRQn 0 */ + HAL_UART_IRQHandler(&huart4); + /* USER CODE BEGIN UART4_IRQn 1 */ + + /* USER CODE END UART4_IRQn 1 */ +} + +/** + * @brief This function handles UART5 global interrupt. + */ +void UART5_IRQHandler(void) +{ + /* USER CODE BEGIN UART5_IRQn 0 */ + + /* USER CODE END UART5_IRQn 0 */ + HAL_UART_IRQHandler(&huart5); + /* USER CODE BEGIN UART5_IRQn 1 */ + + /* USER CODE END UART5_IRQn 1 */ +} + +/** + * @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts. + */ +void TIM6_DAC_IRQHandler(void) +{ + /* USER CODE BEGIN TIM6_DAC_IRQn 0 */ + + /* USER CODE END TIM6_DAC_IRQn 0 */ + HAL_TIM_IRQHandler(&htim6); + /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ + + /* USER CODE END TIM6_DAC_IRQn 1 */ +} + +/** + * @brief This function handles TIM7 global interrupt. + */ +void TIM7_IRQHandler(void) +{ + /* USER CODE BEGIN TIM7_IRQn 0 */ + + /* USER CODE END TIM7_IRQn 0 */ + HAL_TIM_IRQHandler(&htim7); + /* USER CODE BEGIN TIM7_IRQn 1 */ + + /* USER CODE END TIM7_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream0 global interrupt. + */ +void DMA2_Stream0_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream0_IRQn 0 */ + + /* USER CODE END DMA2_Stream0_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_adc1); + /* USER CODE BEGIN DMA2_Stream0_IRQn 1 */ + + /* USER CODE END DMA2_Stream0_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 TX interrupts. + */ +void CAN2_TX_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_TX_IRQn 0 */ + + /* USER CODE END CAN2_TX_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_TX_IRQn 1 */ + + /* USER CODE END CAN2_TX_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 RX0 interrupts. + */ +void CAN2_RX0_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_RX0_IRQn 0 */ + + /* USER CODE END CAN2_RX0_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_RX0_IRQn 1 */ + + /* USER CODE END CAN2_RX0_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 RX1 interrupt. + */ +void CAN2_RX1_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_RX1_IRQn 0 */ + + /* USER CODE END CAN2_RX1_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_RX1_IRQn 1 */ + + /* USER CODE END CAN2_RX1_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 SCE interrupt. + */ +void CAN2_SCE_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_SCE_IRQn 0 */ + + /* USER CODE END CAN2_SCE_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_SCE_IRQn 1 */ + + /* USER CODE END CAN2_SCE_IRQn 1 */ +} + +/** + * @brief This function handles USB On The Go FS global interrupt. + */ +void OTG_FS_IRQHandler(void) +{ + /* USER CODE BEGIN OTG_FS_IRQn 0 */ + + /* USER CODE END OTG_FS_IRQn 0 */ + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + /* USER CODE BEGIN OTG_FS_IRQn 1 */ + + /* USER CODE END OTG_FS_IRQn 1 */ +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.cpp b/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.cpp index eff1b26..cdab077 100644 --- a/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.cpp +++ b/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.cpp @@ -1,348 +1,317 @@ -#include "ctrl_step.hpp" -#include "communication.hpp" - - -CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse, - uint8_t _reduction, float _minAngle, float _maxAngle) : - nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction), - angleLimitMin(_minAngle), angleLimitMax(_maxAngle) -{ - txHeader = - { - .StdId = 0, - .ExtId = 0, - .IDE = CAN_ID_STD, - .RTR = CAN_RTR_DATA, - .DLC = 8, - .TransmitGlobalTime = DISABLE - }; -} - - -void CtrlStepMotor::SetEnable(bool _enable) -{ - state = _enable ? FINISH : STOP; - - uint8_t mode = 0x01; - txHeader.StdId = nodeID << 7 | mode; - - // Int to Bytes - uint32_t val = _enable ? 1 : 0; - auto* b = (unsigned char*) &val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::DoCalibration() -{ - uint8_t mode = 0x02; - txHeader.StdId = nodeID << 7 | mode; - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetCurrentSetPoint(float _val) -{ - state = RUNNING; - - uint8_t mode = 0x03; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetVelocitySetPoint(float _val) -{ - state = RUNNING; - - uint8_t mode = 0x04; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetPositionSetPoint(float _val) -{ - state = RUNNING; - - uint8_t mode = 0x05; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetPositionWithTime(float _pos, float _time) -{ - state = RUNNING; - - uint8_t mode = 0x06; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_pos; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - b = (unsigned char*) &_time; - for (int i = 4; i < 8; i++) - canBuf[i] = *(b + i - 4); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::AddTrajectoryPoint(float _pos, float _vel) -{ - uint8_t mode = 0x07; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_pos; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - b = (unsigned char*) &_vel; - for (int i = 4; i < 8; i++) - canBuf[i] = *(b + i - 4); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetNodeID(uint32_t _id) -{ - uint8_t mode = 0x11; - txHeader.StdId = nodeID << 7 | mode; - - // Int to Bytes - auto* b = (unsigned char*) &_id; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetCurrentLimit(float _val) -{ - uint8_t mode = 0x12; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetVelocityLimit(float _val) -{ - uint8_t mode = 0x13; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetAcceleration(float _val, bool _storeToMem) -{ - uint8_t mode = 0x14; - txHeader.StdId = nodeID << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - canBuf[4] = _storeToMem ? 1 : 0; - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::ApplyPositionAsHome() -{ - uint8_t mode = 0x15; - txHeader.StdId = nodeID << 7 | mode; - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetEnableOnBoot(bool _enable) -{ - uint8_t mode = 0x16; - txHeader.StdId = nodeID << 7 | mode; - - // Int to Bytes - uint32_t val = _enable ? 1 : 0; - auto* b = (unsigned char*) &val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetEnableAck(bool _enable) -{ - uint8_t mode = 0x1B; - txHeader.StdId = nodeID << 7 | mode; - - uint32_t val = _enable ? 1 : 0; - auto* b = (unsigned char*) &val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetEnableStallProtect(bool _enable) -{ - uint8_t mode = 0x1C; - txHeader.StdId = nodeID << 7 | mode; - - uint32_t val = _enable ? 1 : 0; - auto* b = (unsigned char*) &val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::Reboot() -{ - uint8_t mode = 0x7f; - txHeader.StdId = nodeID << 7 | mode; - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::EraseConfigs() -{ - uint8_t mode = 0x7e; - txHeader.StdId = nodeID << 7 | mode; - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetAngle(float _angle) -{ - _angle = inverseDirection ? -_angle : _angle; - if (_angle <= angleLimitMax && _angle >= angleLimitMin) - { - float stepMotorCnt = _angle / 360.0f * reduction * CTRL_CIRCLE_COUNT; - SetPositionSetPoint(stepMotorCnt); - } -} - - -void CtrlStepMotor::SetAngleWithTime(float _angle, float _time) -{ - _angle = inverseDirection ? -_angle : _angle; - float stepMotorCnt = _angle / 360.0f * reduction; - SetPositionWithTime(stepMotorCnt, _time); -} - - -void CtrlStepMotor::UpdateAngle() -{ - uint8_t mode = 0x23; - txHeader.StdId = nodeID << 7 | mode; - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isAck) -{ - if (_isAck) - state = FINISH; - - float tmp = _pos / (float) reduction * 360; - angle = inverseDirection ? -tmp : tmp; -} - - -void CtrlStepMotor::SetDceKp(int32_t _val) -{ - uint8_t mode = 0x17; - txHeader.StdId = nodeID << 7 | mode; - - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetDceKv(int32_t _val) -{ - uint8_t mode = 0x18; - txHeader.StdId = nodeID << 7 | mode; - - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetDceKi(int32_t _val) -{ - uint8_t mode = 0x19; - txHeader.StdId = nodeID << 7 | mode; - - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void CtrlStepMotor::SetDceKd(int32_t _val) -{ - uint8_t mode = 0x1A; - txHeader.StdId = nodeID << 7 | mode; - - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} +#include "ctrl_step.hpp" +#include "communication.hpp" + + +CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse, + uint8_t _reduction, float _angleLimitMin, float _angleLimitMax) : + nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction), + angleLimitMin(_angleLimitMin), angleLimitMax(_angleLimitMax) +{ + txHeader = + { + .StdId = 0, + .ExtId = 0, + .IDE = CAN_ID_STD, + .RTR = CAN_RTR_DATA, + .DLC = 8, + .TransmitGlobalTime = DISABLE + }; +} + + +void CtrlStepMotor::SetEnable(bool _enable) +{ + state = _enable ? FINISH : STOP; + + uint8_t mode = 0x01; + txHeader.StdId = nodeID << 7 | mode; + + // Int to Bytes + uint32_t val = _enable ? 1 : 0; + auto* b = (unsigned char*) &val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::DoCalibration() +{ + uint8_t mode = 0x02; + txHeader.StdId = nodeID << 7 | mode; + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetCurrentSetPoint(float _val) +{ + state = RUNNING; + + uint8_t mode = 0x03; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetVelocitySetPoint(float _val) +{ + state = RUNNING; + + uint8_t mode = 0x04; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetPositionSetPoint(float _val) +{ + uint8_t mode = 0x05; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need ACK + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetPositionWithVelocityLimit(float _pos, float _vel) +{ + uint8_t mode = 0x07; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_pos; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + b = (unsigned char*) &_vel; + for (int i = 4; i < 8; i++) + canBuf[i] = *(b + i - 4); + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetNodeID(uint32_t _id) +{ + uint8_t mode = 0x11; + txHeader.StdId = nodeID << 7 | mode; + + // Int to Bytes + auto* b = (unsigned char*) &_id; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetCurrentLimit(float _val) +{ + uint8_t mode = 0x12; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetVelocityLimit(float _val) +{ + uint8_t mode = 0x13; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetAcceleration(float _val) +{ + uint8_t mode = 0x14; + txHeader.StdId = nodeID << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 0; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::ApplyPositionAsHome() +{ + uint8_t mode = 0x15; + txHeader.StdId = nodeID << 7 | mode; + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetEnableOnBoot(bool _enable) +{ + uint8_t mode = 0x16; + txHeader.StdId = nodeID << 7 | mode; + + // Int to Bytes + uint32_t val = _enable ? 1 : 0; + auto* b = (unsigned char*) &val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetEnableStallProtect(bool _enable) +{ + uint8_t mode = 0x1B; + txHeader.StdId = nodeID << 7 | mode; + + uint32_t val = _enable ? 1 : 0; + auto* b = (unsigned char*) &val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::Reboot() +{ + uint8_t mode = 0x7f; + txHeader.StdId = nodeID << 7 | mode; + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::EraseConfigs() +{ + uint8_t mode = 0x7e; + txHeader.StdId = nodeID << 7 | mode; + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetAngle(float _angle) +{ + _angle = inverseDirection ? -_angle : _angle; + float stepMotorCnt = _angle / 360.0f * (float) reduction; + SetPositionSetPoint(stepMotorCnt); +} + + +void CtrlStepMotor::SetAngleWithVelocityLimit(float _angle, float _vel) +{ + _angle = inverseDirection ? -_angle : _angle; + float stepMotorCnt = _angle / 360.0f * (float) reduction; + SetPositionWithVelocityLimit(stepMotorCnt, _vel); +} + + +void CtrlStepMotor::UpdateAngle() +{ + uint8_t mode = 0x23; + txHeader.StdId = nodeID << 7 | mode; + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isFinished) +{ + state = _isFinished ? FINISH : RUNNING; + + float tmp = _pos / (float) reduction * 360; + angle = inverseDirection ? -tmp : tmp; +} + + +void CtrlStepMotor::SetDceKp(int32_t _val) +{ + uint8_t mode = 0x17; + txHeader.StdId = nodeID << 7 | mode; + + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetDceKv(int32_t _val) +{ + uint8_t mode = 0x18; + txHeader.StdId = nodeID << 7 | mode; + + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetDceKi(int32_t _val) +{ + uint8_t mode = 0x19; + txHeader.StdId = nodeID << 7 | mode; + + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void CtrlStepMotor::SetDceKd(int32_t _val) +{ + uint8_t mode = 0x1A; + txHeader.StdId = nodeID << 7 | mode; + + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + canBuf[4] = 1; // Need save to EEPROM or not + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} diff --git a/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.hpp b/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.hpp index 12c3f81..e2c76fe 100644 --- a/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.hpp +++ b/2.Firmware/Core-STM32F4-fw/Robot/actuators/ctrl_step/ctrl_step.hpp @@ -1,99 +1,95 @@ -#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP -#define DUMMY_CORE_FW_CTRL_STEP_HPP - -#include "fibre/protocol.hpp" -#include "can.h" - -class CtrlStepMotor -{ -public: - enum State - { - RUNNING, - FINISH, - STOP - }; - - - const uint32_t CTRL_CIRCLE_COUNT = 200 * 256; - - CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1, - float _minAngle = -180, float _maxAngle = 180); - - uint8_t nodeID; - float angle = 0; - float angleLimitMax; - float angleLimitMin; - bool inverseDirection; - uint8_t reduction; - State state = STOP; - - void SetAngle(float _angle); - void SetAngleWithTime(float _angle, float _time); - // CAN Command - void SetEnable(bool _enable); - void DoCalibration(); - void SetCurrentSetPoint(float _val); - void SetVelocitySetPoint(float _val); - void SetPositionSetPoint(float _val); - void SetPositionWithTime(float _pos, float _time); - void AddTrajectoryPoint(float _pos, float _vel); - void SetNodeID(uint32_t _id); - void SetCurrentLimit(float _val); - void SetVelocityLimit(float _val); - void SetAcceleration(float _val, bool _storeToMem = false); - void SetDceKp(int32_t _val); - void SetDceKv(int32_t _val); - void SetDceKi(int32_t _val); - void SetDceKd(int32_t _val); - void ApplyPositionAsHome(); - void SetEnableOnBoot(bool _enable); - void SetEnableAck(bool _enable); - void SetEnableStallProtect(bool _enable); - void Reboot(); - void EraseConfigs(); - - void UpdateAngle(); - void UpdateAngleCallback(float _pos, bool _isAck); - - - // Communication protocol definitions - auto MakeProtocolDefinitions() - { - return make_protocol_member_list( - make_protocol_ro_property("angle", &angle), - make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot), - make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs), - make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"), - make_protocol_function("set_position_with_time", *this, &CtrlStepMotor::SetPositionWithTime, - "pos", "time"), - make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"), - make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"), - make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"), - make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"), - make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"), - make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"), - make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc", - "stored"), - make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome), - make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration), - make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"), - make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"), - make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"), - make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"), - make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"), - make_protocol_function("set_enable_ack", *this, &CtrlStepMotor::SetEnableAck, "enable"), - make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect, - "enable"), - make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle) - ); - } - - -private: - CAN_HandleTypeDef* hcan; - uint8_t canBuf[8] = {}; - CAN_TxHeaderTypeDef txHeader = {}; -}; - -#endif //DUMMY_CORE_FW_CTRL_STEP_HPP +#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP +#define DUMMY_CORE_FW_CTRL_STEP_HPP + +#include "fibre/protocol.hpp" +#include "can.h" + +class CtrlStepMotor +{ +public: + enum State + { + RUNNING, + FINISH, + STOP + }; + + + const uint32_t CTRL_CIRCLE_COUNT = 200 * 256; + + CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1, + float _angleLimitMin = -180, float _angleLimitMax = 180); + + uint8_t nodeID; + float angle = 0; + float angleLimitMax; + float angleLimitMin; + bool inverseDirection; + uint8_t reduction; + State state = STOP; + + void SetAngle(float _angle); + void SetAngleWithVelocityLimit(float _angle, float _vel); + // CAN Command + void SetEnable(bool _enable); + void DoCalibration(); + void SetCurrentSetPoint(float _val); + void SetVelocitySetPoint(float _val); + void SetPositionSetPoint(float _val); + void SetPositionWithVelocityLimit(float _pos, float _vel); + void SetNodeID(uint32_t _id); + void SetCurrentLimit(float _val); + void SetVelocityLimit(float _val); + void SetAcceleration(float _val); + void SetDceKp(int32_t _val); + void SetDceKv(int32_t _val); + void SetDceKi(int32_t _val); + void SetDceKd(int32_t _val); + void ApplyPositionAsHome(); + void SetEnableOnBoot(bool _enable); + void SetEnableStallProtect(bool _enable); + void Reboot(); + void EraseConfigs(); + + void UpdateAngle(); + void UpdateAngleCallback(float _pos, bool _isFinished); + + + // Communication protocol definitions + auto MakeProtocolDefinitions() + { + return make_protocol_member_list( + make_protocol_ro_property("angle", &angle), + make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot), + make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs), + make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"), + make_protocol_function("set_position_with_time", *this, + &CtrlStepMotor::SetPositionWithVelocityLimit, "pos", "time"), + make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"), + make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"), + make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"), + make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"), + make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"), + make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"), + make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc"), + make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome), + make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration), + make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"), + make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"), + make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"), + make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"), + make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"), + make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect, + "enable"), + make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle) + ); + } + + +private: + CAN_HandleTypeDef* hcan; + uint8_t canBuf[8] = {}; + CAN_TxHeaderTypeDef txHeader = {}; +}; + +#endif //DUMMY_CORE_FW_CTRL_STEP_HPP diff --git a/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.cpp b/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.cpp index f7f816c..711affa 100644 --- a/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.cpp +++ b/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.cpp @@ -1,509 +1,509 @@ -#include "6dof_kinematic.h" - -inline float cosf(float x) -{ - return arm_cos_f32(x); -} - -inline float sinf(float x) -{ - return arm_sin_f32(x); -} - -static void MatMultiply(const float* _matrix1, const float* _matrix2, float* _matrixOut, - const int _m, const int _l, const int _n) -{ - float tmp; - int i, j, k; - for (i = 0; i < _m; i++) - { - for (j = 0; j < _n; j++) - { - tmp = 0.0f; - for (k = 0; k < _l; k++) - { - tmp += _matrix1[_l * i + k] * _matrix2[_n * k + j]; - } - _matrixOut[_n * i + j] = tmp; - } - } -} - -static void RotMatToEulerAngle(const float* _rotationM, float* _eulerAngles) -{ - float A, B, C, cb; - - if (fabs(_rotationM[6]) >= 1.0 - 0.0001) - { - if (_rotationM[6] < 0) - { - A = 0.0f; - B = (float) M_PI_2; - C = atan2f(_rotationM[1], _rotationM[4]); - } else - { - A = 0.0f; - B = -(float) M_PI_2; - C = -atan2f(_rotationM[1], _rotationM[4]); - } - } else - { - B = atan2f(-_rotationM[6], sqrtf(_rotationM[0] * _rotationM[0] + _rotationM[3] * _rotationM[3])); - cb = cosf(B); - A = atan2f(_rotationM[3] / cb, _rotationM[0] / cb); - C = atan2f(_rotationM[7] / cb, _rotationM[8] / cb); - } - - _eulerAngles[0] = C; - _eulerAngles[1] = B; - _eulerAngles[2] = A; -} - -static void EulerAngleToRotMat(const float* _eulerAngles, float* _rotationM) -{ - float ca, cb, cc, sa, sb, sc; - - cc = cosf(_eulerAngles[0]); - cb = cosf(_eulerAngles[1]); - ca = cosf(_eulerAngles[2]); - sc = sinf(_eulerAngles[0]); - sb = sinf(_eulerAngles[1]); - sa = sinf(_eulerAngles[2]); - - _rotationM[0] = ca * cb; - _rotationM[1] = ca * sb * sc - sa * cc; - _rotationM[2] = ca * sb * cc + sa * sc; - _rotationM[3] = sa * cb; - _rotationM[4] = sa * sb * sc + ca * cc; - _rotationM[5] = sa * sb * cc - ca * sc; - _rotationM[6] = -sb; - _rotationM[7] = cb * sc; - _rotationM[8] = cb * cc; -} - - -DOF6Kinematic::DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT) - : armConfig(ArmConfig_t{L_BS, D_BS, L_SE, L_EW, D_EW, L_WT}) -{ - float tmp_DH_matrix[6][4] = { - {0.0f, armConfig.L_BASE, armConfig.D_BASE, -(float) M_PI_2}, - {-(float) M_PI_2, 0.0f, armConfig.L_ARM, 0.0f}, - {(float) M_PI_2, armConfig.D_ELBOW, 0.0f, (float) M_PI_2}, - {0.0f, armConfig.L_FOREARM, 0.0f, -(float) M_PI_2}, - {0.0f, 0.0f, 0.0f, (float) M_PI_2}, - {0.0f, armConfig.L_WRIST, 0.0f, 0.0f} - }; - memcpy(DH_matrix, tmp_DH_matrix, sizeof(tmp_DH_matrix)); - - float tmp_L1_bs[3] = {armConfig.D_BASE, -armConfig.L_BASE, 0.0f}; - memcpy(L1_base, tmp_L1_bs, sizeof(tmp_L1_bs)); - float tmp_L2_se[3] = {armConfig.L_ARM, 0.0f, 0.0f}; - memcpy(L2_arm, tmp_L2_se, sizeof(tmp_L2_se)); - float tmp_L3_ew[3] = {-armConfig.D_ELBOW, 0.0f, armConfig.L_FOREARM}; - memcpy(L3_elbow, tmp_L3_ew, sizeof(tmp_L3_ew)); - float tmp_L6_wt[3] = {0.0f, 0.0f, armConfig.L_WRIST}; - memcpy(L6_wrist, tmp_L6_wt, sizeof(tmp_L6_wt)); - - l_se_2 = armConfig.L_ARM * armConfig.L_ARM; - l_se = armConfig.L_ARM; - l_ew_2 = armConfig.L_FOREARM * armConfig.L_FOREARM + armConfig.D_ELBOW * armConfig.D_ELBOW; - l_ew = 0; - atan_e = 0; -} - -bool -DOF6Kinematic::SolveFK(const DOF6Kinematic::Joint6D_t &_inputJoint6D, DOF6Kinematic::Pose6D_t &_outputPose6D) -{ - float q_in[6]; - float q[6]; - float cosq, sinq; - float cosa, sina; - float P06[6]; - float R06[9]; - float R[6][9]; - float R02[9]; - float R03[9]; - float R04[9]; - float R05[9]; - float L0_bs[3]; - float L0_se[3]; - float L0_ew[3]; - float L0_wt[3]; - - for (int i = 0; i < 6; i++) - q_in[i] = _inputJoint6D.a[i] / RAD_TO_DEG; - - for (int i = 0; i < 6; i++) - { - q[i] = q_in[i] + DH_matrix[i][0]; - cosq = cosf(q[i]); - sinq = sinf(q[i]); - cosa = cosf(DH_matrix[i][3]); - sina = sinf(DH_matrix[i][3]); - - R[i][0] = cosq; - R[i][1] = -cosa * sinq; - R[i][2] = sina * sinq; - R[i][3] = sinq; - R[i][4] = cosa * cosq; - R[i][5] = -sina * cosq; - R[i][6] = 0.0f; - R[i][7] = sina; - R[i][8] = cosa; - } - - MatMultiply(R[0], R[1], R02, 3, 3, 3); - MatMultiply(R02, R[2], R03, 3, 3, 3); - MatMultiply(R03, R[3], R04, 3, 3, 3); - MatMultiply(R04, R[4], R05, 3, 3, 3); - MatMultiply(R05, R[5], R06, 3, 3, 3); - - MatMultiply(R[0], L1_base, L0_bs, 3, 3, 1); - MatMultiply(R02, L2_arm, L0_se, 3, 3, 1); - MatMultiply(R03, L3_elbow, L0_ew, 3, 3, 1); - MatMultiply(R06, L6_wrist, L0_wt, 3, 3, 1); - - for (int i = 0; i < 3; i++) - P06[i] = L0_bs[i] + L0_se[i] + L0_ew[i] + L0_wt[i]; - - RotMatToEulerAngle(R06, &(P06[3])); - - _outputPose6D.X = P06[0]; - _outputPose6D.Y = P06[1]; - _outputPose6D.Z = P06[2]; - _outputPose6D.A = P06[3] * RAD_TO_DEG; - _outputPose6D.B = P06[4] * RAD_TO_DEG; - _outputPose6D.C = P06[5] * RAD_TO_DEG; - memcpy(_outputPose6D.R, R06, 9 * sizeof(float)); - - return true; -} - -bool DOF6Kinematic::SolveIK(const DOF6Kinematic::Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, - DOF6Kinematic::IKSolves_t &_outputSolves) -{ - float qs[2]; - float qa[2][2]; - float qw[2][3]; - float cosqs, sinqs; - float cosqa[2], sinqa[2]; - float cosqw, sinqw; - float P06[6]; - float R06[9]; - float P0_w[3]; - float P1_w[3]; - float L0_wt[3]; - float L1_sw[3]; - float R10[9]; - float R31[9]; - float R30[9]; - float R36[9]; - float l_sw_2, l_sw, atan_a, acos_a, acos_e; - - int ind_arm, ind_elbow, ind_wrist; - int i; - - if (0 == l_ew) - { - l_ew = sqrtf(l_ew_2); - atan_e = atanf(armConfig.D_ELBOW / armConfig.L_FOREARM); - } - - P06[0] = _inputPose6D.X / 1000.0f; - P06[1] = _inputPose6D.Y / 1000.0f; - P06[2] = _inputPose6D.Z / 1000.0f; - if (!_inputPose6D.hasR) - { - P06[3] = _inputPose6D.A / RAD_TO_DEG; - P06[4] = _inputPose6D.B / RAD_TO_DEG; - P06[5] = _inputPose6D.C / RAD_TO_DEG; - EulerAngleToRotMat(&(P06[3]), R06); - } else - { - memcpy(R06, _inputPose6D.R, 9 * sizeof(float)); - } - for (i = 0; i < 2; i++) - { - qs[i] = _lastJoint6D.a[0]; - qa[i][0] = _lastJoint6D.a[1]; - qa[i][1] = _lastJoint6D.a[2]; - qw[i][0] = _lastJoint6D.a[3]; - qw[i][1] = _lastJoint6D.a[4]; - qw[i][2] = _lastJoint6D.a[5]; - } - MatMultiply(R06, L6_wrist, L0_wt, 3, 3, 1); - for (i = 0; i < 3; i++) - { - P0_w[i] = P06[i] - L0_wt[i]; - } - if (sqrt(P0_w[0] * P0_w[0] + P0_w[1] * P0_w[1]) <= 0.000001) - { - qs[0] = _lastJoint6D.a[0]; - qs[1] = _lastJoint6D.a[0]; - for (i = 0; i < 4; i++) - { - _outputSolves.solFlag[0 + i][0] = -1; - _outputSolves.solFlag[4 + i][0] = -1; - } - } else - { - qs[0] = atan2f(P0_w[1], P0_w[0]); - qs[1] = atan2f(-P0_w[1], -P0_w[0]); - for (i = 0; i < 4; i++) - { - _outputSolves.solFlag[0 + i][0] = 1; - _outputSolves.solFlag[4 + i][0] = 1; - } - } - for (ind_arm = 0; ind_arm < 2; ind_arm++) - { - cosqs = cosf(qs[ind_arm] + DH_matrix[0][0]); - sinqs = sinf(qs[ind_arm] + DH_matrix[0][0]); - - R10[0] = cosqs; - R10[1] = sinqs; - R10[2] = 0.0f; - R10[3] = 0.0f; - R10[4] = 0.0f; - R10[5] = -1.0f; - R10[6] = -sinqs; - R10[7] = cosqs; - R10[8] = 0.0f; - - MatMultiply(R10, P0_w, P1_w, 3, 3, 1); - for (i = 0; i < 3; i++) - { - L1_sw[i] = P1_w[i] - L1_base[i]; - } - l_sw_2 = L1_sw[0] * L1_sw[0] + L1_sw[1] * L1_sw[1]; - l_sw = sqrtf(l_sw_2); - - if (fabs(l_se + l_ew - l_sw) <= 0.000001) - { - qa[0][0] = atan2f(L1_sw[1], L1_sw[0]); - qa[1][0] = qa[0][0]; - qa[0][1] = 0.0f; - qa[1][1] = 0.0f; - if (l_sw > l_se + l_ew) - { - for (i = 0; i < 2; i++) - { - _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 0; - _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 0; - } - } else - { - for (i = 0; i < 2; i++) - { - _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 1; - _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 1; - } - } - } else if (fabs(l_sw - fabs(l_se - l_ew)) <= 0.000001) - { - qa[0][0] = atan2f(L1_sw[1], L1_sw[0]); - qa[1][0] = qa[0][0]; - if (0 == ind_arm) - { - qa[0][1] = (float) M_PI; - qa[1][1] = -(float) M_PI; - } else - { - qa[0][1] = -(float) M_PI; - qa[1][1] = (float) M_PI; - } - if (l_sw < fabs(l_se - l_ew)) - { - for (i = 0; i < 2; i++) - { - _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 0; - _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 0; - } - } else - { - for (i = 0; i < 2; i++) - { - _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 1; - _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 1; - } - } - } else - { - atan_a = atan2f(L1_sw[1], L1_sw[0]); - acos_a = 0.5f * (l_se_2 + l_sw_2 - l_ew_2) / (l_se * l_sw); - if (acos_a >= 1.0f) acos_a = 0.0f; - else if (acos_a <= -1.0f) acos_a = (float) M_PI; - else acos_a = acosf(acos_a); - acos_e = 0.5f * (l_se_2 + l_ew_2 - l_sw_2) / (l_se * l_ew); - if (acos_e >= 1.0f) acos_e = 0.0f; - else if (acos_e <= -1.0f) acos_e = (float) M_PI; - else acos_e = acosf(acos_e); - if (0 == ind_arm) - { - qa[0][0] = atan_a - acos_a + (float) M_PI_2; - qa[0][1] = atan_e - acos_e + (float) M_PI; - qa[1][0] = atan_a + acos_a + (float) M_PI_2; - qa[1][1] = atan_e + acos_e - (float) M_PI; - - } else - { - qa[0][0] = atan_a + acos_a + (float) M_PI_2; - qa[0][1] = atan_e + acos_e - (float) M_PI; - qa[1][0] = atan_a - acos_a + (float) M_PI_2; - qa[1][1] = atan_e - acos_e + (float) M_PI; - } - for (i = 0; i < 2; i++) - { - _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 1; - _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 1; - } - } - for (ind_elbow = 0; ind_elbow < 2; ind_elbow++) - { - cosqa[0] = cosf(qa[ind_elbow][0] + DH_matrix[1][0]); - sinqa[0] = sinf(qa[ind_elbow][0] + DH_matrix[1][0]); - cosqa[1] = cosf(qa[ind_elbow][1] + DH_matrix[2][0]); - sinqa[1] = sinf(qa[ind_elbow][1] + DH_matrix[2][0]); - - R31[0] = cosqa[0] * cosqa[1] - sinqa[0] * sinqa[1]; - R31[1] = cosqa[0] * sinqa[1] + sinqa[0] * cosqa[1]; - R31[2] = 0.0f; - R31[3] = 0.0f; - R31[4] = 0.0f; - R31[5] = 1.0f; - R31[6] = cosqa[0] * sinqa[1] + sinqa[0] * cosqa[1]; - R31[7] = -cosqa[0] * cosqa[1] + sinqa[0] * sinqa[1]; - R31[8] = 0.0f; - - MatMultiply(R31, R10, R30, 3, 3, 3); - MatMultiply(R30, R06, R36, 3, 3, 3); - - if (R36[8] >= 1.0 - 0.000001) - { - cosqw = 1.0f; - qw[0][1] = 0.0f; - qw[1][1] = 0.0f; - } else if (R36[8] <= -1.0 + 0.000001) - { - cosqw = -1.0f; - if (0 == ind_arm) - { - qw[0][1] = (float) M_PI; - qw[1][1] = -(float) M_PI; - } else - { - qw[0][1] = -(float) M_PI; - qw[1][1] = (float) M_PI; - } - } else - { - cosqw = R36[8]; - if (0 == ind_arm) - { - qw[0][1] = acosf(cosqw); - qw[1][1] = -acosf(cosqw); - } else - { - qw[0][1] = -acosf(cosqw); - qw[1][1] = acosf(cosqw); - } - } - if (1.0f == cosqw || -1.0f == cosqw) - { - if (0 == ind_arm) - { - qw[0][0] = _lastJoint6D.a[3]; - cosqw = cosf(_lastJoint6D.a[3] + DH_matrix[3][0]); - sinqw = sinf(_lastJoint6D.a[3] + DH_matrix[3][0]); - qw[0][2] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); - qw[1][2] = _lastJoint6D.a[5]; - cosqw = cosf(_lastJoint6D.a[5] + DH_matrix[5][0]); - sinqw = sinf(_lastJoint6D.a[5] + DH_matrix[5][0]); - qw[1][0] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); - } else - { - qw[0][2] = _lastJoint6D.a[5]; - cosqw = cosf(_lastJoint6D.a[5] + DH_matrix[5][0]); - sinqw = sinf(_lastJoint6D.a[5] + DH_matrix[5][0]); - qw[0][0] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); - qw[1][0] = _lastJoint6D.a[3]; - cosqw = cosf(_lastJoint6D.a[3] + DH_matrix[3][0]); - sinqw = sinf(_lastJoint6D.a[3] + DH_matrix[3][0]); - qw[1][2] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); - } - _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 0][2] = -1; - _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 1][2] = -1; - } else - { - if (0 == ind_arm) - { - qw[0][0] = atan2f(R36[5], R36[2]); - qw[1][0] = atan2f(-R36[5], -R36[2]); - qw[0][2] = atan2f(R36[7], -R36[6]); - qw[1][2] = atan2f(-R36[7], R36[6]); - } else - { - qw[0][0] = atan2f(-R36[5], -R36[2]); - qw[1][0] = atan2f(R36[5], R36[2]); - qw[0][2] = atan2f(-R36[7], R36[6]); - qw[1][2] = atan2f(R36[7], -R36[6]); - } - _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 0][2] = 1; - _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 1][2] = 1; - } - for (ind_wrist = 0; ind_wrist < 2; ind_wrist++) - { - if (qs[ind_arm] > (float) M_PI) - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[0] = - qs[ind_arm] - (float) M_PI; - else if (qs[ind_arm] < -(float) M_PI) - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[0] = - qs[ind_arm] + (float) M_PI; - else - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[0] = qs[ind_arm]; - - for (i = 0; i < 2; i++) - { - if (qa[ind_elbow][i] > (float) M_PI) - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[1 + i] = - qa[ind_elbow][i] - (float) M_PI; - else if (qa[ind_elbow][i] < -(float) M_PI) - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[1 + i] = - qa[ind_elbow][i] + (float) M_PI; - else - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[1 + i] = - qa[ind_elbow][i]; - } - - for (i = 0; i < 3; i++) - { - if (qw[ind_wrist][i] > (float) M_PI) - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[3 + i] = - qw[ind_wrist][i] - (float) M_PI; - else if (qw[ind_wrist][i] < -(float) M_PI) - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[3 + i] = - qw[ind_wrist][i] + (float) M_PI; - else - _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[3 + i] = - qw[ind_wrist][i]; - } - } - } - } - - for (i = 0; i < 8; i++) - for (float &j: _outputSolves.config[i].a) - j *= RAD_TO_DEG; - - return true; -} - -DOF6Kinematic::Joint6D_t -operator-(const DOF6Kinematic::Joint6D_t &_joints1, const DOF6Kinematic::Joint6D_t &_joints2) -{ - DOF6Kinematic::Joint6D_t tmp{}; - for (int i = 0; i < 6; i++) - tmp.a[i] = _joints1.a[i] - _joints2.a[i]; - - return tmp; -} +#include "6dof_kinematic.h" + +inline float cosf(float x) +{ + return arm_cos_f32(x); +} + +inline float sinf(float x) +{ + return arm_sin_f32(x); +} + +static void MatMultiply(const float* _matrix1, const float* _matrix2, float* _matrixOut, + const int _m, const int _l, const int _n) +{ + float tmp; + int i, j, k; + for (i = 0; i < _m; i++) + { + for (j = 0; j < _n; j++) + { + tmp = 0.0f; + for (k = 0; k < _l; k++) + { + tmp += _matrix1[_l * i + k] * _matrix2[_n * k + j]; + } + _matrixOut[_n * i + j] = tmp; + } + } +} + +static void RotMatToEulerAngle(const float* _rotationM, float* _eulerAngles) +{ + float A, B, C, cb; + + if (fabs(_rotationM[6]) >= 1.0 - 0.0001) + { + if (_rotationM[6] < 0) + { + A = 0.0f; + B = (float) M_PI_2; + C = atan2f(_rotationM[1], _rotationM[4]); + } else + { + A = 0.0f; + B = -(float) M_PI_2; + C = -atan2f(_rotationM[1], _rotationM[4]); + } + } else + { + B = atan2f(-_rotationM[6], sqrtf(_rotationM[0] * _rotationM[0] + _rotationM[3] * _rotationM[3])); + cb = cosf(B); + A = atan2f(_rotationM[3] / cb, _rotationM[0] / cb); + C = atan2f(_rotationM[7] / cb, _rotationM[8] / cb); + } + + _eulerAngles[0] = C; + _eulerAngles[1] = B; + _eulerAngles[2] = A; +} + +static void EulerAngleToRotMat(const float* _eulerAngles, float* _rotationM) +{ + float ca, cb, cc, sa, sb, sc; + + cc = cosf(_eulerAngles[0]); + cb = cosf(_eulerAngles[1]); + ca = cosf(_eulerAngles[2]); + sc = sinf(_eulerAngles[0]); + sb = sinf(_eulerAngles[1]); + sa = sinf(_eulerAngles[2]); + + _rotationM[0] = ca * cb; + _rotationM[1] = ca * sb * sc - sa * cc; + _rotationM[2] = ca * sb * cc + sa * sc; + _rotationM[3] = sa * cb; + _rotationM[4] = sa * sb * sc + ca * cc; + _rotationM[5] = sa * sb * cc - ca * sc; + _rotationM[6] = -sb; + _rotationM[7] = cb * sc; + _rotationM[8] = cb * cc; +} + + +DOF6Kinematic::DOF6Kinematic(float L_BS, float D_BS, float L_AM, float L_FA, float D_EW, float L_WT) + : armConfig(ArmConfig_t{L_BS, D_BS, L_AM, L_FA, D_EW, L_WT}) +{ + float tmp_DH_matrix[6][4] = { + {0.0f, armConfig.L_BASE, armConfig.D_BASE, -(float) M_PI_2}, + {-(float) M_PI_2, 0.0f, armConfig.L_ARM, 0.0f}, + {(float) M_PI_2, armConfig.D_ELBOW, 0.0f, (float) M_PI_2}, + {0.0f, armConfig.L_FOREARM, 0.0f, -(float) M_PI_2}, + {0.0f, 0.0f, 0.0f, (float) M_PI_2}, + {0.0f, armConfig.L_WRIST, 0.0f, 0.0f} + }; + memcpy(DH_matrix, tmp_DH_matrix, sizeof(tmp_DH_matrix)); + + float tmp_L1_bs[3] = {armConfig.D_BASE, -armConfig.L_BASE, 0.0f}; + memcpy(L1_base, tmp_L1_bs, sizeof(tmp_L1_bs)); + float tmp_L2_se[3] = {armConfig.L_ARM, 0.0f, 0.0f}; + memcpy(L2_arm, tmp_L2_se, sizeof(tmp_L2_se)); + float tmp_L3_ew[3] = {-armConfig.D_ELBOW, 0.0f, armConfig.L_FOREARM}; + memcpy(L3_elbow, tmp_L3_ew, sizeof(tmp_L3_ew)); + float tmp_L6_wt[3] = {0.0f, 0.0f, armConfig.L_WRIST}; + memcpy(L6_wrist, tmp_L6_wt, sizeof(tmp_L6_wt)); + + l_se_2 = armConfig.L_ARM * armConfig.L_ARM; + l_se = armConfig.L_ARM; + l_ew_2 = armConfig.L_FOREARM * armConfig.L_FOREARM + armConfig.D_ELBOW * armConfig.D_ELBOW; + l_ew = 0; + atan_e = 0; +} + +bool +DOF6Kinematic::SolveFK(const DOF6Kinematic::Joint6D_t &_inputJoint6D, DOF6Kinematic::Pose6D_t &_outputPose6D) +{ + float q_in[6]; + float q[6]; + float cosq, sinq; + float cosa, sina; + float P06[6]; + float R06[9]; + float R[6][9]; + float R02[9]; + float R03[9]; + float R04[9]; + float R05[9]; + float L0_bs[3]; + float L0_se[3]; + float L0_ew[3]; + float L0_wt[3]; + + for (int i = 0; i < 6; i++) + q_in[i] = _inputJoint6D.a[i] / RAD_TO_DEG; + + for (int i = 0; i < 6; i++) + { + q[i] = q_in[i] + DH_matrix[i][0]; + cosq = cosf(q[i]); + sinq = sinf(q[i]); + cosa = cosf(DH_matrix[i][3]); + sina = sinf(DH_matrix[i][3]); + + R[i][0] = cosq; + R[i][1] = -cosa * sinq; + R[i][2] = sina * sinq; + R[i][3] = sinq; + R[i][4] = cosa * cosq; + R[i][5] = -sina * cosq; + R[i][6] = 0.0f; + R[i][7] = sina; + R[i][8] = cosa; + } + + MatMultiply(R[0], R[1], R02, 3, 3, 3); + MatMultiply(R02, R[2], R03, 3, 3, 3); + MatMultiply(R03, R[3], R04, 3, 3, 3); + MatMultiply(R04, R[4], R05, 3, 3, 3); + MatMultiply(R05, R[5], R06, 3, 3, 3); + + MatMultiply(R[0], L1_base, L0_bs, 3, 3, 1); + MatMultiply(R02, L2_arm, L0_se, 3, 3, 1); + MatMultiply(R03, L3_elbow, L0_ew, 3, 3, 1); + MatMultiply(R06, L6_wrist, L0_wt, 3, 3, 1); + + for (int i = 0; i < 3; i++) + P06[i] = L0_bs[i] + L0_se[i] + L0_ew[i] + L0_wt[i]; + + RotMatToEulerAngle(R06, &(P06[3])); + + _outputPose6D.X = P06[0]; + _outputPose6D.Y = P06[1]; + _outputPose6D.Z = P06[2]; + _outputPose6D.A = P06[3] * RAD_TO_DEG; + _outputPose6D.B = P06[4] * RAD_TO_DEG; + _outputPose6D.C = P06[5] * RAD_TO_DEG; + memcpy(_outputPose6D.R, R06, 9 * sizeof(float)); + + return true; +} + +bool DOF6Kinematic::SolveIK(const DOF6Kinematic::Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, + DOF6Kinematic::IKSolves_t &_outputSolves) +{ + float qs[2]; + float qa[2][2]; + float qw[2][3]; + float cosqs, sinqs; + float cosqa[2], sinqa[2]; + float cosqw, sinqw; + float P06[6]; + float R06[9]; + float P0_w[3]; + float P1_w[3]; + float L0_wt[3]; + float L1_sw[3]; + float R10[9]; + float R31[9]; + float R30[9]; + float R36[9]; + float l_sw_2, l_sw, atan_a, acos_a, acos_e; + + int ind_arm, ind_elbow, ind_wrist; + int i; + + if (0 == l_ew) + { + l_ew = sqrtf(l_ew_2); + atan_e = atanf(armConfig.D_ELBOW / armConfig.L_FOREARM); + } + + P06[0] = _inputPose6D.X / 1000.0f; + P06[1] = _inputPose6D.Y / 1000.0f; + P06[2] = _inputPose6D.Z / 1000.0f; + if (!_inputPose6D.hasR) + { + P06[3] = _inputPose6D.A / RAD_TO_DEG; + P06[4] = _inputPose6D.B / RAD_TO_DEG; + P06[5] = _inputPose6D.C / RAD_TO_DEG; + EulerAngleToRotMat(&(P06[3]), R06); + } else + { + memcpy(R06, _inputPose6D.R, 9 * sizeof(float)); + } + for (i = 0; i < 2; i++) + { + qs[i] = _lastJoint6D.a[0]; + qa[i][0] = _lastJoint6D.a[1]; + qa[i][1] = _lastJoint6D.a[2]; + qw[i][0] = _lastJoint6D.a[3]; + qw[i][1] = _lastJoint6D.a[4]; + qw[i][2] = _lastJoint6D.a[5]; + } + MatMultiply(R06, L6_wrist, L0_wt, 3, 3, 1); + for (i = 0; i < 3; i++) + { + P0_w[i] = P06[i] - L0_wt[i]; + } + if (sqrt(P0_w[0] * P0_w[0] + P0_w[1] * P0_w[1]) <= 0.000001) + { + qs[0] = _lastJoint6D.a[0]; + qs[1] = _lastJoint6D.a[0]; + for (i = 0; i < 4; i++) + { + _outputSolves.solFlag[0 + i][0] = -1; + _outputSolves.solFlag[4 + i][0] = -1; + } + } else + { + qs[0] = atan2f(P0_w[1], P0_w[0]); + qs[1] = atan2f(-P0_w[1], -P0_w[0]); + for (i = 0; i < 4; i++) + { + _outputSolves.solFlag[0 + i][0] = 1; + _outputSolves.solFlag[4 + i][0] = 1; + } + } + for (ind_arm = 0; ind_arm < 2; ind_arm++) + { + cosqs = cosf(qs[ind_arm] + DH_matrix[0][0]); + sinqs = sinf(qs[ind_arm] + DH_matrix[0][0]); + + R10[0] = cosqs; + R10[1] = sinqs; + R10[2] = 0.0f; + R10[3] = 0.0f; + R10[4] = 0.0f; + R10[5] = -1.0f; + R10[6] = -sinqs; + R10[7] = cosqs; + R10[8] = 0.0f; + + MatMultiply(R10, P0_w, P1_w, 3, 3, 1); + for (i = 0; i < 3; i++) + { + L1_sw[i] = P1_w[i] - L1_base[i]; + } + l_sw_2 = L1_sw[0] * L1_sw[0] + L1_sw[1] * L1_sw[1]; + l_sw = sqrtf(l_sw_2); + + if (fabs(l_se + l_ew - l_sw) <= 0.000001) + { + qa[0][0] = atan2f(L1_sw[1], L1_sw[0]); + qa[1][0] = qa[0][0]; + qa[0][1] = 0.0f; + qa[1][1] = 0.0f; + if (l_sw > l_se + l_ew) + { + for (i = 0; i < 2; i++) + { + _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 0; + _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 0; + } + } else + { + for (i = 0; i < 2; i++) + { + _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 1; + _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 1; + } + } + } else if (fabs(l_sw - fabs(l_se - l_ew)) <= 0.000001) + { + qa[0][0] = atan2f(L1_sw[1], L1_sw[0]); + qa[1][0] = qa[0][0]; + if (0 == ind_arm) + { + qa[0][1] = (float) M_PI; + qa[1][1] = -(float) M_PI; + } else + { + qa[0][1] = -(float) M_PI; + qa[1][1] = (float) M_PI; + } + if (l_sw < fabs(l_se - l_ew)) + { + for (i = 0; i < 2; i++) + { + _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 0; + _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 0; + } + } else + { + for (i = 0; i < 2; i++) + { + _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 1; + _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 1; + } + } + } else + { + atan_a = atan2f(L1_sw[1], L1_sw[0]); + acos_a = 0.5f * (l_se_2 + l_sw_2 - l_ew_2) / (l_se * l_sw); + if (acos_a >= 1.0f) acos_a = 0.0f; + else if (acos_a <= -1.0f) acos_a = (float) M_PI; + else acos_a = acosf(acos_a); + acos_e = 0.5f * (l_se_2 + l_ew_2 - l_sw_2) / (l_se * l_ew); + if (acos_e >= 1.0f) acos_e = 0.0f; + else if (acos_e <= -1.0f) acos_e = (float) M_PI; + else acos_e = acosf(acos_e); + if (0 == ind_arm) + { + qa[0][0] = atan_a - acos_a + (float) M_PI_2; + qa[0][1] = atan_e - acos_e + (float) M_PI; + qa[1][0] = atan_a + acos_a + (float) M_PI_2; + qa[1][1] = atan_e + acos_e - (float) M_PI; + + } else + { + qa[0][0] = atan_a + acos_a + (float) M_PI_2; + qa[0][1] = atan_e + acos_e - (float) M_PI; + qa[1][0] = atan_a - acos_a + (float) M_PI_2; + qa[1][1] = atan_e - acos_e + (float) M_PI; + } + for (i = 0; i < 2; i++) + { + _outputSolves.solFlag[4 * ind_arm + 0 + i][1] = 1; + _outputSolves.solFlag[4 * ind_arm + 2 + i][1] = 1; + } + } + for (ind_elbow = 0; ind_elbow < 2; ind_elbow++) + { + cosqa[0] = cosf(qa[ind_elbow][0] + DH_matrix[1][0]); + sinqa[0] = sinf(qa[ind_elbow][0] + DH_matrix[1][0]); + cosqa[1] = cosf(qa[ind_elbow][1] + DH_matrix[2][0]); + sinqa[1] = sinf(qa[ind_elbow][1] + DH_matrix[2][0]); + + R31[0] = cosqa[0] * cosqa[1] - sinqa[0] * sinqa[1]; + R31[1] = cosqa[0] * sinqa[1] + sinqa[0] * cosqa[1]; + R31[2] = 0.0f; + R31[3] = 0.0f; + R31[4] = 0.0f; + R31[5] = 1.0f; + R31[6] = cosqa[0] * sinqa[1] + sinqa[0] * cosqa[1]; + R31[7] = -cosqa[0] * cosqa[1] + sinqa[0] * sinqa[1]; + R31[8] = 0.0f; + + MatMultiply(R31, R10, R30, 3, 3, 3); + MatMultiply(R30, R06, R36, 3, 3, 3); + + if (R36[8] >= 1.0 - 0.000001) + { + cosqw = 1.0f; + qw[0][1] = 0.0f; + qw[1][1] = 0.0f; + } else if (R36[8] <= -1.0 + 0.000001) + { + cosqw = -1.0f; + if (0 == ind_arm) + { + qw[0][1] = (float) M_PI; + qw[1][1] = -(float) M_PI; + } else + { + qw[0][1] = -(float) M_PI; + qw[1][1] = (float) M_PI; + } + } else + { + cosqw = R36[8]; + if (0 == ind_arm) + { + qw[0][1] = acosf(cosqw); + qw[1][1] = -acosf(cosqw); + } else + { + qw[0][1] = -acosf(cosqw); + qw[1][1] = acosf(cosqw); + } + } + if (1.0f == cosqw || -1.0f == cosqw) + { + if (0 == ind_arm) + { + qw[0][0] = _lastJoint6D.a[3]; + cosqw = cosf(_lastJoint6D.a[3] + DH_matrix[3][0]); + sinqw = sinf(_lastJoint6D.a[3] + DH_matrix[3][0]); + qw[0][2] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); + qw[1][2] = _lastJoint6D.a[5]; + cosqw = cosf(_lastJoint6D.a[5] + DH_matrix[5][0]); + sinqw = sinf(_lastJoint6D.a[5] + DH_matrix[5][0]); + qw[1][0] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); + } else + { + qw[0][2] = _lastJoint6D.a[5]; + cosqw = cosf(_lastJoint6D.a[5] + DH_matrix[5][0]); + sinqw = sinf(_lastJoint6D.a[5] + DH_matrix[5][0]); + qw[0][0] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); + qw[1][0] = _lastJoint6D.a[3]; + cosqw = cosf(_lastJoint6D.a[3] + DH_matrix[3][0]); + sinqw = sinf(_lastJoint6D.a[3] + DH_matrix[3][0]); + qw[1][2] = atan2f(cosqw * R36[3] - sinqw * R36[0], cosqw * R36[0] + sinqw * R36[3]); + } + _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 0][2] = -1; + _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 1][2] = -1; + } else + { + if (0 == ind_arm) + { + qw[0][0] = atan2f(R36[5], R36[2]); + qw[1][0] = atan2f(-R36[5], -R36[2]); + qw[0][2] = atan2f(R36[7], -R36[6]); + qw[1][2] = atan2f(-R36[7], R36[6]); + } else + { + qw[0][0] = atan2f(-R36[5], -R36[2]); + qw[1][0] = atan2f(R36[5], R36[2]); + qw[0][2] = atan2f(-R36[7], R36[6]); + qw[1][2] = atan2f(R36[7], -R36[6]); + } + _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 0][2] = 1; + _outputSolves.solFlag[4 * ind_arm + 2 * ind_elbow + 1][2] = 1; + } + for (ind_wrist = 0; ind_wrist < 2; ind_wrist++) + { + if (qs[ind_arm] > (float) M_PI) + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[0] = + qs[ind_arm] - (float) M_PI; + else if (qs[ind_arm] < -(float) M_PI) + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[0] = + qs[ind_arm] + (float) M_PI; + else + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[0] = qs[ind_arm]; + + for (i = 0; i < 2; i++) + { + if (qa[ind_elbow][i] > (float) M_PI) + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[1 + i] = + qa[ind_elbow][i] - (float) M_PI; + else if (qa[ind_elbow][i] < -(float) M_PI) + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[1 + i] = + qa[ind_elbow][i] + (float) M_PI; + else + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[1 + i] = + qa[ind_elbow][i]; + } + + for (i = 0; i < 3; i++) + { + if (qw[ind_wrist][i] > (float) M_PI) + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[3 + i] = + qw[ind_wrist][i] - (float) M_PI; + else if (qw[ind_wrist][i] < -(float) M_PI) + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[3 + i] = + qw[ind_wrist][i] + (float) M_PI; + else + _outputSolves.config[4 * ind_arm + 2 * ind_elbow + ind_wrist].a[3 + i] = + qw[ind_wrist][i]; + } + } + } + } + + for (i = 0; i < 8; i++) + for (float &j: _outputSolves.config[i].a) + j *= RAD_TO_DEG; + + return true; +} + +DOF6Kinematic::Joint6D_t +operator-(const DOF6Kinematic::Joint6D_t &_joints1, const DOF6Kinematic::Joint6D_t &_joints2) +{ + DOF6Kinematic::Joint6D_t tmp{}; + for (int i = 0; i < 6; i++) + tmp.a[i] = _joints1.a[i] - _joints2.a[i]; + + return tmp; +} diff --git a/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.h b/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.h index a656ca6..b834903 100644 --- a/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.h +++ b/2.Firmware/Core-STM32F4-fw/Robot/algorithms/kinematic/6dof_kinematic.h @@ -1,83 +1,83 @@ -#ifndef DOF6_KINEMATIC_SOLVER_H -#define DOF6_KINEMATIC_SOLVER_H - -#include "stm32f405xx.h" -#include "arm_math.h" -#include "memory.h" - -class DOF6Kinematic -{ -private: - const float RAD_TO_DEG = 57.295777754771045f; - - // DH parameters - struct ArmConfig_t - { - float L_BASE; - float D_BASE; - float L_ARM; - float L_FOREARM; - float D_ELBOW; - float L_WRIST; - }; - ArmConfig_t armConfig; - - float DH_matrix[6][4] = {0}; // home,d,a,alpha - float L1_base[3] = {0}; - float L2_arm[3] = {0}; - float L3_elbow[3] = {0}; - float L6_wrist[3] = {0}; - - float l_se_2; - float l_se; - float l_ew_2; - float l_ew; - float atan_e; - -public: - struct Joint6D_t - { - Joint6D_t() - = default; - - Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6) - : a{a1, a2, a3, a4, a5, a6} - {} - - float a[6]; - - friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2); - }; - - struct Pose6D_t - { - Pose6D_t() - = default; - - Pose6D_t(float x, float y, float z, float a, float b, float c) - : X(x), Y(y), Z(z), A(a), B(b), C(c), hasR(false) - {} - - float X{}, Y{}, Z{}; - float A{}, B{}, C{}; - float R[9]{}; - - // if Pose was calculated by FK then it's true automatically (so that no need to do extra calc), - // otherwise if manually set params then it should be set to false. - bool hasR{}; - }; - - struct IKSolves_t - { - Joint6D_t config[8]; - char solFlag[8][3]; - }; - - DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT); - - bool SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D); - - bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves); -}; - -#endif //DOF6_KINEMATIC_SOLVER_H +#ifndef DOF6_KINEMATIC_SOLVER_H +#define DOF6_KINEMATIC_SOLVER_H + +#include "stm32f405xx.h" +#include "arm_math.h" +#include "memory.h" + +class DOF6Kinematic +{ +private: + const float RAD_TO_DEG = 57.295777754771045f; + + // DH parameters + struct ArmConfig_t + { + float L_BASE; + float D_BASE; + float L_ARM; + float L_FOREARM; + float D_ELBOW; + float L_WRIST; + }; + ArmConfig_t armConfig; + + float DH_matrix[6][4] = {0}; // home,d,a,alpha + float L1_base[3] = {0}; + float L2_arm[3] = {0}; + float L3_elbow[3] = {0}; + float L6_wrist[3] = {0}; + + float l_se_2; + float l_se; + float l_ew_2; + float l_ew; + float atan_e; + +public: + struct Joint6D_t + { + Joint6D_t() + = default; + + Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6) + : a{a1, a2, a3, a4, a5, a6} + {} + + float a[6]; + + friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2); + }; + + struct Pose6D_t + { + Pose6D_t() + = default; + + Pose6D_t(float x, float y, float z, float a, float b, float c) + : X(x), Y(y), Z(z), A(a), B(b), C(c), hasR(false) + {} + + float X{}, Y{}, Z{}; + float A{}, B{}, C{}; + float R[9]{}; + + // if Pose was calculated by FK then it's true automatically (so that no need to do extra calc), + // otherwise if manually set params then it should be set to false. + bool hasR{}; + }; + + struct IKSolves_t + { + Joint6D_t config[8]; + char solFlag[8][3]; + }; + + DOF6Kinematic(float L_BS, float D_BS, float L_AM, float L_FA, float D_EW, float L_WT); + + bool SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D); + + bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves); +}; + +#endif //DOF6_KINEMATIC_SOLVER_H diff --git a/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.cpp b/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.cpp index 12a8213..677668e 100644 --- a/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.cpp +++ b/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.cpp @@ -1,458 +1,546 @@ -#include -#include "communication.hpp" -#include "dummy_robot.h" - - -inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints) -{ - float max = -1; - - for (float i: _joints.a) - { - if (abs(i) > max) - max = abs(i); - } - - return max; -} - - -DummyRobot::DummyRobot(CAN_HandleTypeDef* _hcan) : - hcan(_hcan) -{ - motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 30, -180, 180); - motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 50, -170, 170); - motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 30, -73, 90); - motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 30, 35, 180); - motorJ[4] = new CtrlStepMotor(_hcan, 4, false, 24, -180, 180); - motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 30, -120, 120); - motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -720, 720); - hand = new DummyHand(_hcan, 7); - - dof6Solver = new DOF6Kinematic(0.109f, 0.035f, 0.146f, 0.115f, 0.052f, 0.072f); -} - - -DummyRobot::~DummyRobot() -{ - for (int j = 0; j <= 6; j++) - delete motorJ[j]; - - delete hand; - delete dof6Solver; -} - - -void DummyRobot::Reboot() -{ - motorJ[ALL]->Reboot(); - osDelay(500); // waiting for all joints done - HAL_NVIC_SystemReset(); -} - - -float DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t _joints) -{ - float maxAngle = AbsMaxOf6(_joints - currentJoints); - float time = maxAngle / jointSpeed; - - jointsStateFlag = 0; - - for (int j = 1; j <= 6; j++) - motorJ[j]->SetAngleWithTime(_joints.a[j - 1] - initPose.a[j - 1], time); - - return time; -} - - -float DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6) -{ - float joints[6] = {_j1, _j2, _j3, _j4, _j5, _j6}; - bool valid = true; - - for (int j = 1; j <= 6; j++) - { - if (joints[j - 1] > motorJ[j]->angleLimitMax || - joints[j - 1] < motorJ[j]->angleLimitMin) - valid = false; - } - - float time = 0; - if (valid) - time = MoveJoints(DOF6Kinematic::Joint6D_t( - joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5])); - - return time; -} - - -float DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c) -{ - DOF6Kinematic::Pose6D_t pose6D(_x, _y, _z, _a, _b, _c); - DOF6Kinematic::IKSolves_t ikSolves{}; - DOF6Kinematic::Joint6D_t lastJoint6D{}; - - dof6Solver->SolveIK(pose6D, lastJoint6D, ikSolves); - - bool valid[8]; - int validCnt = 0; - - for (int i = 0; i < 8; i++) - { - valid[i] = true; - - for (int j = 1; j <= 6; j++) - { - if (ikSolves.config[i].a[j - 1] > motorJ[j]->angleLimitMax || - ikSolves.config[i].a[j - 1] < motorJ[j]->angleLimitMin) - { - valid[i] = false; - continue; - } - } - - if (valid[i]) validCnt++; - } - - if (validCnt) - { - float min = 1000; - uint8_t index = 0; - for (int i = 0; i < 8; i++) - { - if (valid[i]) - { - for (int j = 0; j < 6; j++) - lastJoint6D.a[j] = ikSolves.config[i].a[j]; - DOF6Kinematic::Joint6D_t tmp = currentJoints - lastJoint6D; - float maxAngle = AbsMaxOf6(tmp); - if (maxAngle < min) - { - min = maxAngle; - index = i; - } - } - } - - return MoveJ(ikSolves.config[index].a[0], ikSolves.config[index].a[1], ikSolves.config[index].a[2], - ikSolves.config[index].a[3], ikSolves.config[index].a[4], ikSolves.config[index].a[5]); - } else - return 0; -} - - -void DummyRobot::MoveTrajectoryJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6) -{ - -} - - -void DummyRobot::MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c) -{ - -} - - -void DummyRobot::UpdateJointAngles() -{ - motorJ[ALL]->UpdateAngle(); -} - - -void DummyRobot::UpdateJointAnglesCallback() -{ - for (int i = 1; i <= 6; i++) - { - currentJoints.a[i - 1] = motorJ[i]->angle + initPose.a[i - 1]; - - if (motorJ[i]->state == CtrlStepMotor::FINISH) - jointsStateFlag |= (1 << i); - else - jointsStateFlag &= ~(1 << i); - } -} - - -void DummyRobot::SetJointSpeed(float _speed) -{ - if (_speed < 0)_speed = 0; - else if (_speed > 100) _speed = 100; - - jointSpeed = _speed; -} - - -void DummyRobot::SetJointAcceleration(float _acc) -{ - if (_acc < 0)_acc = 0; - else if (_acc > 1000) _acc = 1000; - - motorJ[ALL]->SetAcceleration(_acc, false); -} - - -void DummyRobot::CalibrateHomeOffset() -{ - // 1.Manually move joints to L-Pose [precisely] - // ... - motorJ[2]->SetCurrentLimit(0.5); - motorJ[3]->SetCurrentLimit(0.5); - osDelay(500); - - // 2.Apply Home-Offset the first time - motorJ[ALL]->ApplyPositionAsHome(); - osDelay(500); - - // 3.Go to Resting-Pose - initPose = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0); - currentJoints = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0); - Resting(); - while (IsMoving()) - osDelay(10); - osDelay(500); - - // 4.Apply Home-Offset the second time - motorJ[ALL]->ApplyPositionAsHome(); - osDelay(500); - motorJ[2]->SetCurrentLimit(1); - motorJ[3]->SetCurrentLimit(1); - osDelay(500); - - Reboot(); -} - - -void DummyRobot::Homing() -{ - float lastSpeed = jointSpeed; - SetJointSpeed(10); - - MoveJ(0, 0, 90, 0, 0, 0); - while (IsMoving()) - osDelay(10); - - SetJointSpeed(lastSpeed); -} - - -void DummyRobot::Resting() -{ - float lastSpeed = jointSpeed; - SetJointSpeed(10); - - MoveJ(REST_POSE.a[0], REST_POSE.a[1], REST_POSE.a[2], - REST_POSE.a[3], REST_POSE.a[4], REST_POSE.a[5]); - while (IsMoving()) - osDelay(10); - - SetJointSpeed(lastSpeed); -} - - -void DummyRobot::SetEnable(bool _enable) -{ - motorJ[ALL]->SetEnable(_enable); - hand->SetEnable(_enable); -} - - -void DummyRobot::UpdateJointPose6D() -{ - dof6Solver->SolveFK(currentJoints, currentPose6D); - currentPose6D.X *= 1000; // m -> mm - currentPose6D.Y *= 1000; // m -> mm - currentPose6D.Z *= 1000; // m -> mm -} - - -bool DummyRobot::IsMoving() -{ - return jointsStateFlag != 0b1111110; -} - - -void DummyRobot::SetCommandMode(uint8_t _mode) -{ - commandMode = static_cast(_mode); - switch (commandMode) - { - case COMMAND_TARGET_POINT_SEQUENTIAL: - SetJointAcceleration(DEFAULT_JOINT_ACCELERATION); - break; - case COMMAND_TARGET_POINT_INTERRUPTABLE: - SetJointAcceleration(DEFAULT_JOINT_ACCELERATION); - break; - case COMMAND_CONTINUES_TRAJECTORY: - SetJointAcceleration(1000); - break; - } -} - - -DummyHand::DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id) : - nodeID(_id), hcan(_hcan) -{ - txHeader = - { - .StdId = 0, - .ExtId = 0, - .IDE = CAN_ID_STD, - .RTR = CAN_RTR_DATA, - .DLC = 8, - .TransmitGlobalTime = DISABLE - }; -} - - -void DummyHand::SetAngle(float _angle) -{ - if (_angle > 30)_angle = 30; - if (_angle < 0)_angle = 0; - - uint8_t mode = 0x02; - txHeader.StdId = 7 << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_angle; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void DummyHand::SetMaxCurrent(float _val) -{ - if (_val > 1)_val = 1; - if (_val < 0)_val = 0; - - uint8_t mode = 0x01; - txHeader.StdId = 7 << 7 | mode; - - // Float to Bytes - auto* b = (unsigned char*) &_val; - for (int i = 0; i < 4; i++) - canBuf[i] = *(b + i); - - CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); -} - - -void DummyHand::SetEnable(bool _enable) -{ - if (_enable) - SetMaxCurrent(maxCurrent); - else - SetMaxCurrent(0); -} - - -uint32_t DummyRobot::CommandHandler::Push(const std::string &_cmd) -{ - osStatus_t status = osMessageQueuePut(commandFifo, _cmd.c_str(), 0U, 0U); - if (status == osOK) - return osMessageQueueGetSpace(commandFifo); - - return 0xFF; // failed -} - - -void DummyRobot::CommandHandler::EmergencyStop() -{ - context->MoveJ(context->currentJoints.a[0], context->currentJoints.a[1], context->currentJoints.a[2], - context->currentJoints.a[3], context->currentJoints.a[4], context->currentJoints.a[5]); - - context->isStopped = true; - ClearFifo(); -} - - -void DummyRobot::CommandHandler::Resume() -{ - context->isStopped = false; -} - - -std::string DummyRobot::CommandHandler::Pop(uint32_t timeout) -{ - osStatus_t status = osMessageQueueGet(commandFifo, strBuffer, nullptr, timeout); - - return std::string{strBuffer}; -} - - -uint32_t DummyRobot::CommandHandler::GetSpace() -{ - return osMessageQueueGetSpace(commandFifo); -} - - -uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd) -{ - uint8_t argNum; - - switch (context->commandMode) - { - case COMMAND_TARGET_POINT_SEQUENTIAL: - if (_cmd[0] == '>') - { - float joints[6]; - float speed; - - argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2, - joints + 3, joints + 4, joints + 5, &speed); - if (argNum == 6) - { - context->MoveJ(joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5]); - } else if (argNum == 7) - { - context->SetJointSpeed(speed); - context->MoveJ(joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5]); - } - - while (context->IsMoving()) - osDelay(10); - - Respond(*usbStreamOutputPtr, "ok"); - } - break; - - case COMMAND_TARGET_POINT_INTERRUPTABLE: - if (_cmd[0] == '>') - { - float joints[6]; - float speed; - - argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2, - joints + 3, joints + 4, joints + 5, &speed); - if (argNum == 6) - { - context->MoveJ(joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5]); - } else if (argNum == 7) - { - context->SetJointSpeed(speed); - context->MoveJ(joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5]); - } - - Respond(*usbStreamOutputPtr, "ok"); - } - break; - - case COMMAND_CONTINUES_TRAJECTORY: - break; - } - - return osMessageQueueGetSpace(commandFifo); -} - - -void DummyRobot::CommandHandler::ClearFifo() -{ - osMessageQueueReset(commandFifo); -} - +#include "communication.hpp" +#include "dummy_robot.h" + +inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints, uint8_t &_index) +{ + float max = -1; + for (uint8_t i = 0; i < 6; i++) + { + if (abs(_joints.a[i]) > max) + { + max = abs(_joints.a[i]); + _index = i; + } + } + + return max; +} + + +DummyRobot::DummyRobot(CAN_HandleTypeDef* _hcan) : + hcan(_hcan) +{ + motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180); + motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 50, -170, 170); + motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 30, -73, 90); + motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 30, 35, 180); + motorJ[4] = new CtrlStepMotor(_hcan, 4, false, 24, -180, 180); + motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 30, -120, 120); + motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -720, 720); + hand = new DummyHand(_hcan, 7); + + dof6Solver = new DOF6Kinematic(0.109f, 0.035f, 0.146f, 0.115f, 0.052f, 0.072f); +} + + +DummyRobot::~DummyRobot() +{ + for (int j = 0; j <= 6; j++) + delete motorJ[j]; + + delete hand; + delete dof6Solver; +} + + +void DummyRobot::Init() +{ + SetCommandMode(DEFAULT_COMMAND_MODE); + SetJointSpeed(DEFAULT_JOINT_SPEED); +} + + +void DummyRobot::Reboot() +{ + motorJ[ALL]->Reboot(); + osDelay(500); // waiting for all joints done + HAL_NVIC_SystemReset(); +} + + +void DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t _joints) +{ + for (int j = 1; j <= 6; j++) + { + motorJ[j]->SetAngleWithVelocityLimit(_joints.a[j - 1] - initPose.a[j - 1], + dynamicJointSpeeds.a[j - 1]); + } +} + + +bool DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6) +{ + DOF6Kinematic::Joint6D_t targetJointsTmp(_j1, _j2, _j3, _j4, _j5, _j6); + bool valid = true; + + for (int j = 1; j <= 6; j++) + { + if (targetJointsTmp.a[j - 1] > motorJ[j]->angleLimitMax || + targetJointsTmp.a[j - 1] < motorJ[j]->angleLimitMin) + valid = false; + } + + if (valid) + { + DOF6Kinematic::Joint6D_t deltaJoints = targetJointsTmp - currentJoints; + uint8_t index; + float maxAngle = AbsMaxOf6(deltaJoints, index); + float time = maxAngle * (float) (motorJ[index + 1]->reduction) / jointSpeed; + for (int j = 1; j <= 6; j++) + { + dynamicJointSpeeds.a[j - 1] = + abs(deltaJoints.a[j - 1] * (float) (motorJ[j]->reduction) / time * 0.1f); //0~10r/s + } + + jointsStateFlag = 0; + targetJoints = targetJointsTmp; + + return true; + } + + return false; +} + + +bool DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c) +{ + DOF6Kinematic::Pose6D_t pose6D(_x, _y, _z, _a, _b, _c); + DOF6Kinematic::IKSolves_t ikSolves{}; + DOF6Kinematic::Joint6D_t lastJoint6D{}; + + dof6Solver->SolveIK(pose6D, lastJoint6D, ikSolves); + + bool valid[8]; + int validCnt = 0; + + for (int i = 0; i < 8; i++) + { + valid[i] = true; + + for (int j = 1; j <= 6; j++) + { + if (ikSolves.config[i].a[j - 1] > motorJ[j]->angleLimitMax || + ikSolves.config[i].a[j - 1] < motorJ[j]->angleLimitMin) + { + valid[i] = false; + continue; + } + } + + if (valid[i]) validCnt++; + } + + if (validCnt) + { + float min = 1000; + uint8_t indexConfig = 0, indexJoint = 0; + for (int i = 0; i < 8; i++) + { + if (valid[i]) + { + for (int j = 0; j < 6; j++) + lastJoint6D.a[j] = ikSolves.config[i].a[j]; + DOF6Kinematic::Joint6D_t tmp = currentJoints - lastJoint6D; + float maxAngle = AbsMaxOf6(tmp, indexJoint); + if (maxAngle < min) + { + min = maxAngle; + indexConfig = i; + } + } + } + + return MoveJ(ikSolves.config[indexConfig].a[0], ikSolves.config[indexConfig].a[1], + ikSolves.config[indexConfig].a[2], ikSolves.config[indexConfig].a[3], + ikSolves.config[indexConfig].a[4], ikSolves.config[indexConfig].a[5]); + } + + return false; +} + +void DummyRobot::UpdateJointAngles() +{ + motorJ[ALL]->UpdateAngle(); +} + + +void DummyRobot::UpdateJointAnglesCallback() +{ + for (int i = 1; i <= 6; i++) + { + currentJoints.a[i - 1] = motorJ[i]->angle + initPose.a[i - 1]; + + if (motorJ[i]->state == CtrlStepMotor::FINISH) + jointsStateFlag |= (1 << i); + else + jointsStateFlag &= ~(1 << i); + } +} + + +void DummyRobot::SetJointSpeed(float _speed) +{ + if (_speed < 0)_speed = 0; + else if (_speed > 100) _speed = 100; + + jointSpeed = _speed * jointSpeedRatio; +} + + +void DummyRobot::SetJointAcceleration(float _acc) +{ + if (_acc < 0)_acc = 0; + else if (_acc > 100) _acc = 100; + + for (int i = 1; i <= 6; i++) + motorJ[i]->SetAcceleration(_acc / 100 * DEFAULT_JOINT_ACCELERATION_BASES.a[i - 1]); +} + + +void DummyRobot::CalibrateHomeOffset() +{ + // Disable FixUpdate, but not disable motors + isEnabled = false; + motorJ[ALL]->SetEnable(true); + + // 1.Manually move joints to L-Pose [precisely] + // ... + motorJ[2]->SetCurrentLimit(0.5); + motorJ[3]->SetCurrentLimit(0.5); + osDelay(500); + + // 2.Apply Home-Offset the first time + motorJ[ALL]->ApplyPositionAsHome(); + osDelay(500); + + // 3.Go to Resting-Pose + initPose = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0); + currentJoints = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0); + Resting(); + osDelay(500); + + // 4.Apply Home-Offset the second time + motorJ[ALL]->ApplyPositionAsHome(); + osDelay(500); + motorJ[2]->SetCurrentLimit(1); + motorJ[3]->SetCurrentLimit(1); + osDelay(500); + + Reboot(); +} + + +void DummyRobot::Homing() +{ + float lastSpeed = jointSpeed; + SetJointSpeed(10); + + MoveJ(0, 0, 90, 0, 0, 0); + MoveJoints(targetJoints); + while (IsMoving()) + osDelay(10); + + SetJointSpeed(lastSpeed); +} + + +void DummyRobot::Resting() +{ + float lastSpeed = jointSpeed; + SetJointSpeed(10); + + MoveJ(REST_POSE.a[0], REST_POSE.a[1], REST_POSE.a[2], + REST_POSE.a[3], REST_POSE.a[4], REST_POSE.a[5]); + MoveJoints(targetJoints); + while (IsMoving()) + osDelay(10); + + SetJointSpeed(lastSpeed); +} + + +void DummyRobot::SetEnable(bool _enable) +{ + motorJ[ALL]->SetEnable(_enable); + isEnabled = _enable; +} + + +void DummyRobot::UpdateJointPose6D() +{ + dof6Solver->SolveFK(currentJoints, currentPose6D); + currentPose6D.X *= 1000; // m -> mm + currentPose6D.Y *= 1000; // m -> mm + currentPose6D.Z *= 1000; // m -> mm +} + + +bool DummyRobot::IsMoving() +{ + return jointsStateFlag != 0b1111110; +} + + +bool DummyRobot::IsEnabled() +{ + return isEnabled; +} + + +void DummyRobot::SetCommandMode(uint32_t _mode) +{ + if (_mode < COMMAND_TARGET_POINT_SEQUENTIAL || + _mode > COMMAND_MOTOR_TUNING) + return; + + commandMode = static_cast(_mode); + + switch (commandMode) + { + case COMMAND_TARGET_POINT_SEQUENTIAL: + case COMMAND_TARGET_POINT_INTERRUPTABLE: + jointSpeedRatio = 1; + SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_LOW); + break; + case COMMAND_CONTINUES_TRAJECTORY: + SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_HIGH); + jointSpeedRatio = 0.3; + break; + case COMMAND_MOTOR_TUNING: + break; + } +} + + +DummyHand::DummyHand(CAN_HandleTypeDef* _hcan, uint8_t +_id) : + nodeID(_id), hcan(_hcan) +{ + txHeader = + { + .StdId = 0, + .ExtId = 0, + .IDE = CAN_ID_STD, + .RTR = CAN_RTR_DATA, + .DLC = 8, + .TransmitGlobalTime = DISABLE + }; +} + + +void DummyHand::SetAngle(float _angle) +{ + if (_angle > 30)_angle = 30; + if (_angle < 0)_angle = 0; + + uint8_t mode = 0x02; + txHeader.StdId = 7 << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_angle; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void DummyHand::SetMaxCurrent(float _val) +{ + if (_val > 1)_val = 1; + if (_val < 0)_val = 0; + + uint8_t mode = 0x01; + txHeader.StdId = 7 << 7 | mode; + + // Float to Bytes + auto* b = (unsigned char*) &_val; + for (int i = 0; i < 4; i++) + canBuf[i] = *(b + i); + + CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); +} + + +void DummyHand::SetEnable(bool _enable) +{ + if (_enable) + SetMaxCurrent(maxCurrent); + else + SetMaxCurrent(0); +} + + +uint32_t DummyRobot::CommandHandler::Push(const std::string &_cmd) +{ + osStatus_t status = osMessageQueuePut(commandFifo, _cmd.c_str(), 0U, 0U); + if (status == osOK) + return osMessageQueueGetSpace(commandFifo); + + return 0xFF; // failed +} + + +void DummyRobot::CommandHandler::EmergencyStop() +{ + context->MoveJ(context->currentJoints.a[0], context->currentJoints.a[1], context->currentJoints.a[2], + context->currentJoints.a[3], context->currentJoints.a[4], context->currentJoints.a[5]); + context->MoveJoints(context->targetJoints); + context->isEnabled = false; + ClearFifo(); +} + + +std::string DummyRobot::CommandHandler::Pop(uint32_t timeout) +{ + osStatus_t status = osMessageQueueGet(commandFifo, strBuffer, nullptr, timeout); + + return std::string{strBuffer}; +} + + +uint32_t DummyRobot::CommandHandler::GetSpace() +{ + return osMessageQueueGetSpace(commandFifo); +} + + +uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd) +{ + uint8_t argNum; + + switch (context->commandMode) + { + case COMMAND_TARGET_POINT_SEQUENTIAL: + case COMMAND_CONTINUES_TRAJECTORY: + if (_cmd[0] == '>') + { + float joints[6]; + float speed; + + argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2, + joints + 3, joints + 4, joints + 5, &speed); + if (argNum == 6) + { + context->MoveJ(joints[0], joints[1], joints[2], + joints[3], joints[4], joints[5]); + } else if (argNum == 7) + { + context->SetJointSpeed(speed); + context->MoveJ(joints[0], joints[1], joints[2], + joints[3], joints[4], joints[5]); + } + // Trigger a transmission immediately, in case IsMoving() returns false + context->MoveJoints(context->targetJoints); + + while (context->IsMoving() && context->IsEnabled()) + osDelay(5); + Respond(*usbStreamOutputPtr, "ok"); + Respond(*uart4StreamOutputPtr, "ok"); + } else if (_cmd[0] == '@') + { + float pose[6]; + float speed; + + argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2, + pose + 3, pose + 4, pose + 5, &speed); + if (argNum == 6) + { + context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]); + } else if (argNum == 7) + { + context->SetJointSpeed(speed); + context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]); + } + // Trigger a transmission immediately, in case IsMoving() returns false + context->MoveJoints(context->targetJoints); + + while (context->IsMoving()) + osDelay(5); + Respond(*usbStreamOutputPtr, "ok"); + Respond(*uart4StreamOutputPtr, "ok"); + } + + break; + + case COMMAND_TARGET_POINT_INTERRUPTABLE: + if (_cmd[0] == '>') + { + float joints[6]; + float speed; + + argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2, + joints + 3, joints + 4, joints + 5, &speed); + if (argNum == 6) + { + context->MoveJ(joints[0], joints[1], joints[2], + joints[3], joints[4], joints[5]); + } else if (argNum == 7) + { + context->SetJointSpeed(speed); + context->MoveJ(joints[0], joints[1], joints[2], + joints[3], joints[4], joints[5]); + } + Respond(*usbStreamOutputPtr, "ok"); + Respond(*uart4StreamOutputPtr, "ok"); + } else if (_cmd[0] == '@') + { + float pose[6]; + float speed; + + argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2, + pose + 3, pose + 4, pose + 5, &speed); + if (argNum == 6) + { + context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]); + } else if (argNum == 7) + { + context->SetJointSpeed(speed); + context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]); + } + Respond(*usbStreamOutputPtr, "ok"); + Respond(*uart4StreamOutputPtr, "ok"); + } + break; + + case COMMAND_MOTOR_TUNING: + break; + } + + return osMessageQueueGetSpace(commandFifo); +} + + +void DummyRobot::CommandHandler::ClearFifo() +{ + osMessageQueueReset(commandFifo); +} + + +void DummyRobot::TuningHelper::SetTuningFlag(uint8_t _flag) +{ + tuningFlag = _flag; +} + + +void DummyRobot::TuningHelper::Tick(uint32_t _timeMillis) +{ + time += PI * 2 * frequency * (float) _timeMillis / 1000.0f; + float delta = amplitude * sinf(time); + + for (int i = 1; i <= 6; i++) + if (tuningFlag & (1 << (i - 1))) + context->motorJ[i]->SetAngle(delta); +} + + +void DummyRobot::TuningHelper::SetFreqAndAmp(float _freq, float _amp) +{ + if (_freq > 5)_freq = 5; + else if (_freq < 0.1) _freq = 0.1; + if (_amp > 50)_amp = 50; + else if (_amp < 1) _amp = 1; + + frequency = _freq; + amplitude = _amp; +} diff --git a/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.h b/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.h index 6a1bd54..6bb7bda 100644 --- a/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.h +++ b/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.h @@ -1,167 +1,205 @@ -#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H -#define REF_STM32F4_FW_DUMMY_ROBOT_H - -#include "algorithms/kinematic/6dof_kinematic.h" -#include "actuators/ctrl_step/ctrl_step.hpp" - -#define ALL 0 - -/* - | PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` | - | ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- | - | **Joint1** | 1 | 25 | 1000 | 80 | 300 | 250 | - | **Joint2** | 1.5 | 25 | 1000 | 250 | 300 | 200 | - | **Joint3** | 1 | 25 | 1000 | 350 | 500 | 250 | - | **Joint4** | 1.5 | 25 | 1000 | 350 | 500 | 250 | - | **Joint5** | 1.5 | 25 | 1000 | 350 | 500 | 250 | - | **Joint6** | 1.5 | 25 | 1000 | 350 | 500 | 250 | - */ - - -class DummyHand -{ -public: - uint8_t nodeID = 7; - float maxCurrent = 0.7; - - - DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id); - - - void SetAngle(float _angle); - void SetMaxCurrent(float _val); - void SetEnable(bool _enable); - - - // Communication protocol definitions - auto MakeProtocolDefinitions() - { - return make_protocol_member_list( - make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"), - make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"), - make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current") - ); - } - - -private: - CAN_HandleTypeDef* hcan; - uint8_t canBuf[8]; - CAN_TxHeaderTypeDef txHeader; - float minAngle = 0; - float maxAngle = 45; -}; - - -class DummyRobot -{ -public: - explicit DummyRobot(CAN_HandleTypeDef* _hcan); - ~DummyRobot(); - - - enum CommandMode - { - COMMAND_TARGET_POINT_SEQUENTIAL = 1, - COMMAND_TARGET_POINT_INTERRUPTABLE, - COMMAND_CONTINUES_TRAJECTORY - }; - - // This is the pose when power on. - const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0}; - const float DEFAULT_JOINT_SPEED = 30; - const float DEFAULT_JOINT_ACCELERATION = 50; - - DOF6Kinematic::Joint6D_t currentJoints = REST_POSE; - DOF6Kinematic::Joint6D_t initPose = REST_POSE; - DOF6Kinematic::Pose6D_t currentPose6D = {}; - volatile uint8_t jointsStateFlag = 0b00000000; - CommandMode commandMode = COMMAND_TARGET_POINT_SEQUENTIAL; - bool isStopped = false; - CtrlStepMotor* motorJ[7] = {nullptr}; - DummyHand* hand = {nullptr}; - - - float MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6); - float MoveL(float _x, float _y, float _z, float _a, float _b, float _c); - void MoveTrajectoryJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6); - void MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c); - void SetJointSpeed(float _speed); - void SetJointAcceleration(float _acc); - void UpdateJointAngles(); - void UpdateJointAnglesCallback(); - void UpdateJointPose6D(); - void Reboot(); - void SetEnable(bool _enable); - void CalibrateHomeOffset(); - void Homing(); - void Resting(); - bool IsMoving(); - void SetCommandMode(uint8_t _mode); - - - // Communication protocol definitions - auto MakeProtocolDefinitions() - { - return make_protocol_member_list( - make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset), - make_protocol_function("homing", *this, &DummyRobot::Homing), - make_protocol_function("resting", *this, &DummyRobot::Resting), - make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()), - make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()), - make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()), - make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()), - make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()), - make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()), - make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()), - make_protocol_object("hand", hand->MakeProtocolDefinitions()), - make_protocol_function("reboot", *this, &DummyRobot::Reboot), - make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"), - make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"), - make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"), - make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"), - make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"), - make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode") - - ); - } - - - class CommandHandler - { - public: - explicit CommandHandler(DummyRobot* _context) : context(_context) - { - commandFifo = osMessageQueueNew(16, 64, nullptr); - commandLifo = osMessageQueueNew(1, 64, nullptr); - } - - uint32_t Push(const std::string &_cmd); - std::string Pop(uint32_t timeout); - uint32_t ParseCommand(const std::string &_cmd); - uint32_t GetSpace(); - void ClearFifo(); - void EmergencyStop(); - void Resume(); - - - private: - DummyRobot* context; - osMessageQueueId_t commandFifo; - osMessageQueueId_t commandLifo; - char strBuffer[64]{}; - }; - CommandHandler commandHandler = CommandHandler(this); - - -private: - CAN_HandleTypeDef* hcan; - float jointSpeed = DEFAULT_JOINT_SPEED; - DOF6Kinematic* dof6Solver; - - - float MoveJoints(DOF6Kinematic::Joint6D_t _joints); -}; - - -#endif //REF_STM32F4_FW_DUMMY_ROBOT_H +#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H +#define REF_STM32F4_FW_DUMMY_ROBOT_H + +#include "algorithms/kinematic/6dof_kinematic.h" +#include "actuators/ctrl_step/ctrl_step.hpp" + +#define ALL 0 + +/* + | PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` | + | ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- | + | **Joint1** | 2 | 30 | 1000 | 80 | 200 | 250 | + | **Joint2** | 2 | 30 | 1000 | 80 | 200 | 200 | + | **Joint3** | 2 | 30 | 1500 | 80 | 200 | 250 | + | **Joint4** | 2 | 30 | 1000 | 80 | 200 | 250 | + | **Joint5** | 2 | 30 | 1000 | 80 | 200 | 250 | + | **Joint6** | 2 | 30 | 1000 | 80 | 200 | 250 | + */ + + +class DummyHand +{ +public: + uint8_t nodeID = 7; + float maxCurrent = 0.7; + + + DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id); + + + void SetAngle(float _angle); + void SetMaxCurrent(float _val); + void SetEnable(bool _enable); + + + // Communication protocol definitions + auto MakeProtocolDefinitions() + { + return make_protocol_member_list( + make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"), + make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"), + make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current") + ); + } + + +private: + CAN_HandleTypeDef* hcan; + uint8_t canBuf[8]; + CAN_TxHeaderTypeDef txHeader; + float minAngle = 0; + float maxAngle = 45; +}; + + +class DummyRobot +{ +public: + explicit DummyRobot(CAN_HandleTypeDef* _hcan); + ~DummyRobot(); + + + enum CommandMode + { + COMMAND_TARGET_POINT_SEQUENTIAL = 1, + COMMAND_TARGET_POINT_INTERRUPTABLE, + COMMAND_CONTINUES_TRAJECTORY, + COMMAND_MOTOR_TUNING + }; + + + class TuningHelper + { + public: + explicit TuningHelper(DummyRobot* _context) : context(_context) + { + } + + void SetTuningFlag(uint8_t _flag); + void Tick(uint32_t _timeMillis); + void SetFreqAndAmp(float _freq, float _amp); + + + // Communication protocol definitions + auto MakeProtocolDefinitions() + { + return make_protocol_member_list( + make_protocol_function("set_tuning_freq_amp", *this, + &TuningHelper::SetFreqAndAmp, "freq", "amp"), + make_protocol_function("set_tuning_flag", *this, + &TuningHelper::SetTuningFlag, "flag") + ); + } + + + private: + DummyRobot* context; + float time = 0; + uint8_t tuningFlag = 0; + float frequency = 1; + float amplitude = 1; + }; + TuningHelper tuningHelper = TuningHelper(this); + + + // This is the pose when power on. + const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0}; + const float DEFAULT_JOINT_SPEED = 30; // degree/s + const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = {150, 100, 200, 200, 200, 200}; + const float DEFAULT_JOINT_ACCELERATION_LOW = 30; // 0~100 + const float DEFAULT_JOINT_ACCELERATION_HIGH = 100; // 0~100 + const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE; + + + DOF6Kinematic::Joint6D_t currentJoints = REST_POSE; + DOF6Kinematic::Joint6D_t targetJoints = REST_POSE; + DOF6Kinematic::Joint6D_t initPose = REST_POSE; + DOF6Kinematic::Pose6D_t currentPose6D = {}; + volatile uint8_t jointsStateFlag = 0b00000000; + CommandMode commandMode = DEFAULT_COMMAND_MODE; + CtrlStepMotor* motorJ[7] = {nullptr}; + DummyHand* hand = {nullptr}; + + + void Init(); + bool MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6); + bool MoveL(float _x, float _y, float _z, float _a, float _b, float _c); + void MoveJoints(DOF6Kinematic::Joint6D_t _joints); + void SetJointSpeed(float _speed); + void SetJointAcceleration(float _acc); + void UpdateJointAngles(); + void UpdateJointAnglesCallback(); + void UpdateJointPose6D(); + void Reboot(); + void SetEnable(bool _enable); + void CalibrateHomeOffset(); + void Homing(); + void Resting(); + bool IsMoving(); + bool IsEnabled(); + void SetCommandMode(uint32_t _mode); + + + // Communication protocol definitions + auto MakeProtocolDefinitions() + { + return make_protocol_member_list( + make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset), + make_protocol_function("homing", *this, &DummyRobot::Homing), + make_protocol_function("resting", *this, &DummyRobot::Resting), + make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()), + make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()), + make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()), + make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()), + make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()), + make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()), + make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()), + make_protocol_object("hand", hand->MakeProtocolDefinitions()), + make_protocol_function("reboot", *this, &DummyRobot::Reboot), + make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"), + make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"), + make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"), + make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"), + make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"), + make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode"), + make_protocol_object("tuning", tuningHelper.MakeProtocolDefinitions()) + ); + } + + + class CommandHandler + { + public: + explicit CommandHandler(DummyRobot* _context) : context(_context) + { + commandFifo = osMessageQueueNew(16, 64, nullptr); + } + + uint32_t Push(const std::string &_cmd); + std::string Pop(uint32_t timeout); + uint32_t ParseCommand(const std::string &_cmd); + uint32_t GetSpace(); + void ClearFifo(); + void EmergencyStop(); + + + private: + DummyRobot* context; + osMessageQueueId_t commandFifo; + char strBuffer[64]{}; + }; + CommandHandler commandHandler = CommandHandler(this); + + +private: + CAN_HandleTypeDef* hcan; + float jointSpeed = DEFAULT_JOINT_SPEED; + float jointSpeedRatio = 1; + DOF6Kinematic::Joint6D_t dynamicJointSpeeds = {1, 1, 1, 1, 1, 1}; + DOF6Kinematic* dof6Solver; + bool isEnabled = false; +}; + + +#endif //REF_STM32F4_FW_DUMMY_ROBOT_H diff --git a/2.Firmware/Core-STM32F4-fw/UserApp/main.cpp b/2.Firmware/Core-STM32F4-fw/UserApp/main.cpp index 0fe51bf..bec441a 100644 --- a/2.Firmware/Core-STM32F4-fw/UserApp/main.cpp +++ b/2.Firmware/Core-STM32F4-fw/UserApp/main.cpp @@ -1,164 +1,183 @@ -#include "common_inc.h" - - -// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c) -SSD1306 oled(&hi2c0); -// On-board Sensor, used hi2c1 -MPU6050 mpu6050(&hi2c1); -// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14 -Timer timerCtrlLoop(&htim7, 200); -// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs -PWM pwm(21000, 21000); -// Robot instance -DummyRobot dummy(&hcan1); - - -/* Thread Definitions -----------------------------------------------------*/ -osThreadId_t controlLoopFixUpdateHandle; -void ThreadControlLoopFixUpdate(void* argument) -{ - for (;;) - { - // Suspended here until got Notification. - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - - switch (dummy.commandMode) - { - case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL: - case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE: - dummy.UpdateJointAngles(); - dummy.UpdateJointPose6D(); - break; - case DummyRobot::COMMAND_CONTINUES_TRAJECTORY: - // ToDo: handle trajectory, while update state at the mean time - dummy.UpdateJointPose6D(); - break; - } - } -} - - -osThreadId_t ControlLoopUpdateHandle; -void ThreadControlLoopUpdate(void* argument) -{ - for (;;) - { - dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever)); - } -} - - -osThreadId_t oledTaskHandle; -void ThreadOledUpdate(void* argument) -{ - uint32_t t = micros(); - char buf[16]; - char cmdModeNames[3][4] = {"SEQ", "INT", "TRJ"}; - - for (;;) - { - mpu6050.Update(true); - - oled.clearBuffer(); - - oled.setFont(u8g2_font_5x8_tr); - oled.setCursor(0, 10); - oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay); - oled.setCursor(85, 10); - oled.printf("| FPS:%lu", 1000000 / (micros() - t)); - t = micros(); - - oled.drawBox(0, 15, 128, 3); - oled.setCursor(0, 30); - oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d", - (int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]), - (int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]), - (int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5])); - - oled.drawBox(40, 35, 128, 24); - oled.setFont(u8g2_font_6x12_tr); - oled.setDrawColor(0); - oled.setCursor(42, 45); - oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X), - (int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z)); - oled.setCursor(42, 56); - oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A), - (int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C)); - oled.setDrawColor(1); - oled.setCursor(0, 45); - oled.printf("[XYZ]:"); - oled.setCursor(0, 56); - oled.printf("[ABC]:"); - - oled.setFont(u8g2_font_10x20_tr); - oled.setCursor(0, 78); - for (int i = 1; i <= 6; i++) - buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_'); - buf[6] = 0; - oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf); - - oled.sendBuffer(); - } -} - - -/* Timer Callbacks -------------------------------------------------------*/ -void OnTimer7Callback() -{ - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - - // Wake & invoke thread IMMEDIATELY. - vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); -} - - -/* Default Entry -------------------------------------------------------*/ -void Main(void) -{ - // Init all communication staff, include USB-CDC/VCP/UART/CAN etc. - InitCommunication(); - - // Init IMU. - do - { - mpu6050.Init(); - osDelay(100); - } while (!mpu6050.testConnection()); - mpu6050.InitFilter(200, 100, 50); - - // Init OLED 128x80. - oled.Init(); - pwm.Start(); - - // Init & Run User Threads. - const osThreadAttr_t controlLoopTask_attributes = { - .name = "ControlLoopFixUpdateTask", - .stack_size = 1000 * 4, - .priority = (osPriority_t) osPriorityRealtime, - }; - controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr, - &controlLoopTask_attributes); - - const osThreadAttr_t ControlLoopUpdateTask_attributes = { - .name = "ControlLoopUpdateTask", - .stack_size = 1000 * 4, - .priority = (osPriority_t) osPriorityNormal, - }; - ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr, - &ControlLoopUpdateTask_attributes); - - const osThreadAttr_t oledTask_attributes = { - .name = "OledTask", - .stack_size = 1000 * 4, - .priority = (osPriority_t) osPriorityNormal, // should >= Normal - }; - oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes); - - // Start Timer Callbacks. - timerCtrlLoop.SetCallback(OnTimer7Callback); - timerCtrlLoop.Start(); - - // System started, light switch-led up. - pwm.SetDuty(PWM::CH_A1, 0.5); -} +#include "common_inc.h" + + +// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c) +SSD1306 oled(&hi2c0); +// On-board Sensor, used hi2c1 +MPU6050 mpu6050(&hi2c1); +// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14 +Timer timerCtrlLoop(&htim7, 200); +// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs +PWM pwm(21000, 21000); +// Robot instance +DummyRobot dummy(&hcan1); + + +/* Thread Definitions -----------------------------------------------------*/ +osThreadId_t controlLoopFixUpdateHandle; +void ThreadControlLoopFixUpdate(void* argument) +{ + for (;;) + { + // Suspended here until got Notification. + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); + + if (dummy.IsEnabled()) + { + // Send control command to Motors & update Joint states + switch (dummy.commandMode) + { + case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL: + case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE: + case DummyRobot::COMMAND_CONTINUES_TRAJECTORY: + dummy.MoveJoints(dummy.targetJoints); + dummy.UpdateJointPose6D(); + break; + case DummyRobot::COMMAND_MOTOR_TUNING: + dummy.tuningHelper.Tick(10); + dummy.UpdateJointPose6D(); + break; + } + } else + { + // Just update Joint states + dummy.UpdateJointAngles(); + dummy.UpdateJointPose6D(); + } + } +} + + +osThreadId_t ControlLoopUpdateHandle; +void ThreadControlLoopUpdate(void* argument) +{ + for (;;) + { + dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever)); + } +} + + +osThreadId_t oledTaskHandle; +void ThreadOledUpdate(void* argument) +{ + uint32_t t = micros(); + char buf[16]; + char cmdModeNames[4][4] = {"SEQ", "INT", "TRJ", "TUN"}; + + for (;;) + { + mpu6050.Update(true); + + oled.clearBuffer(); + oled.setFont(u8g2_font_5x8_tr); + oled.setCursor(0, 10); + oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay); + oled.setCursor(85, 10); + oled.printf("| FPS:%lu", 1000000 / (micros() - t)); + t = micros(); + + oled.drawBox(0, 15, 128, 3); + oled.setCursor(0, 30); + oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d", + (int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]), + (int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]), + (int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5])); + + oled.drawBox(40, 35, 128, 24); + oled.setFont(u8g2_font_6x12_tr); + oled.setDrawColor(0); + oled.setCursor(42, 45); + oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X), + (int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z)); + oled.setCursor(42, 56); + oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A), + (int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C)); + oled.setDrawColor(1); + oled.setCursor(0, 45); + oled.printf("[XYZ]:"); + oled.setCursor(0, 56); + oled.printf("[ABC]:"); + + oled.setFont(u8g2_font_10x20_tr); + oled.setCursor(0, 78); + if (dummy.IsEnabled()) + { + for (int i = 1; i <= 6; i++) + buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_'); + buf[6] = 0; + oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf); + } else + { + oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], "======"); + } + + oled.sendBuffer(); + } +} + + +/* Timer Callbacks -------------------------------------------------------*/ +void OnTimer7Callback() +{ + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + + // Wake & invoke thread IMMEDIATELY. + vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + + +/* Default Entry -------------------------------------------------------*/ +void Main(void) +{ + // Init all communication staff, including USB-CDC/VCP/UART/CAN etc. + InitCommunication(); + + // Init Robot. + dummy.Init(); + + // Init IMU. + do + { + mpu6050.Init(); + osDelay(100); + } while (!mpu6050.testConnection()); + mpu6050.InitFilter(200, 100, 50); + + // Init OLED 128x80. + oled.Init(); + pwm.Start(); + + // Init & Run User Threads. + const osThreadAttr_t controlLoopTask_attributes = { + .name = "ControlLoopFixUpdateTask", + .stack_size = 2000, + .priority = (osPriority_t) osPriorityRealtime, + }; + controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr, + &controlLoopTask_attributes); + + const osThreadAttr_t ControlLoopUpdateTask_attributes = { + .name = "ControlLoopUpdateTask", + .stack_size = 2000, + .priority = (osPriority_t) osPriorityNormal, + }; + ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr, + &ControlLoopUpdateTask_attributes); + + const osThreadAttr_t oledTask_attributes = { + .name = "OledTask", + .stack_size = 2000, + .priority = (osPriority_t) osPriorityNormal, // should >= Normal + }; + oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes); + + // Start Timer Callbacks. + timerCtrlLoop.SetCallback(OnTimer7Callback); + timerCtrlLoop.Start(); + + // System started, light switch-led up. + Respond(*uart4StreamOutputPtr, "[sys] Heap remain: %d Bytes\n", xPortGetMinimumEverFreeHeapSize()); + pwm.SetDuty(PWM::CH_A1, 0.5); +} diff --git a/2.Firmware/Core-STM32F4-fw/UserApp/protocols/ascii_protocol.cpp b/2.Firmware/Core-STM32F4-fw/UserApp/protocols/ascii_protocol.cpp index 6338eac..179f066 100644 --- a/2.Firmware/Core-STM32F4-fw/UserApp/protocols/ascii_protocol.cpp +++ b/2.Firmware/Core-STM32F4-fw/UserApp/protocols/ascii_protocol.cpp @@ -1,123 +1,116 @@ -#include "common_inc.h" - -extern DummyRobot dummy; - - -void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) -{ - /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ - if (_cmd[0] == '!' || dummy.isStopped) - { - std::string s(_cmd); - if (s.find("STOP") != std::string::npos) - { - dummy.commandHandler.EmergencyStop(); - Respond(_responseChannel, "!!!Stopped!!!"); - } else if (s.find("RESUME") != std::string::npos) - { - dummy.commandHandler.Resume(); - Respond(_responseChannel, "Resumed"); - } - } else if (_cmd[0] == '#') - { - std::string s(_cmd); - if (s.find("GETJPOS") != std::string::npos) - { - Respond(*usbStreamOutputPtr, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f", - dummy.currentJoints.a[0], dummy.currentJoints.a[1], - dummy.currentJoints.a[2], dummy.currentJoints.a[3], - dummy.currentJoints.a[4], dummy.currentJoints.a[5]); - } else if (s.find("GETLPOS") != std::string::npos) - { - dummy.UpdateJointPose6D(); - Respond(*usbStreamOutputPtr, "POSE %.2f %.2f %.2f %.2f %.2f %.2f", - dummy.currentPose6D.X, dummy.currentPose6D.Y, - dummy.currentPose6D.Z, dummy.currentPose6D.A, - dummy.currentPose6D.B, dummy.currentPose6D.C); - } else if (s.find("CMDMODE") != std::string::npos) - { - int mode; - sscanf(_cmd, "CMDMODE %d", &mode); - dummy.SetCommandMode(mode); - Respond(*usbStreamOutputPtr, "Set command mode to [%d]", mode); - } else - Respond(*usbStreamOutputPtr, "ok"); - } else if (_cmd[0] == '>' || _cmd[0] == '@') - { - uint32_t freeSize = dummy.commandHandler.Push(_cmd); - Respond(_responseChannel, "%d", freeSize); - } - -/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ -} - - -void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) -{ - /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ - uint8_t argNum; - - if (_cmd[0] == '#') - { - std::string s(_cmd); - if (s.find("GETJPOS") != std::string::npos) - { - Respond(_responseChannel, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f", - dummy.currentJoints.a[0], dummy.currentJoints.a[1], dummy.currentJoints.a[2], - dummy.currentJoints.a[3], dummy.currentJoints.a[4], dummy.currentJoints.a[5]); - } else - Respond(_responseChannel, "ok"); - } else if (_cmd[0] == '>') - { - float joints[6]; - float speed; - - argNum = sscanf(_cmd, ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2, - joints + 3, joints + 4, joints + 5, &speed); - if (argNum == 6) - { - dummy.MoveJ(joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5]); - } else if (argNum == 7) - { - dummy.SetJointSpeed(speed); - dummy.MoveJ(joints[0], joints[1], joints[2], - joints[3], joints[4], joints[5]); - } - - while (dummy.IsMoving()) - osDelay(10); - Respond(_responseChannel, "ok"); - - } else if (_cmd[0] == '@') - { - float pose[6]; - float speed; - - argNum = sscanf(_cmd, "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2, - pose + 3, pose + 4, pose + 5, &speed); - if (argNum == 6) - { - dummy.MoveL(pose[0], pose[1], pose[2], - pose[3], pose[4], pose[5]); - } else if (argNum == 7) - { - dummy.SetJointSpeed(speed); - dummy.MoveL(pose[0], pose[1], pose[2], - pose[3], pose[4], pose[5]); - } - - while (dummy.IsMoving()) - osDelay(10); - Respond(_responseChannel, "ok"); - } -/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ -} - - -void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) -{ - /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ - -/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ +#include "common_inc.h" + +extern DummyRobot dummy; + + +void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) +{ + /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ + if (_cmd[0] == '!' || !dummy.IsEnabled()) + { + std::string s(_cmd); + if (s.find("STOP") != std::string::npos) + { + dummy.commandHandler.EmergencyStop(); + Respond(_responseChannel, "Stopped ok"); + } else if (s.find("START") != std::string::npos) + { + dummy.SetEnable(true); + Respond(_responseChannel, "Started ok"); + } else if (s.find("DISABLE") != std::string::npos) + { + dummy.SetEnable(false); + Respond(_responseChannel, "Disabled ok"); + } + } else if (_cmd[0] == '#') + { + std::string s(_cmd); + if (s.find("GETJPOS") != std::string::npos) + { + Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f", + dummy.currentJoints.a[0], dummy.currentJoints.a[1], + dummy.currentJoints.a[2], dummy.currentJoints.a[3], + dummy.currentJoints.a[4], dummy.currentJoints.a[5]); + } else if (s.find("GETLPOS") != std::string::npos) + { + dummy.UpdateJointPose6D(); + Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f", + dummy.currentPose6D.X, dummy.currentPose6D.Y, + dummy.currentPose6D.Z, dummy.currentPose6D.A, + dummy.currentPose6D.B, dummy.currentPose6D.C); + } else if (s.find("CMDMODE") != std::string::npos) + { + uint32_t mode; + sscanf(_cmd, "#CMDMODE %lu", &mode); + dummy.SetCommandMode(mode); + Respond(_responseChannel, "Set command mode to [%lu]", mode); + } else + Respond(_responseChannel, "ok"); + } else if (_cmd[0] == '>' || _cmd[0] == '@') + { + uint32_t freeSize = dummy.commandHandler.Push(_cmd); + Respond(_responseChannel, "%d", freeSize); + } + +/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ +} + + +void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) +{ + /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ + if (_cmd[0] == '!' || !dummy.IsEnabled()) + { + std::string s(_cmd); + if (s.find("STOP") != std::string::npos) + { + dummy.commandHandler.EmergencyStop(); + Respond(_responseChannel, "Stopped ok"); + } else if (s.find("START") != std::string::npos) + { + dummy.SetEnable(true); + Respond(_responseChannel, "Started ok"); + } else if (s.find("DISABLE") != std::string::npos) + { + dummy.SetEnable(false); + Respond(_responseChannel, "Disabled ok"); + } + } else if (_cmd[0] == '#') + { + std::string s(_cmd); + if (s.find("GETJPOS") != std::string::npos) + { + Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f", + dummy.currentJoints.a[0], dummy.currentJoints.a[1], + dummy.currentJoints.a[2], dummy.currentJoints.a[3], + dummy.currentJoints.a[4], dummy.currentJoints.a[5]); + } else if (s.find("GETLPOS") != std::string::npos) + { + dummy.UpdateJointPose6D(); + Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f", + dummy.currentPose6D.X, dummy.currentPose6D.Y, + dummy.currentPose6D.Z, dummy.currentPose6D.A, + dummy.currentPose6D.B, dummy.currentPose6D.C); + } else if (s.find("CMDMODE") != std::string::npos) + { + uint32_t mode; + sscanf(_cmd, "#CMDMODE %lu", &mode); + dummy.SetCommandMode(mode); + Respond(_responseChannel, "Set command mode to [%lu]", mode); + } else + Respond(_responseChannel, "ok"); + } else if (_cmd[0] == '>' || _cmd[0] == '@') + { + uint32_t freeSize = dummy.commandHandler.Push(_cmd); + Respond(_responseChannel, "%d", freeSize); + } +/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ +} + + +void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) +{ + /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ + +/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ } \ No newline at end of file diff --git a/2.Firmware/Core-STM32F4-fw/UserApp/protocols/cmd_protocol.cpp b/2.Firmware/Core-STM32F4-fw/UserApp/protocols/cmd_protocol.cpp index bb93970..affc607 100644 --- a/2.Firmware/Core-STM32F4-fw/UserApp/protocols/cmd_protocol.cpp +++ b/2.Firmware/Core-STM32F4-fw/UserApp/protocols/cmd_protocol.cpp @@ -1,44 +1,30 @@ - -#include "common_inc.h" - -/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/ -extern DummyRobot dummy; - -class HelperFunctions -{ -public: - /*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/ - int32_t TestFunction(int32_t delta) - { - static int cnt = 0; - return cnt += delta; - } - - void SaveConfigurationHelper() - {} - - void EraseConfigurationHelper() - {} - - float GetTemperatureHelper() - { return AdcGetChipTemperature(); } - - void SystemResetHelper() - { NVIC_SystemReset(); } - -} staticFunctions; - - -// Define options that intractable with "reftool". -static inline auto MakeObjTree() -{ - /*--------------- 3.Add Your Protocol Variables & Functions Here ----------------*/ - return make_protocol_member_list( - // Add Read-Only Variables - make_protocol_ro_property("serial_number", &serialNumber), - make_protocol_object("robot", dummy.MakeProtocolDefinitions()) - ); -} - - -COMMIT_PROTOCOL + +#include "common_inc.h" + +/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/ +extern DummyRobot dummy; + +class HelperFunctions +{ +public: + /*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/ + float GetTemperatureHelper() + { return AdcGetChipTemperature(); } + +} staticFunctions; + + +// Define options that intractable with "reftool". +static inline auto MakeObjTree() +{ + /*--------------- 3.Add Your Protocol Variables & Functions Here ----------------*/ + return make_protocol_member_list( + // Add Read-Only Variables + make_protocol_ro_property("serial_number", &serialNumber), + make_protocol_function("get_temperature", staticFunctions, &HelperFunctions::GetTemperatureHelper), + make_protocol_object("robot", dummy.MakeProtocolDefinitions()) + ); +} + + +COMMIT_PROTOCOL diff --git a/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/Ctrl-Step-fw.iml b/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/Ctrl-Step-fw.iml index f08604b..6d70257 100644 --- a/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/Ctrl-Step-fw.iml +++ b/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/Ctrl-Step-fw.iml @@ -1,2 +1,2 @@ - + \ No newline at end of file diff --git a/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/modules.xml b/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/modules.xml index 185ccab..0ba51c0 100644 --- a/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/modules.xml +++ b/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/.idea/modules.xml @@ -1,8 +1,8 @@ - - - - - - - + + + + + + + \ No newline at end of file