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