[Fw] Update REF protocols.

This commit is contained in:
peng-zhihui 2022-02-09 11:52:03 +08:00
parent f0e2c8da3a
commit fe1007c668
21 changed files with 3793 additions and 3705 deletions

View File

@ -1,2 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" /> <module classpath="CMake" type="CPP_MODULE" version="4" />

View File

@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<project version="4"> <project version="4">
<component name="ProjectModuleManager"> <component name="ProjectModuleManager">
<modules> <modules>
<module fileurl="file://$PROJECT_DIR$/.idea/REF-STM32F4.iml" filepath="$PROJECT_DIR$/.idea/REF-STM32F4.iml" /> <module fileurl="file://$PROJECT_DIR$/.idea/REF-STM32F4.iml" filepath="$PROJECT_DIR$/.idea/REF-STM32F4.iml" />
</modules> </modules>
</component> </component>
</project> </project>

View File

@ -1,95 +1,89 @@
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "communication.hpp" #include "communication.hpp"
#include "common_inc.h" #include "common_inc.h"
/* Private defines -----------------------------------------------------------*/ /* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/ /* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/ /* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/ /* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/
volatile bool endpointListValid = false; volatile bool endpointListValid = false;
/* Private function prototypes -----------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/ /* Function implementations --------------------------------------------------*/
// @brief Sends a line on the specified output. // @brief Sends a line on the specified output.
osThreadId_t commTaskHandle; osThreadId_t commTaskHandle;
const osThreadAttr_t commTask_attributes = { const osThreadAttr_t commTask_attributes = {
.name = "commTask", .name = "commTask",
.stack_size = 10000 * 4, .stack_size = 45000,
.priority = (osPriority_t) osPriorityNormal, .priority = (osPriority_t) osPriorityNormal,
}; };
void InitCommunication(void) void InitCommunication(void)
{ {
printf("\r\nHello, REF v%.1f Started!\r\n", CONFIG_FW_VERSION); // Start command handling thread
commTaskHandle = osThreadNew(CommunicationTask, nullptr, &commTask_attributes);
// Start command handling thread
commTaskHandle = osThreadNew(CommunicationTask, NULL, &commTask_attributes); while (!endpointListValid)
osDelay(1);
while (!endpointListValid) }
osDelay(1);
} extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
osThreadId_t usbIrqTaskHandle;
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
osThreadId_t usbIrqTaskHandle; void UsbDeferredInterruptTask(void* ctx)
{
void UsbDeferredInterruptTask(void* ctx) (void) ctx; // unused parameter
{
(void) ctx; // unused parameter for (;;)
{
for (;;) // Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
{ osStatus semaphore_status = osSemaphoreAcquire(sem_usb_irq, osWaitForever);
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler) if (semaphore_status == osOK)
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);
// We have a new incoming USB transmission: handle it // Let the irq (OTG_FS_IRQHandler) fire again.
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
// 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
// Thread to handle deffered processing of USB interrupt, and void CommunicationTask(void* ctx)
// read commands out of the UART DMA circular buffer {
void CommunicationTask(void* ctx) (void) ctx; // unused parameter
{
(void) ctx; // unused parameter CommitProtocol();
CommitProtocol(); // Allow main init to continue
endpointListValid = true;
// Allow main init to continue
endpointListValid = true; StartUartServer();
StartUsbServer();
StartUartServer(); StartCanServer(CAN1);
StartUsbServer(); StartCanServer(CAN2);
StartCanServer(CAN1);
StartCanServer(CAN2); for (;;)
{
for (;;) osDelay(1000); // nothing to do
{ }
osDelay(1000); // nothing to do }
}
} extern "C" {
int _write(int file, const char* data, int len);
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)
// @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);
#ifdef DEBUG_VIA_USB_SERIAL
usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); return len;
uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr); }
#endif
uart5StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
return len;
}

View File

@ -1,252 +1,250 @@
/* /*
* *
* Zero-config node ID negotiation * Zero-config node ID negotiation
* ------------------------------- * -------------------------------
* *
* A heartbeat message is a message with a 8 byte unique serial number as payload. * 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. * A regular message is any message that is not a heartbeat message.
* *
* All nodes MUST obey these four rules: * All nodes MUST obey these four rules:
* *
* a) At a given point in time, a node MUST consider a node ID taken (by others) * a) At a given point in time, a node MUST consider a node ID taken (by others)
* if any of the following is true: * if any of the following is true:
* - the node received a (not self-emitted) heartbeat message with that node ID * - the node received a (not self-emitted) heartbeat message with that node ID
* within the last second * within the last second
* - the node attempted and failed at sending a heartbeat message with that * - 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) * 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 * 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 * if, within the last second, it did not succeed in sending a heartbeat
* message with that node ID. * message with that node ID.
* *
* c) At a given point in time, a node MUST NOT send any heartbeat message with * c) At a given point in time, a node MUST NOT send any heartbeat message with
* a node ID that is taken. * a node ID that is taken.
* *
* d) At a given point in time, a node MUST NOT send any regular message with * d) At a given point in time, a node MUST NOT send any regular message with
* a node ID that is not self-assigned. * a node ID that is not self-assigned.
* *
* Hardware allocation * Hardware allocation
* ------------------- * -------------------
* RX FIFO0: * RX FIFO0:
* - filter bank 0: heartbeat messages * - filter bank 0: heartbeat messages
*/ */
#include "common_inc.h" #include "common_inc.h"
#include <stm32f4xx_hal.h> #include <stm32f4xx_hal.h>
#include <cmsis_os.h> #include <cmsis_os.h>
// defined in can.c // defined in can.c
extern CAN_HandleTypeDef hcan1; extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2; extern CAN_HandleTypeDef hcan2;
CAN_context can1Ctx; CAN_context can1Ctx;
CAN_context can2Ctx; CAN_context can2Ctx;
static CAN_context* ctxs = nullptr; static CAN_context* ctxs = nullptr;
static CAN_RxHeaderTypeDef headerRx; static CAN_RxHeaderTypeDef headerRx;
static uint8_t data[8]; static uint8_t data[8];
struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan) struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
{ {
if (hcan->Instance == CAN1) if (hcan->Instance == CAN1)
return &can1Ctx; return &can1Ctx;
else if (hcan->Instance == CAN2) else if (hcan->Instance == CAN2)
return &can2Ctx; return &can2Ctx;
else else
return nullptr; return nullptr;
} }
bool StartCanServer(CAN_TypeDef* hcan) bool StartCanServer(CAN_TypeDef* hcan)
{ {
if (hcan == CAN1) if (hcan == CAN1)
{ {
ctxs = &can1Ctx; ctxs = &can1Ctx;
ctxs->handle = &hcan1; ctxs->handle = &hcan1;
} else if (hcan == CAN2) } else if (hcan == CAN2)
{ {
ctxs = &can2Ctx; ctxs = &can2Ctx;
ctxs->handle = &hcan2; ctxs->handle = &hcan2;
} else } else
return false; // fail if none of the above checks matched return false; // fail if none of the above checks matched
HAL_StatusTypeDef status; HAL_StatusTypeDef status;
ctxs->node_id = 0; ctxs->node_id = 0;
ctxs->serial_number = serialNumber; ctxs->serial_number = serialNumber;
osSemaphoreDef(sem_send_heartbeat); osSemaphoreDef(sem_send_heartbeat);
ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat)); ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
//// Set up filter //// Set up filter
CAN_FilterTypeDef sFilterConfig = { CAN_FilterTypeDef sFilterConfig = {
.FilterIdHigh = 0x0000, .FilterIdHigh = 0x0000,
.FilterIdLow = 0x0000, .FilterIdLow = 0x0000,
.FilterMaskIdHigh = 0x0000, .FilterMaskIdHigh = 0x0000,
.FilterMaskIdLow = 0x0000, .FilterMaskIdLow = 0x0000,
.FilterFIFOAssignment = CAN_RX_FIFO0, .FilterFIFOAssignment = CAN_RX_FIFO0,
.FilterBank = 0, .FilterBank = 0,
.FilterMode = CAN_FILTERMODE_IDMASK, .FilterMode = CAN_FILTERMODE_IDMASK,
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters .FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
.FilterActivation = ENABLE, .FilterActivation = ENABLE,
.SlaveStartFilterBank = 0 .SlaveStartFilterBank = 0
}; };
status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig); status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig);
if (status != HAL_OK) if (status != HAL_OK)
return false; return false;
status = HAL_CAN_Start(ctxs->handle); status = HAL_CAN_Start(ctxs->handle);
if (status != HAL_OK) if (status != HAL_OK)
return false; return false;
status = HAL_CAN_ActivateNotification(ctxs->handle, status = HAL_CAN_ActivateNotification(ctxs->handle,
CAN_IT_TX_MAILBOX_EMPTY | CAN_IT_TX_MAILBOX_EMPTY |
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING |
/* we probably only want this */ /* we probably only want this */
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL | CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN | CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK | CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE | CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE | CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
CAN_IT_ERROR); CAN_IT_ERROR);
if (status != HAL_OK) if (status != HAL_OK)
return false; return false;
return true; return true;
} }
void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx) void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{ {
// CAN_context* ctx = get_can_ctx(hcan); // CAN_context* ctx = get_can_ctx(hcan);
// if (!ctx) return; // if (!ctx) return;
// ctx->tx_msg_cnt++; // ctx->tx_msg_cnt++;
if (hcan->Instance == CAN1) if (hcan->Instance == CAN1)
osSemaphoreRelease(sem_can1_tx); osSemaphoreRelease(sem_can1_tx);
else if (hcan->Instance == CAN2) else if (hcan->Instance == CAN2)
osSemaphoreRelease(sem_can2_tx); osSemaphoreRelease(sem_can2_tx);
} }
void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx) void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{ {
if (!get_can_ctx(hcan)) if (!get_can_ctx(hcan))
return; return;
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++; get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
} }
void tx_error(CAN_context* ctx, uint8_t mailbox_idx) void tx_error(CAN_context* ctx, uint8_t mailbox_idx)
{ {
} }
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 0); } { tx_complete_callback(hcan, 0); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 1); } { tx_complete_callback(hcan, 1); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 2); } { tx_complete_callback(hcan, 2); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 0); } { tx_aborted_callback(hcan, 0); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 1); } { tx_aborted_callback(hcan, 1); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 2); } { tx_aborted_callback(hcan, 2); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
{ {
CAN_context* ctx = get_can_ctx(hcan); CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return; if (!ctx) return;
ctx->received_msg_cnt++; ctx->received_msg_cnt++;
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data); HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data);
if (status != HAL_OK) if (status != HAL_OK)
{ {
ctx->unexpected_errors++; ctx->unexpected_errors++;
return; return;
} }
OnCanMessage(ctx, &headerRx, data); OnCanMessage(ctx, &headerRx, data);
} }
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; } { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; } { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; } { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; } { if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; } { if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan) void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
{ {
//__asm volatile ("bkpt"); //__asm volatile ("bkpt");
CAN_context* ctx = get_can_ctx(hcan); CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return; if (!ctx) return;
volatile uint32_t original_error = hcan->ErrorCode; volatile uint32_t original_error = hcan->ErrorCode;
(void) original_error; (void) original_error;
// handle transmit errors in all three mailboxes // handle transmit errors in all three mailboxes
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0) if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0)
{ {
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ); SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0; hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0) } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0)
{ {
tx_error(ctx, 0); tx_error(ctx, 0);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0; hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
} }
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1) if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1)
{ {
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ); SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1; hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1) } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1)
{ {
tx_error(ctx, 1); tx_error(ctx, 1);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1; hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
} }
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2) if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2)
{ {
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ); SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2; hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2) } else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2)
{ {
tx_error(ctx, 2); tx_error(ctx, 2);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG; hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK; hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2; hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
} }
if (hcan->ErrorCode) if (hcan->ErrorCode)
ctx->unexpected_errors++; ctx->unexpected_errors++;
} }
void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader) void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
{ {
osStatus semaphore_status; osStatus semaphore_status;
if (canCtx->handle->Instance == CAN1) if (canCtx->handle->Instance == CAN1)
semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever); semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
else if (canCtx->handle->Instance == CAN2) else if (canCtx->handle->Instance == CAN2)
semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever); semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
else else
return; return;
if (semaphore_status == osOK) if (semaphore_status == osOK)
{ HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox); }
}
}

View File

@ -1,206 +1,206 @@
#include "common_inc.h" #include "common_inc.h"
#include "interface_uart.hpp" #include "interface_uart.hpp"
#include "ascii_processor.hpp" #include "ascii_processor.hpp"
#include "fibre/protocol.hpp" #include "fibre/protocol.hpp"
#include "usart.h" #include "usart.h"
#define UART_TX_BUFFER_SIZE 64 #define UART_TX_BUFFER_SIZE 64
#define UART_RX_BUFFER_SIZE 64 #define UART_RX_BUFFER_SIZE 64
// DMA open loop continous circular buffer // DMA open loop continous circular buffer
// 1ms delay periodic, chase DMA ptr around // 1ms delay periodic, chase DMA ptr around
static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE]; static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE];
static uint32_t dma_last_rcv_idx[2]; static uint32_t dma_last_rcv_idx[2];
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable // FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
// static thread_local uint32_t deadline_ms = 0; // static thread_local uint32_t deadline_ms = 0;
osThreadId_t uartServerTaskHandle; osThreadId_t uartServerTaskHandle;
class UART4Sender : public StreamSink class UART4Sender : public StreamSink
{ {
public: public:
UART4Sender() UART4Sender()
{ {
channelType = CHANNEL_TYPE_UART4; channelType = CHANNEL_TYPE_UART4;
} }
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
{ {
// Loop to ensure all bytes get sent // Loop to ensure all bytes get sent
while (length) while (length)
{ {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready // wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data // TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1; return -1;
// transmit chunk // transmit chunk
memcpy(tx_buf_, buffer, chunk); memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1; return -1;
buffer += chunk; buffer += chunk;
length -= chunk; length -= chunk;
if (processed_bytes) if (processed_bytes)
*processed_bytes += chunk; *processed_bytes += chunk;
} }
return 0; return 0;
} }
size_t get_free_space() override size_t get_free_space() override
{ return SIZE_MAX; } { return SIZE_MAX; }
private: private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_stream_output; } uart4_stream_output;
class UART5Sender : public StreamSink class UART5Sender : public StreamSink
{ {
public: public:
UART5Sender() UART5Sender()
{ {
channelType = CHANNEL_TYPE_UART5; channelType = CHANNEL_TYPE_UART5;
} }
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
{ {
// Loop to ensure all bytes get sent // Loop to ensure all bytes get sent
while (length) while (length)
{ {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready // wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data // TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1; return -1;
// transmit chunk // transmit chunk
memcpy(tx_buf_, buffer, chunk); memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK) if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK)
return -1; return -1;
buffer += chunk; buffer += chunk;
length -= chunk; length -= chunk;
if (processed_bytes) if (processed_bytes)
*processed_bytes += chunk; *processed_bytes += chunk;
} }
return 0; return 0;
} }
size_t get_free_space() override size_t get_free_space() override
{ return SIZE_MAX; } { return SIZE_MAX; }
private: private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart5_stream_output; } uart5_stream_output;
StreamSink* uart4StreamOutputPtr = &uart4_stream_output; StreamSink* uart4StreamOutputPtr = &uart4_stream_output;
StreamBasedPacketSink uart4_packet_output(uart4_stream_output); StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
StreamToPacketSegmenter uart4_stream_input(uart4_channel); StreamToPacketSegmenter uart4_stream_input(uart4_channel);
StreamSink* uart5StreamOutputPtr = &uart5_stream_output; StreamSink* uart5StreamOutputPtr = &uart5_stream_output;
StreamBasedPacketSink uart5_packet_output(uart5_stream_output); StreamBasedPacketSink uart5_packet_output(uart5_stream_output);
BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output); BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output);
StreamToPacketSegmenter uart5_stream_input(uart5_channel); StreamToPacketSegmenter uart5_stream_input(uart5_channel);
static void UartServerTask(void* ctx) static void UartServerTask(void* ctx)
{ {
(void) ctx; (void) ctx;
for (;;) for (;;)
{ {
// Check for UART errors and restart recieve DMA transfer if required // Check for UART errors and restart recieve DMA transfer if required
if (huart4.ErrorCode != HAL_UART_ERROR_NONE) if (huart4.ErrorCode != HAL_UART_ERROR_NONE)
{ {
HAL_UART_AbortReceive(&huart4); HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0])); 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 // Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap) // Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx[0]) if (new_rcv_idx < dma_last_rcv_idx[0])
{ {
uart4_stream_input.process_bytes(dma_rx_buffer[0] + 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], UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0],
nullptr); // TODO: use process_all nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0], 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); UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output);
dma_last_rcv_idx[0] = 0; dma_last_rcv_idx[0] = 0;
} }
if (new_rcv_idx > dma_last_rcv_idx[0]) if (new_rcv_idx > dma_last_rcv_idx[0])
{ {
uart4_stream_input.process_bytes(dma_rx_buffer[0] + 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], new_rcv_idx - dma_last_rcv_idx[0],
nullptr); // TODO: use process_all nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0], ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output); new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output);
dma_last_rcv_idx[0] = new_rcv_idx; dma_last_rcv_idx[0] = new_rcv_idx;
} }
// Check for UART errors and restart recieve DMA transfer if required // Check for UART errors and restart recieve DMA transfer if required
if (huart5.ErrorCode != HAL_UART_ERROR_NONE) if (huart5.ErrorCode != HAL_UART_ERROR_NONE)
{ {
HAL_UART_AbortReceive(&huart5); HAL_UART_AbortReceive(&huart5);
HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1])); 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 // Fetch the circular buffer "write pointer", where it would write next
new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR; new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap) // Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx[1]) if (new_rcv_idx < dma_last_rcv_idx[1])
{ {
uart4_stream_input.process_bytes(dma_rx_buffer[1] + 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], UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1],
nullptr); // TODO: use process_all nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1], 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); UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output);
dma_last_rcv_idx[1] = 0; dma_last_rcv_idx[1] = 0;
} }
if (new_rcv_idx > dma_last_rcv_idx[1]) if (new_rcv_idx > dma_last_rcv_idx[1])
{ {
uart4_stream_input.process_bytes(dma_rx_buffer[1] + 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], new_rcv_idx - dma_last_rcv_idx[1],
nullptr); // TODO: use process_all nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1], ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output); new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output);
dma_last_rcv_idx[1] = new_rcv_idx; dma_last_rcv_idx[1] = new_rcv_idx;
} }
osDelay(1); osDelay(1);
}; };
} }
const osThreadAttr_t uartServerTask_attributes = { const osThreadAttr_t uartServerTask_attributes = {
.name = "UartServerTask", .name = "UartServerTask",
.stack_size = 1000 * 4, .stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal, .priority = (osPriority_t) osPriorityNormal,
}; };
void StartUartServer() void StartUartServer()
{ {
// DMA is set up to recieve in a circular buffer forever. // DMA is set up to receive in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read // 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 // 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])); 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; 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])); 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; dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
// Start UART communication thread // Start UART communication thread
uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes); uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes);
} }
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart)
{ {
if (huart->Instance == UART4) if (huart->Instance == UART4)
osSemaphoreRelease(sem_uart4_dma); osSemaphoreRelease(sem_uart4_dma);
else if (huart->Instance == UART5) else if (huart->Instance == UART5)
osSemaphoreRelease(sem_uart5_dma); osSemaphoreRelease(sem_uart5_dma);
} }

View File

@ -1,190 +1,190 @@
#include "common_inc.h" #include "common_inc.h"
#include "ascii_processor.hpp" #include "ascii_processor.hpp"
#include "usbd_cdc.h" #include "usbd_cdc.h"
#include "usbd_cdc_if.h" #include "usbd_cdc_if.h"
#include "usb_device.h" #include "usb_device.h"
#include "interface_usb.hpp" #include "interface_usb.hpp"
osThreadId_t usbServerTaskHandle; osThreadId_t usbServerTaskHandle;
USBStats_t usb_stats_ = {0}; USBStats_t usb_stats_ = {0};
class USBSender : public PacketSink class USBSender : public PacketSink
{ {
public: public:
USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx) USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx)
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) : endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx)
{} {}
int process_packet(const uint8_t *buffer, size_t length) override int process_packet(const uint8_t *buffer, size_t length) override
{ {
// cannot send partial packets // cannot send partial packets
if (length > USB_TX_DATA_SIZE) if (length > USB_TX_DATA_SIZE)
return -1; return -1;
// wait for USB interface to become ready // wait for USB interface to become ready
if (osSemaphoreAcquire(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) 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 // 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 // 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. // TX buffer if this wait times out. The implication is that the channel is no longer lossless.
// TODO: handle endpoint reset properly // TODO: handle endpoint reset properly
usb_stats_.tx_overrun_cnt++; usb_stats_.tx_overrun_cnt++;
} }
// transmit packet // transmit packet
uint8_t status = CDC_Transmit_FS(const_cast<uint8_t *>(buffer), length, endpoint_pair_); uint8_t status = CDC_Transmit_FS(const_cast<uint8_t *>(buffer), length, endpoint_pair_);
if (status != USBD_OK) if (status != USBD_OK)
{ {
osSemaphoreRelease(sem_usb_tx_); osSemaphoreRelease(sem_usb_tx_);
return -1; return -1;
} }
usb_stats_.tx_cnt++; usb_stats_.tx_cnt++;
return 0; return 0;
} }
private: private:
uint8_t endpoint_pair_; uint8_t endpoint_pair_;
const osSemaphoreId &sem_usb_tx_; const osSemaphoreId &sem_usb_tx_;
}; };
// Note we could have independent semaphores here to allow concurrent transmission // 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_cdc(CDC_OUT_EP, sem_usb_tx);
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx); USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
class TreatPacketSinkAsStreamSink : public StreamSink class TreatPacketSinkAsStreamSink : public StreamSink
{ {
public: public:
TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output) TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output)
{ {
channelType = CHANNEL_TYPE_USB; channelType = CHANNEL_TYPE_USB;
} }
int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes) int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes)
{ {
// Loop to ensure all bytes get sent // Loop to ensure all bytes get sent
while (length) while (length)
{ {
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
if (output_.process_packet(buffer, length) != 0) if (output_.process_packet(buffer, length) != 0)
return -1; return -1;
buffer += chunk; buffer += chunk;
length -= chunk; length -= chunk;
if (processed_bytes) if (processed_bytes)
*processed_bytes += chunk; *processed_bytes += chunk;
} }
return 0; return 0;
} }
size_t get_free_space() size_t get_free_space()
{ return SIZE_MAX; } { return SIZE_MAX; }
private: private:
PacketSink &output_; PacketSink &output_;
} usb_stream_output(usb_packet_output_cdc); } 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) // This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed)
// TODO: less spaghetti code // TODO: less spaghetti code
StreamSink *usbStreamOutputPtr = &usb_stream_output; StreamSink *usbStreamOutputPtr = &usb_stream_output;
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native); BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
struct USBInterface struct USBInterface
{ {
uint8_t *rx_buf = nullptr; uint8_t *rx_buf = nullptr;
uint32_t rx_len = 0; uint32_t rx_len = 0;
bool data_pending = false; bool data_pending = false;
uint8_t out_ep; uint8_t out_ep;
uint8_t in_ep; uint8_t in_ep;
USBSender &usb_sender; USBSender &usb_sender;
}; };
// Note: statics make this less modular. // 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 // Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread
static USBInterface CDC_interface = { static USBInterface CDC_interface = {
.rx_buf = nullptr, .rx_buf = nullptr,
.rx_len = 0, .rx_len = 0,
.data_pending = false, .data_pending = false,
.out_ep = CDC_OUT_EP, .out_ep = CDC_OUT_EP,
.in_ep = CDC_IN_EP, .in_ep = CDC_IN_EP,
.usb_sender = usb_packet_output_cdc, .usb_sender = usb_packet_output_cdc,
}; };
static USBInterface ODrive_interface = { static USBInterface ODrive_interface = {
.rx_buf = nullptr, .rx_buf = nullptr,
.rx_len = 0, .rx_len = 0,
.data_pending = false, .data_pending = false,
.out_ep = ODRIVE_OUT_EP, .out_ep = ODRIVE_OUT_EP,
.in_ep = ODRIVE_IN_EP, .in_ep = ODRIVE_IN_EP,
.usb_sender = usb_packet_output_native, .usb_sender = usb_packet_output_native,
}; };
static void UsbServerTask(void *ctx) static void UsbServerTask(void *ctx)
{ {
(void) ctx; (void) ctx;
for (;;) for (;;)
{ {
// const uint32_t usb_check_timeout = 1; // ms // const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever); osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever);
if (sem_stat == osOK) if (sem_stat == osOK)
{ {
usb_stats_.rx_cnt++; usb_stats_.rx_cnt++;
// CDC Interface // CDC Interface
if (CDC_interface.data_pending) if (CDC_interface.data_pending)
{ {
CDC_interface.data_pending = false; CDC_interface.data_pending = false;
ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output); 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 USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet
} }
// Native Interface // Native Interface
if (ODrive_interface.data_pending) if (ODrive_interface.data_pending)
{ {
ODrive_interface.data_pending = false; ODrive_interface.data_pending = false;
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len); usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet
} }
} }
} }
} }
// Called from CDC_Receive_FS callback function, this allows the communication // Called from CDC_Receive_FS callback function, this allows the communication
// thread to handle the incoming data // thread to handle the incoming data
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair)
{ {
USBInterface *usb_iface; USBInterface *usb_iface;
if (endpoint_pair == CDC_interface.out_ep) if (endpoint_pair == CDC_interface.out_ep)
{ {
usb_iface = &CDC_interface; usb_iface = &CDC_interface;
} else if (endpoint_pair == ODrive_interface.out_ep) } else if (endpoint_pair == ODrive_interface.out_ep)
{ {
usb_iface = &ODrive_interface; usb_iface = &ODrive_interface;
} else } else
{ {
return; return;
} }
// We don't allow the next USB packet until the previous one has been processed completely. // 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. // 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_buf = buf;
usb_iface->rx_len = len; usb_iface->rx_len = len;
usb_iface->data_pending = true; usb_iface->data_pending = true;
osSemaphoreRelease(sem_usb_rx); osSemaphoreRelease(sem_usb_rx);
} }
const osThreadAttr_t usbServerTask_attributes = { const osThreadAttr_t usbServerTask_attributes = {
.name = "UsbServerTask", .name = "UsbServerTask",
.stack_size = 512 * 4, .stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal, .priority = (osPriority_t) osPriorityNormal,
}; };
void StartUsbServer() void StartUsbServer()
{ {
// Start USB communication thread // Start USB communication thread
usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes); usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes);
} }

View File

@ -1,114 +1,113 @@
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! #THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1) set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.19) cmake_minimum_required(VERSION 3.19)
# specify cross compilers and tools # specify cross compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc) set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++) set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar) set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_OBJCOPY arm-none-eabi-objcopy) set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump) set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size) set(SIZE arm-none-eabi-size)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# project settings # project settings
project(Core-STM32F4-fw C CXX ASM) project(Core-STM32F4-fw C CXX ASM)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD 11)
# for use printf & scanf with float # for use printf & scanf with float
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float") set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
#Uncomment for hardware floating point #Uncomment for hardware floating point
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
#Uncomment for software floating point #Uncomment for software floating point
#add_compile_options(-mfloat-abi=soft) #add_compile_options(-mfloat-abi=soft)
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
# uncomment to mitigate c++17 absolute addresses warnings # uncomment to mitigate c++17 absolute addresses warnings
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed") message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast) add_compile_options(-Ofast)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
message(STATUS "Maximum optimization for speed, debug info included") message(STATUS "Maximum optimization for speed, debug info included")
add_compile_options(-Ofast -g) add_compile_options(-Ofast -g)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
message(STATUS "Maximum optimization for size") message(STATUS "Maximum optimization for size")
add_compile_options(-Os) add_compile_options(-Os)
else () else ()
message(STATUS "Minimal optimization, debug info included") message(STATUS "Minimal optimization, debug info included")
add_compile_options(-Og -g) add_compile_options(-Og -g)
endif () endif ()
include_directories( include_directories(
Core/Inc Core/Inc
Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_Driver/Inc
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
Drivers/CMSIS/Device/ST/STM32F4xx/Include Drivers/CMSIS/Device/ST/STM32F4xx/Include
Drivers/CMSIS/Include Drivers/CMSIS/Include
Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/include
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Core/Inc
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
USB_DEVICE/App USB_DEVICE/App
USB_DEVICE/Target USB_DEVICE/Target
3rdParty/fibre/cpp/include 3rdParty/fibre/cpp/include
3rdParty/u8g2 3rdParty/u8g2
3rdParty/u8g2/cpp 3rdParty/u8g2/cpp
Bsp Bsp
Bsp/imu Bsp/imu
Bsp/imu/filters Bsp/imu/filters
Bsp/communication Bsp/communication
Bsp/memory Bsp/memory
Bsp/utils Bsp/utils
Bsp/gpio Bsp/gpio
Bsp/utils/software_i2c Bsp/utils/software_i2c
Bsp/utils/arm_math Bsp/utils/arm_math
Robot Robot
UserApp UserApp
) )
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP) add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
add_definitions(-DDEBUG_VIA_USB_SERIAL)
file(GLOB_RECURSE SOURCES
file(GLOB_RECURSE SOURCES "startup/*.*"
"startup/*.*" "Drivers/*.*"
"Drivers/*.*" "Core/*.*"
"Core/*.*" "UserApp/*.*"
"UserApp/*.*" "3rdParty/*.*"
"3rdParty/*.*" "Middlewares/*.*"
"Middlewares/*.*" "USB_DEVICE/*.*"
"USB_DEVICE/*.*" "Robot/*.*"
"Robot/*.*" "Bsp/*.*"
"Bsp/*.*" )
)
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
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(-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(-mcpu=cortex-m4 -mthumb -mthumb-interwork) add_link_options(-T ${LINKER_SCRIPT})
add_link_options(-T ${LINKER_SCRIPT})
link_directories("Drivers/CMSIS/Lib")
link_directories("Drivers/CMSIS/Lib") link_libraries("arm_cortexM4lf_math.a")
link_libraries("arm_cortexM4lf_math.a")
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE} COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE} COMMENT "Building ${HEX_FILE}
COMMENT "Building ${HEX_FILE} Building ${BIN_FILE}")
Building ${BIN_FILE}")

View File

@ -1,114 +1,113 @@
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! #THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1) set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.19) cmake_minimum_required(VERSION 3.19)
# specify cross compilers and tools # specify cross compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc) set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++) set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar) set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_OBJCOPY arm-none-eabi-objcopy) set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump) set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size) set(SIZE arm-none-eabi-size)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# project settings # project settings
project(Core-STM32F4-fw C CXX ASM) project(Core-STM32F4-fw C CXX ASM)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD 11)
# for use printf & scanf with float # for use printf & scanf with float
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float") set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
#Uncomment for hardware floating point #Uncomment for hardware floating point
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
#Uncomment for software floating point #Uncomment for software floating point
#add_compile_options(-mfloat-abi=soft) #add_compile_options(-mfloat-abi=soft)
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
# uncomment to mitigate c++17 absolute addresses warnings # uncomment to mitigate c++17 absolute addresses warnings
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed") message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast) add_compile_options(-Ofast)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
message(STATUS "Maximum optimization for speed, debug info included") message(STATUS "Maximum optimization for speed, debug info included")
add_compile_options(-Ofast -g) add_compile_options(-Ofast -g)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
message(STATUS "Maximum optimization for size") message(STATUS "Maximum optimization for size")
add_compile_options(-Os) add_compile_options(-Os)
else () else ()
message(STATUS "Minimal optimization, debug info included") message(STATUS "Minimal optimization, debug info included")
add_compile_options(-Og -g) add_compile_options(-Og -g)
endif () endif ()
include_directories( include_directories(
Core/Inc Core/Inc
Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_Driver/Inc
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
Drivers/CMSIS/Device/ST/STM32F4xx/Include Drivers/CMSIS/Device/ST/STM32F4xx/Include
Drivers/CMSIS/Include Drivers/CMSIS/Include
Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/include
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Core/Inc
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
USB_DEVICE/App USB_DEVICE/App
USB_DEVICE/Target USB_DEVICE/Target
3rdParty/fibre/cpp/include 3rdParty/fibre/cpp/include
3rdParty/u8g2 3rdParty/u8g2
3rdParty/u8g2/cpp 3rdParty/u8g2/cpp
Bsp Bsp
Bsp/imu Bsp/imu
Bsp/imu/filters Bsp/imu/filters
Bsp/communication Bsp/communication
Bsp/memory Bsp/memory
Bsp/utils Bsp/utils
Bsp/gpio Bsp/gpio
Bsp/utils/software_i2c Bsp/utils/software_i2c
Bsp/utils/arm_math Bsp/utils/arm_math
Robot Robot
UserApp UserApp
) )
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP) add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
# add_definitions(-DDEBUG_VIA_USB_SERIAL)
file(GLOB_RECURSE SOURCES
file(GLOB_RECURSE SOURCES "startup/*.*"
"startup/*.*" "Drivers/*.*"
"Drivers/*.*" "Core/*.*"
"Core/*.*" "UserApp/*.*"
"UserApp/*.*" "3rdParty/*.*"
"3rdParty/*.*" "Middlewares/*.*"
"Middlewares/*.*" "USB_DEVICE/*.*"
"USB_DEVICE/*.*" "Robot/*.*"
"Robot/*.*" "Bsp/*.*"
"Bsp/*.*" )
)
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
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(-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(-mcpu=cortex-m4 -mthumb -mthumb-interwork) add_link_options(-T ${LINKER_SCRIPT})
add_link_options(-T ${LINKER_SCRIPT})
link_directories("Drivers/CMSIS/Lib")
link_directories("Drivers/CMSIS/Lib") link_libraries("arm_cortexM4lf_math.a")
link_libraries("arm_cortexM4lf_math.a")
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE} COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE} COMMENT "Building ${HEX_FILE}
COMMENT "Building ${HEX_FILE} Building ${BIN_FILE}")
Building ${BIN_FILE}")

View File

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

File diff suppressed because it is too large Load Diff

View File

@ -1,348 +1,317 @@
#include "ctrl_step.hpp" #include "ctrl_step.hpp"
#include "communication.hpp" #include "communication.hpp"
CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse, CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse,
uint8_t _reduction, float _minAngle, float _maxAngle) : uint8_t _reduction, float _angleLimitMin, float _angleLimitMax) :
nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction), nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
angleLimitMin(_minAngle), angleLimitMax(_maxAngle) angleLimitMin(_angleLimitMin), angleLimitMax(_angleLimitMax)
{ {
txHeader = txHeader =
{ {
.StdId = 0, .StdId = 0,
.ExtId = 0, .ExtId = 0,
.IDE = CAN_ID_STD, .IDE = CAN_ID_STD,
.RTR = CAN_RTR_DATA, .RTR = CAN_RTR_DATA,
.DLC = 8, .DLC = 8,
.TransmitGlobalTime = DISABLE .TransmitGlobalTime = DISABLE
}; };
} }
void CtrlStepMotor::SetEnable(bool _enable) void CtrlStepMotor::SetEnable(bool _enable)
{ {
state = _enable ? FINISH : STOP; state = _enable ? FINISH : STOP;
uint8_t mode = 0x01; uint8_t mode = 0x01;
txHeader.StdId = nodeID << 7 | mode; txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes // Int to Bytes
uint32_t val = _enable ? 1 : 0; uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val; auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i); canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} }
void CtrlStepMotor::DoCalibration() void CtrlStepMotor::DoCalibration()
{ {
uint8_t mode = 0x02; uint8_t mode = 0x02;
txHeader.StdId = nodeID << 7 | mode; txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} }
void CtrlStepMotor::SetCurrentSetPoint(float _val) void CtrlStepMotor::SetCurrentSetPoint(float _val)
{ {
state = RUNNING; state = RUNNING;
uint8_t mode = 0x03; uint8_t mode = 0x03;
txHeader.StdId = nodeID << 7 | mode; txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes // Float to Bytes
auto* b = (unsigned char*) &_val; auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i); canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} }
void CtrlStepMotor::SetVelocitySetPoint(float _val) void CtrlStepMotor::SetVelocitySetPoint(float _val)
{ {
state = RUNNING; state = RUNNING;
uint8_t mode = 0x04; uint8_t mode = 0x04;
txHeader.StdId = nodeID << 7 | mode; txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes // Float to Bytes
auto* b = (unsigned char*) &_val; auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i); canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} }
void CtrlStepMotor::SetPositionSetPoint(float _val) void CtrlStepMotor::SetPositionSetPoint(float _val)
{ {
state = RUNNING; uint8_t mode = 0x05;
txHeader.StdId = nodeID << 7 | mode;
uint8_t mode = 0x05;
txHeader.StdId = nodeID << 7 | mode; // Float to Bytes
auto* b = (unsigned char*) &_val;
// Float to Bytes for (int i = 0; i < 4; i++)
auto* b = (unsigned char*) &_val; canBuf[i] = *(b + i);
for (int i = 0; i < 4; i++) canBuf[4] = 1; // Need ACK
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); }
}
void CtrlStepMotor::SetPositionWithVelocityLimit(float _pos, float _vel)
void CtrlStepMotor::SetPositionWithTime(float _pos, float _time) {
{ uint8_t mode = 0x07;
state = RUNNING; txHeader.StdId = nodeID << 7 | mode;
uint8_t mode = 0x06; // Float to Bytes
txHeader.StdId = nodeID << 7 | mode; auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++)
// Float to Bytes canBuf[i] = *(b + i);
auto* b = (unsigned char*) &_pos; b = (unsigned char*) &_vel;
for (int i = 0; i < 4; i++) for (int i = 4; i < 8; i++)
canBuf[i] = *(b + i); canBuf[i] = *(b + i - 4);
b = (unsigned char*) &_time; CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
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;
void CtrlStepMotor::AddTrajectoryPoint(float _pos, float _vel)
{ // Int to Bytes
uint8_t mode = 0x07; auto* b = (unsigned char*) &_id;
txHeader.StdId = nodeID << 7 | mode; for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
// Float to Bytes canBuf[4] = 1; // Need save to EEPROM or not
auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++) CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
canBuf[i] = *(b + i); }
b = (unsigned char*) &_vel;
for (int i = 4; i < 8; i++) void CtrlStepMotor::SetCurrentLimit(float _val)
canBuf[i] = *(b + i - 4); {
uint8_t mode = 0x12;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); txHeader.StdId = nodeID << 7 | mode;
}
// Float to Bytes
auto* b = (unsigned char*) &_val;
void CtrlStepMotor::SetNodeID(uint32_t _id) for (int i = 0; i < 4; i++)
{ canBuf[i] = *(b + i);
uint8_t mode = 0x11; canBuf[4] = 1; // Need save to EEPROM or not
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
// Int to Bytes }
auto* b = (unsigned char*) &_id;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i); void CtrlStepMotor::SetVelocityLimit(float _val)
{
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); uint8_t mode = 0x13;
} txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
void CtrlStepMotor::SetCurrentLimit(float _val) auto* b = (unsigned char*) &_val;
{ for (int i = 0; i < 4; i++)
uint8_t mode = 0x12; canBuf[i] = *(b + i);
txHeader.StdId = nodeID << 7 | mode; canBuf[4] = 1; // Need save to EEPROM or not
// Float to Bytes CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
auto* b = (unsigned char*) &_val; }
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
void CtrlStepMotor::SetAcceleration(float _val)
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); {
} uint8_t mode = 0x14;
txHeader.StdId = nodeID << 7 | mode;
void CtrlStepMotor::SetVelocityLimit(float _val) // Float to Bytes
{ auto* b = (unsigned char*) &_val;
uint8_t mode = 0x13; for (int i = 0; i < 4; i++)
txHeader.StdId = nodeID << 7 | mode; canBuf[i] = *(b + i);
canBuf[4] = 0; // Need save to EEPROM or not
// Float to Bytes
auto* b = (unsigned char*) &_val; CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
for (int i = 0; i < 4; i++) }
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); void CtrlStepMotor::ApplyPositionAsHome()
} {
uint8_t mode = 0x15;
txHeader.StdId = nodeID << 7 | mode;
void CtrlStepMotor::SetAcceleration(float _val, bool _storeToMem)
{ CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
uint8_t mode = 0x14; }
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes void CtrlStepMotor::SetEnableOnBoot(bool _enable)
auto* b = (unsigned char*) &_val; {
for (int i = 0; i < 4; i++) uint8_t mode = 0x16;
canBuf[i] = *(b + i); txHeader.StdId = nodeID << 7 | mode;
canBuf[4] = _storeToMem ? 1 : 0;
// Int to Bytes
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); uint32_t val = _enable ? 1 : 0;
} auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
void CtrlStepMotor::ApplyPositionAsHome() canBuf[4] = 1; // Need save to EEPROM or not
{
uint8_t mode = 0x15; CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
txHeader.StdId = nodeID << 7 | mode; }
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} void CtrlStepMotor::SetEnableStallProtect(bool _enable)
{
uint8_t mode = 0x1B;
void CtrlStepMotor::SetEnableOnBoot(bool _enable) txHeader.StdId = nodeID << 7 | mode;
{
uint8_t mode = 0x16; uint32_t val = _enable ? 1 : 0;
txHeader.StdId = nodeID << 7 | mode; auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
// Int to Bytes canBuf[i] = *(b + i);
uint32_t val = _enable ? 1 : 0; canBuf[4] = 1; // Need save to EEPROM or not
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++) CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
canBuf[i] = *(b + i); }
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} void CtrlStepMotor::Reboot()
{
uint8_t mode = 0x7f;
void CtrlStepMotor::SetEnableAck(bool _enable) txHeader.StdId = nodeID << 7 | mode;
{
uint8_t mode = 0x1B; CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
txHeader.StdId = nodeID << 7 | mode; }
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val; void CtrlStepMotor::EraseConfigs()
for (int i = 0; i < 4; i++) {
canBuf[i] = *(b + i); uint8_t mode = 0x7e;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
} CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
{ void CtrlStepMotor::SetAngle(float _angle)
uint8_t mode = 0x1C; {
txHeader.StdId = nodeID << 7 | mode; _angle = inverseDirection ? -_angle : _angle;
float stepMotorCnt = _angle / 360.0f * (float) reduction;
uint32_t val = _enable ? 1 : 0; SetPositionSetPoint(stepMotorCnt);
auto* b = (unsigned char*) &val; }
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
void CtrlStepMotor::SetAngleWithVelocityLimit(float _angle, float _vel)
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); {
} _angle = inverseDirection ? -_angle : _angle;
float stepMotorCnt = _angle / 360.0f * (float) reduction;
SetPositionWithVelocityLimit(stepMotorCnt, _vel);
void CtrlStepMotor::Reboot() }
{
uint8_t mode = 0x7f;
txHeader.StdId = nodeID << 7 | mode; void CtrlStepMotor::UpdateAngle()
{
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); uint8_t mode = 0x23;
} txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
void CtrlStepMotor::EraseConfigs() }
{
uint8_t mode = 0x7e;
txHeader.StdId = nodeID << 7 | mode; void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isFinished)
{
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); state = _isFinished ? FINISH : RUNNING;
}
float tmp = _pos / (float) reduction * 360;
angle = inverseDirection ? -tmp : tmp;
void CtrlStepMotor::SetAngle(float _angle) }
{
_angle = inverseDirection ? -_angle : _angle;
if (_angle <= angleLimitMax && _angle >= angleLimitMin) void CtrlStepMotor::SetDceKp(int32_t _val)
{ {
float stepMotorCnt = _angle / 360.0f * reduction * CTRL_CIRCLE_COUNT; uint8_t mode = 0x17;
SetPositionSetPoint(stepMotorCnt); txHeader.StdId = nodeID << 7 | mode;
}
} auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
void CtrlStepMotor::SetAngleWithTime(float _angle, float _time) canBuf[4] = 1; // Need save to EEPROM or not
{
_angle = inverseDirection ? -_angle : _angle; CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
float stepMotorCnt = _angle / 360.0f * reduction; }
SetPositionWithTime(stepMotorCnt, _time);
}
void CtrlStepMotor::SetDceKv(int32_t _val)
{
void CtrlStepMotor::UpdateAngle() uint8_t mode = 0x18;
{ txHeader.StdId = nodeID << 7 | mode;
uint8_t mode = 0x23;
txHeader.StdId = nodeID << 7 | mode; auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); canBuf[i] = *(b + i);
} canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isAck) }
{
if (_isAck)
state = FINISH; void CtrlStepMotor::SetDceKi(int32_t _val)
{
float tmp = _pos / (float) reduction * 360; uint8_t mode = 0x19;
angle = inverseDirection ? -tmp : tmp; txHeader.StdId = nodeID << 7 | mode;
}
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
void CtrlStepMotor::SetDceKp(int32_t _val) canBuf[i] = *(b + i);
{ canBuf[4] = 1; // Need save to EEPROM or not
uint8_t mode = 0x17;
txHeader.StdId = nodeID << 7 | mode; CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i); void CtrlStepMotor::SetDceKd(int32_t _val)
{
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader); uint8_t mode = 0x1A;
} txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
void CtrlStepMotor::SetDceKv(int32_t _val) for (int i = 0; i < 4; i++)
{ canBuf[i] = *(b + i);
uint8_t mode = 0x18; canBuf[4] = 1; // Need save to EEPROM or not
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
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);
}

View File

@ -1,99 +1,95 @@
#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP #ifndef DUMMY_CORE_FW_CTRL_STEP_HPP
#define DUMMY_CORE_FW_CTRL_STEP_HPP #define DUMMY_CORE_FW_CTRL_STEP_HPP
#include "fibre/protocol.hpp" #include "fibre/protocol.hpp"
#include "can.h" #include "can.h"
class CtrlStepMotor class CtrlStepMotor
{ {
public: public:
enum State enum State
{ {
RUNNING, RUNNING,
FINISH, FINISH,
STOP STOP
}; };
const uint32_t CTRL_CIRCLE_COUNT = 200 * 256; const uint32_t CTRL_CIRCLE_COUNT = 200 * 256;
CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1, CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1,
float _minAngle = -180, float _maxAngle = 180); float _angleLimitMin = -180, float _angleLimitMax = 180);
uint8_t nodeID; uint8_t nodeID;
float angle = 0; float angle = 0;
float angleLimitMax; float angleLimitMax;
float angleLimitMin; float angleLimitMin;
bool inverseDirection; bool inverseDirection;
uint8_t reduction; uint8_t reduction;
State state = STOP; State state = STOP;
void SetAngle(float _angle); void SetAngle(float _angle);
void SetAngleWithTime(float _angle, float _time); void SetAngleWithVelocityLimit(float _angle, float _vel);
// CAN Command // CAN Command
void SetEnable(bool _enable); void SetEnable(bool _enable);
void DoCalibration(); void DoCalibration();
void SetCurrentSetPoint(float _val); void SetCurrentSetPoint(float _val);
void SetVelocitySetPoint(float _val); void SetVelocitySetPoint(float _val);
void SetPositionSetPoint(float _val); void SetPositionSetPoint(float _val);
void SetPositionWithTime(float _pos, float _time); void SetPositionWithVelocityLimit(float _pos, float _vel);
void AddTrajectoryPoint(float _pos, float _vel); void SetNodeID(uint32_t _id);
void SetNodeID(uint32_t _id); void SetCurrentLimit(float _val);
void SetCurrentLimit(float _val); void SetVelocityLimit(float _val);
void SetVelocityLimit(float _val); void SetAcceleration(float _val);
void SetAcceleration(float _val, bool _storeToMem = false); void SetDceKp(int32_t _val);
void SetDceKp(int32_t _val); void SetDceKv(int32_t _val);
void SetDceKv(int32_t _val); void SetDceKi(int32_t _val);
void SetDceKi(int32_t _val); void SetDceKd(int32_t _val);
void SetDceKd(int32_t _val); void ApplyPositionAsHome();
void ApplyPositionAsHome(); void SetEnableOnBoot(bool _enable);
void SetEnableOnBoot(bool _enable); void SetEnableStallProtect(bool _enable);
void SetEnableAck(bool _enable); void Reboot();
void SetEnableStallProtect(bool _enable); void EraseConfigs();
void Reboot();
void EraseConfigs(); void UpdateAngle();
void UpdateAngleCallback(float _pos, bool _isFinished);
void UpdateAngle();
void UpdateAngleCallback(float _pos, bool _isAck);
// Communication protocol definitions
auto MakeProtocolDefinitions()
// Communication protocol definitions {
auto MakeProtocolDefinitions() return make_protocol_member_list(
{ make_protocol_ro_property("angle", &angle),
return make_protocol_member_list( make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot),
make_protocol_ro_property("angle", &angle), make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs),
make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot), make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"),
make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs), make_protocol_function("set_position_with_time", *this,
make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"), &CtrlStepMotor::SetPositionWithVelocityLimit, "pos", "time"),
make_protocol_function("set_position_with_time", *this, &CtrlStepMotor::SetPositionWithTime, make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"),
"pos", "time"), make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"),
make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"), make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"),
make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"), make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"),
make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"), make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"),
make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"), make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"),
make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"), make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc"),
make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"), make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome),
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc", make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration),
"stored"), make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"),
make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome), make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"),
make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration), make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"),
make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"), make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"),
make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"), make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"),
make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"), make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect,
make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"), "enable"),
make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"), make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle)
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 = {};
private: };
CAN_HandleTypeDef* hcan;
uint8_t canBuf[8] = {}; #endif //DUMMY_CORE_FW_CTRL_STEP_HPP
CAN_TxHeaderTypeDef txHeader = {};
};
#endif //DUMMY_CORE_FW_CTRL_STEP_HPP

View File

@ -1,83 +1,83 @@
#ifndef DOF6_KINEMATIC_SOLVER_H #ifndef DOF6_KINEMATIC_SOLVER_H
#define DOF6_KINEMATIC_SOLVER_H #define DOF6_KINEMATIC_SOLVER_H
#include "stm32f405xx.h" #include "stm32f405xx.h"
#include "arm_math.h" #include "arm_math.h"
#include "memory.h" #include "memory.h"
class DOF6Kinematic class DOF6Kinematic
{ {
private: private:
const float RAD_TO_DEG = 57.295777754771045f; const float RAD_TO_DEG = 57.295777754771045f;
// DH parameters // DH parameters
struct ArmConfig_t struct ArmConfig_t
{ {
float L_BASE; float L_BASE;
float D_BASE; float D_BASE;
float L_ARM; float L_ARM;
float L_FOREARM; float L_FOREARM;
float D_ELBOW; float D_ELBOW;
float L_WRIST; float L_WRIST;
}; };
ArmConfig_t armConfig; ArmConfig_t armConfig;
float DH_matrix[6][4] = {0}; // home,d,a,alpha float DH_matrix[6][4] = {0}; // home,d,a,alpha
float L1_base[3] = {0}; float L1_base[3] = {0};
float L2_arm[3] = {0}; float L2_arm[3] = {0};
float L3_elbow[3] = {0}; float L3_elbow[3] = {0};
float L6_wrist[3] = {0}; float L6_wrist[3] = {0};
float l_se_2; float l_se_2;
float l_se; float l_se;
float l_ew_2; float l_ew_2;
float l_ew; float l_ew;
float atan_e; float atan_e;
public: public:
struct Joint6D_t struct Joint6D_t
{ {
Joint6D_t() Joint6D_t()
= default; = default;
Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6) Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6)
: a{a1, a2, a3, a4, a5, a6} : a{a1, a2, a3, a4, a5, a6}
{} {}
float a[6]; float a[6];
friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2); friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2);
}; };
struct Pose6D_t struct Pose6D_t
{ {
Pose6D_t() Pose6D_t()
= default; = default;
Pose6D_t(float x, float y, float z, float a, float b, float c) 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) : X(x), Y(y), Z(z), A(a), B(b), C(c), hasR(false)
{} {}
float X{}, Y{}, Z{}; float X{}, Y{}, Z{};
float A{}, B{}, C{}; float A{}, B{}, C{};
float R[9]{}; float R[9]{};
// if Pose was calculated by FK then it's true automatically (so that no need to do extra calc), // 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. // otherwise if manually set params then it should be set to false.
bool hasR{}; bool hasR{};
}; };
struct IKSolves_t struct IKSolves_t
{ {
Joint6D_t config[8]; Joint6D_t config[8];
char solFlag[8][3]; char solFlag[8][3];
}; };
DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT); 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 SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D);
bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves); bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves);
}; };
#endif //DOF6_KINEMATIC_SOLVER_H #endif //DOF6_KINEMATIC_SOLVER_H

File diff suppressed because it is too large Load Diff

View File

@ -1,167 +1,205 @@
#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H #ifndef REF_STM32F4_FW_DUMMY_ROBOT_H
#define REF_STM32F4_FW_DUMMY_ROBOT_H #define REF_STM32F4_FW_DUMMY_ROBOT_H
#include "algorithms/kinematic/6dof_kinematic.h" #include "algorithms/kinematic/6dof_kinematic.h"
#include "actuators/ctrl_step/ctrl_step.hpp" #include "actuators/ctrl_step/ctrl_step.hpp"
#define ALL 0 #define ALL 0
/* /*
| PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` | | PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
| ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- | | ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
| **Joint1** | 1 | 25 | 1000 | 80 | 300 | 250 | | **Joint1** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint2** | 1.5 | 25 | 1000 | 250 | 300 | 200 | | **Joint2** | 2 | 30 | 1000 | 80 | 200 | 200 |
| **Joint3** | 1 | 25 | 1000 | 350 | 500 | 250 | | **Joint3** | 2 | 30 | 1500 | 80 | 200 | 250 |
| **Joint4** | 1.5 | 25 | 1000 | 350 | 500 | 250 | | **Joint4** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint5** | 1.5 | 25 | 1000 | 350 | 500 | 250 | | **Joint5** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint6** | 1.5 | 25 | 1000 | 350 | 500 | 250 | | **Joint6** | 2 | 30 | 1000 | 80 | 200 | 250 |
*/ */
class DummyHand class DummyHand
{ {
public: public:
uint8_t nodeID = 7; uint8_t nodeID = 7;
float maxCurrent = 0.7; float maxCurrent = 0.7;
DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id); DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);
void SetAngle(float _angle); void SetAngle(float _angle);
void SetMaxCurrent(float _val); void SetMaxCurrent(float _val);
void SetEnable(bool _enable); void SetEnable(bool _enable);
// Communication protocol definitions // Communication protocol definitions
auto MakeProtocolDefinitions() auto MakeProtocolDefinitions()
{ {
return make_protocol_member_list( return make_protocol_member_list(
make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"), make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"), make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current") make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
); );
} }
private: private:
CAN_HandleTypeDef* hcan; CAN_HandleTypeDef* hcan;
uint8_t canBuf[8]; uint8_t canBuf[8];
CAN_TxHeaderTypeDef txHeader; CAN_TxHeaderTypeDef txHeader;
float minAngle = 0; float minAngle = 0;
float maxAngle = 45; float maxAngle = 45;
}; };
class DummyRobot class DummyRobot
{ {
public: public:
explicit DummyRobot(CAN_HandleTypeDef* _hcan); explicit DummyRobot(CAN_HandleTypeDef* _hcan);
~DummyRobot(); ~DummyRobot();
enum CommandMode enum CommandMode
{ {
COMMAND_TARGET_POINT_SEQUENTIAL = 1, COMMAND_TARGET_POINT_SEQUENTIAL = 1,
COMMAND_TARGET_POINT_INTERRUPTABLE, COMMAND_TARGET_POINT_INTERRUPTABLE,
COMMAND_CONTINUES_TRAJECTORY COMMAND_CONTINUES_TRAJECTORY,
}; COMMAND_MOTOR_TUNING
};
// 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; class TuningHelper
const float DEFAULT_JOINT_ACCELERATION = 50; {
public:
DOF6Kinematic::Joint6D_t currentJoints = REST_POSE; explicit TuningHelper(DummyRobot* _context) : context(_context)
DOF6Kinematic::Joint6D_t initPose = REST_POSE; {
DOF6Kinematic::Pose6D_t currentPose6D = {}; }
volatile uint8_t jointsStateFlag = 0b00000000;
CommandMode commandMode = COMMAND_TARGET_POINT_SEQUENTIAL; void SetTuningFlag(uint8_t _flag);
bool isStopped = false; void Tick(uint32_t _timeMillis);
CtrlStepMotor* motorJ[7] = {nullptr}; void SetFreqAndAmp(float _freq, float _amp);
DummyHand* hand = {nullptr};
// Communication protocol definitions
float MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6); auto MakeProtocolDefinitions()
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); return make_protocol_member_list(
void MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c); make_protocol_function("set_tuning_freq_amp", *this,
void SetJointSpeed(float _speed); &TuningHelper::SetFreqAndAmp, "freq", "amp"),
void SetJointAcceleration(float _acc); make_protocol_function("set_tuning_flag", *this,
void UpdateJointAngles(); &TuningHelper::SetTuningFlag, "flag")
void UpdateJointAnglesCallback(); );
void UpdateJointPose6D(); }
void Reboot();
void SetEnable(bool _enable);
void CalibrateHomeOffset(); private:
void Homing(); DummyRobot* context;
void Resting(); float time = 0;
bool IsMoving(); uint8_t tuningFlag = 0;
void SetCommandMode(uint8_t _mode); float frequency = 1;
float amplitude = 1;
};
// Communication protocol definitions TuningHelper tuningHelper = TuningHelper(this);
auto MakeProtocolDefinitions()
{
return make_protocol_member_list( // This is the pose when power on.
make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset), const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0};
make_protocol_function("homing", *this, &DummyRobot::Homing), const float DEFAULT_JOINT_SPEED = 30; // degree/s
make_protocol_function("resting", *this, &DummyRobot::Resting), const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = {150, 100, 200, 200, 200, 200};
make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()), const float DEFAULT_JOINT_ACCELERATION_LOW = 30; // 0~100
make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()), const float DEFAULT_JOINT_ACCELERATION_HIGH = 100; // 0~100
make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()), const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;
make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()), DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()), DOF6Kinematic::Joint6D_t targetJoints = REST_POSE;
make_protocol_object("hand", hand->MakeProtocolDefinitions()), DOF6Kinematic::Joint6D_t initPose = REST_POSE;
make_protocol_function("reboot", *this, &DummyRobot::Reboot), DOF6Kinematic::Pose6D_t currentPose6D = {};
make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"), volatile uint8_t jointsStateFlag = 0b00000000;
make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"), CommandMode commandMode = DEFAULT_COMMAND_MODE;
make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"), CtrlStepMotor* motorJ[7] = {nullptr};
make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"), DummyHand* hand = {nullptr};
make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"),
make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode")
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);
class CommandHandler void SetJointAcceleration(float _acc);
{ void UpdateJointAngles();
public: void UpdateJointAnglesCallback();
explicit CommandHandler(DummyRobot* _context) : context(_context) void UpdateJointPose6D();
{ void Reboot();
commandFifo = osMessageQueueNew(16, 64, nullptr); void SetEnable(bool _enable);
commandLifo = osMessageQueueNew(1, 64, nullptr); void CalibrateHomeOffset();
} void Homing();
void Resting();
uint32_t Push(const std::string &_cmd); bool IsMoving();
std::string Pop(uint32_t timeout); bool IsEnabled();
uint32_t ParseCommand(const std::string &_cmd); void SetCommandMode(uint32_t _mode);
uint32_t GetSpace();
void ClearFifo();
void EmergencyStop(); // Communication protocol definitions
void Resume(); auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
private: make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset),
DummyRobot* context; make_protocol_function("homing", *this, &DummyRobot::Homing),
osMessageQueueId_t commandFifo; make_protocol_function("resting", *this, &DummyRobot::Resting),
osMessageQueueId_t commandLifo; make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()),
char strBuffer[64]{}; make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()),
}; make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()),
CommandHandler commandHandler = CommandHandler(this); make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()),
private: make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()),
CAN_HandleTypeDef* hcan; make_protocol_object("hand", hand->MakeProtocolDefinitions()),
float jointSpeed = DEFAULT_JOINT_SPEED; make_protocol_function("reboot", *this, &DummyRobot::Reboot),
DOF6Kinematic* dof6Solver; 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"),
float MoveJoints(DOF6Kinematic::Joint6D_t _joints); 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())
#endif //REF_STM32F4_FW_DUMMY_ROBOT_H );
}
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

View File

@ -1,164 +1,183 @@
#include "common_inc.h" #include "common_inc.h"
// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c) // On-board Screen, can choose from hi2c2 or hi2c0(soft i2c)
SSD1306 oled(&hi2c0); SSD1306 oled(&hi2c0);
// On-board Sensor, used hi2c1 // On-board Sensor, used hi2c1
MPU6050 mpu6050(&hi2c1); MPU6050 mpu6050(&hi2c1);
// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14 // 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14
Timer timerCtrlLoop(&htim7, 200); Timer timerCtrlLoop(&htim7, 200);
// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs // 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs
PWM pwm(21000, 21000); PWM pwm(21000, 21000);
// Robot instance // Robot instance
DummyRobot dummy(&hcan1); DummyRobot dummy(&hcan1);
/* Thread Definitions -----------------------------------------------------*/ /* Thread Definitions -----------------------------------------------------*/
osThreadId_t controlLoopFixUpdateHandle; osThreadId_t controlLoopFixUpdateHandle;
void ThreadControlLoopFixUpdate(void* argument) void ThreadControlLoopFixUpdate(void* argument)
{ {
for (;;) for (;;)
{ {
// Suspended here until got Notification. // Suspended here until got Notification.
ulTaskNotifyTake(pdTRUE, portMAX_DELAY); ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
switch (dummy.commandMode) if (dummy.IsEnabled())
{ {
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL: // Send control command to Motors & update Joint states
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE: switch (dummy.commandMode)
dummy.UpdateJointAngles(); {
dummy.UpdateJointPose6D(); case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
break; case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY: case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
// ToDo: handle trajectory, while update state at the mean time dummy.MoveJoints(dummy.targetJoints);
dummy.UpdateJointPose6D(); dummy.UpdateJointPose6D();
break; break;
} case DummyRobot::COMMAND_MOTOR_TUNING:
} dummy.tuningHelper.Tick(10);
} dummy.UpdateJointPose6D();
break;
}
osThreadId_t ControlLoopUpdateHandle; } else
void ThreadControlLoopUpdate(void* argument) {
{ // Just update Joint states
for (;;) dummy.UpdateJointAngles();
{ dummy.UpdateJointPose6D();
dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever)); }
} }
} }
osThreadId_t oledTaskHandle; osThreadId_t ControlLoopUpdateHandle;
void ThreadOledUpdate(void* argument) void ThreadControlLoopUpdate(void* argument)
{ {
uint32_t t = micros(); for (;;)
char buf[16]; {
char cmdModeNames[3][4] = {"SEQ", "INT", "TRJ"}; dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever));
}
for (;;) }
{
mpu6050.Update(true);
osThreadId_t oledTaskHandle;
oled.clearBuffer(); void ThreadOledUpdate(void* argument)
{
oled.setFont(u8g2_font_5x8_tr); uint32_t t = micros();
oled.setCursor(0, 10); char buf[16];
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay); char cmdModeNames[4][4] = {"SEQ", "INT", "TRJ", "TUN"};
oled.setCursor(85, 10);
oled.printf("| FPS:%lu", 1000000 / (micros() - t)); for (;;)
t = micros(); {
mpu6050.Update(true);
oled.drawBox(0, 15, 128, 3);
oled.setCursor(0, 30); oled.clearBuffer();
oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d", oled.setFont(u8g2_font_5x8_tr);
(int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]), oled.setCursor(0, 10);
(int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]), oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
(int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5])); oled.setCursor(85, 10);
oled.printf("| FPS:%lu", 1000000 / (micros() - t));
oled.drawBox(40, 35, 128, 24); t = micros();
oled.setFont(u8g2_font_6x12_tr);
oled.setDrawColor(0); oled.drawBox(0, 15, 128, 3);
oled.setCursor(42, 45); oled.setCursor(0, 30);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X), oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d",
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z)); (int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]),
oled.setCursor(42, 56); (int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]),
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A), (int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5]));
(int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
oled.setDrawColor(1); oled.drawBox(40, 35, 128, 24);
oled.setCursor(0, 45); oled.setFont(u8g2_font_6x12_tr);
oled.printf("[XYZ]:"); oled.setDrawColor(0);
oled.setCursor(0, 56); oled.setCursor(42, 45);
oled.printf("[ABC]:"); oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X),
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z));
oled.setFont(u8g2_font_10x20_tr); oled.setCursor(42, 56);
oled.setCursor(0, 78); oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A),
for (int i = 1; i <= 6; i++) (int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_'); oled.setDrawColor(1);
buf[6] = 0; oled.setCursor(0, 45);
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf); oled.printf("[XYZ]:");
oled.setCursor(0, 56);
oled.sendBuffer(); oled.printf("[ABC]:");
}
} oled.setFont(u8g2_font_10x20_tr);
oled.setCursor(0, 78);
if (dummy.IsEnabled())
/* Timer Callbacks -------------------------------------------------------*/ {
void OnTimer7Callback() for (int i = 1; i <= 6; i++)
{ buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
BaseType_t xHigherPriorityTaskWoken = pdFALSE; buf[6] = 0;
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
// Wake & invoke thread IMMEDIATELY. } else
vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken); {
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], "======");
} }
oled.sendBuffer();
/* Default Entry -------------------------------------------------------*/ }
void Main(void) }
{
// Init all communication staff, include USB-CDC/VCP/UART/CAN etc.
InitCommunication(); /* Timer Callbacks -------------------------------------------------------*/
void OnTimer7Callback()
// Init IMU. {
do BaseType_t xHigherPriorityTaskWoken = pdFALSE;
{
mpu6050.Init(); // Wake & invoke thread IMMEDIATELY.
osDelay(100); vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken);
} while (!mpu6050.testConnection()); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
mpu6050.InitFilter(200, 100, 50); }
// Init OLED 128x80.
oled.Init(); /* Default Entry -------------------------------------------------------*/
pwm.Start(); void Main(void)
{
// Init & Run User Threads. // Init all communication staff, including USB-CDC/VCP/UART/CAN etc.
const osThreadAttr_t controlLoopTask_attributes = { InitCommunication();
.name = "ControlLoopFixUpdateTask",
.stack_size = 1000 * 4, // Init Robot.
.priority = (osPriority_t) osPriorityRealtime, dummy.Init();
};
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr, // Init IMU.
&controlLoopTask_attributes); do
{
const osThreadAttr_t ControlLoopUpdateTask_attributes = { mpu6050.Init();
.name = "ControlLoopUpdateTask", osDelay(100);
.stack_size = 1000 * 4, } while (!mpu6050.testConnection());
.priority = (osPriority_t) osPriorityNormal, mpu6050.InitFilter(200, 100, 50);
};
ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr, // Init OLED 128x80.
&ControlLoopUpdateTask_attributes); oled.Init();
pwm.Start();
const osThreadAttr_t oledTask_attributes = {
.name = "OledTask", // Init & Run User Threads.
.stack_size = 1000 * 4, const osThreadAttr_t controlLoopTask_attributes = {
.priority = (osPriority_t) osPriorityNormal, // should >= Normal .name = "ControlLoopFixUpdateTask",
}; .stack_size = 2000,
oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes); .priority = (osPriority_t) osPriorityRealtime,
};
// Start Timer Callbacks. controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
timerCtrlLoop.SetCallback(OnTimer7Callback); &controlLoopTask_attributes);
timerCtrlLoop.Start();
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
// System started, light switch-led up. .name = "ControlLoopUpdateTask",
pwm.SetDuty(PWM::CH_A1, 0.5); .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);
}

View File

@ -1,123 +1,116 @@
#include "common_inc.h" #include "common_inc.h"
extern DummyRobot dummy; extern DummyRobot dummy;
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{ {
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/ /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
if (_cmd[0] == '!' || dummy.isStopped) if (_cmd[0] == '!' || !dummy.IsEnabled())
{ {
std::string s(_cmd); std::string s(_cmd);
if (s.find("STOP") != std::string::npos) if (s.find("STOP") != std::string::npos)
{ {
dummy.commandHandler.EmergencyStop(); dummy.commandHandler.EmergencyStop();
Respond(_responseChannel, "!!!Stopped!!!"); Respond(_responseChannel, "Stopped ok");
} else if (s.find("RESUME") != std::string::npos) } else if (s.find("START") != std::string::npos)
{ {
dummy.commandHandler.Resume(); dummy.SetEnable(true);
Respond(_responseChannel, "Resumed"); Respond(_responseChannel, "Started ok");
} } else if (s.find("DISABLE") != std::string::npos)
} else if (_cmd[0] == '#') {
{ dummy.SetEnable(false);
std::string s(_cmd); Respond(_responseChannel, "Disabled ok");
if (s.find("GETJPOS") != std::string::npos) }
{ } else if (_cmd[0] == '#')
Respond(*usbStreamOutputPtr, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f", {
dummy.currentJoints.a[0], dummy.currentJoints.a[1], std::string s(_cmd);
dummy.currentJoints.a[2], dummy.currentJoints.a[3], if (s.find("GETJPOS") != std::string::npos)
dummy.currentJoints.a[4], dummy.currentJoints.a[5]); {
} else if (s.find("GETLPOS") != std::string::npos) Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
{ dummy.currentJoints.a[0], dummy.currentJoints.a[1],
dummy.UpdateJointPose6D(); dummy.currentJoints.a[2], dummy.currentJoints.a[3],
Respond(*usbStreamOutputPtr, "POSE %.2f %.2f %.2f %.2f %.2f %.2f", dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
dummy.currentPose6D.X, dummy.currentPose6D.Y, } else if (s.find("GETLPOS") != std::string::npos)
dummy.currentPose6D.Z, dummy.currentPose6D.A, {
dummy.currentPose6D.B, dummy.currentPose6D.C); dummy.UpdateJointPose6D();
} else if (s.find("CMDMODE") != std::string::npos) Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
{ dummy.currentPose6D.X, dummy.currentPose6D.Y,
int mode; dummy.currentPose6D.Z, dummy.currentPose6D.A,
sscanf(_cmd, "CMDMODE %d", &mode); dummy.currentPose6D.B, dummy.currentPose6D.C);
dummy.SetCommandMode(mode); } else if (s.find("CMDMODE") != std::string::npos)
Respond(*usbStreamOutputPtr, "Set command mode to [%d]", mode); {
} else uint32_t mode;
Respond(*usbStreamOutputPtr, "ok"); sscanf(_cmd, "#CMDMODE %lu", &mode);
} else if (_cmd[0] == '>' || _cmd[0] == '@') dummy.SetCommandMode(mode);
{ Respond(_responseChannel, "Set command mode to [%lu]", mode);
uint32_t freeSize = dummy.commandHandler.Push(_cmd); } else
Respond(_responseChannel, "%d", freeSize); Respond(_responseChannel, "ok");
} } else if (_cmd[0] == '>' || _cmd[0] == '@')
{
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/ uint32_t freeSize = dummy.commandHandler.Push(_cmd);
} Respond(_responseChannel, "%d", freeSize);
}
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel) /*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
{ }
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
uint8_t argNum;
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
if (_cmd[0] == '#') {
{ /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
std::string s(_cmd); if (_cmd[0] == '!' || !dummy.IsEnabled())
if (s.find("GETJPOS") != std::string::npos) {
{ std::string s(_cmd);
Respond(_responseChannel, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f", if (s.find("STOP") != std::string::npos)
dummy.currentJoints.a[0], dummy.currentJoints.a[1], dummy.currentJoints.a[2], {
dummy.currentJoints.a[3], dummy.currentJoints.a[4], dummy.currentJoints.a[5]); dummy.commandHandler.EmergencyStop();
} else Respond(_responseChannel, "Stopped ok");
Respond(_responseChannel, "ok"); } else if (s.find("START") != std::string::npos)
} else if (_cmd[0] == '>') {
{ dummy.SetEnable(true);
float joints[6]; Respond(_responseChannel, "Started ok");
float speed; } else if (s.find("DISABLE") != std::string::npos)
{
argNum = sscanf(_cmd, ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2, dummy.SetEnable(false);
joints + 3, joints + 4, joints + 5, &speed); Respond(_responseChannel, "Disabled ok");
if (argNum == 6) }
{ } else if (_cmd[0] == '#')
dummy.MoveJ(joints[0], joints[1], joints[2], {
joints[3], joints[4], joints[5]); std::string s(_cmd);
} else if (argNum == 7) if (s.find("GETJPOS") != std::string::npos)
{ {
dummy.SetJointSpeed(speed); Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.MoveJ(joints[0], joints[1], joints[2], dummy.currentJoints.a[0], dummy.currentJoints.a[1],
joints[3], joints[4], joints[5]); dummy.currentJoints.a[2], dummy.currentJoints.a[3],
} dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
} else if (s.find("GETLPOS") != std::string::npos)
while (dummy.IsMoving()) {
osDelay(10); dummy.UpdateJointPose6D();
Respond(_responseChannel, "ok"); Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentPose6D.X, dummy.currentPose6D.Y,
} else if (_cmd[0] == '@') dummy.currentPose6D.Z, dummy.currentPose6D.A,
{ dummy.currentPose6D.B, dummy.currentPose6D.C);
float pose[6]; } else if (s.find("CMDMODE") != std::string::npos)
float speed; {
uint32_t mode;
argNum = sscanf(_cmd, "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2, sscanf(_cmd, "#CMDMODE %lu", &mode);
pose + 3, pose + 4, pose + 5, &speed); dummy.SetCommandMode(mode);
if (argNum == 6) Respond(_responseChannel, "Set command mode to [%lu]", mode);
{ } else
dummy.MoveL(pose[0], pose[1], pose[2], Respond(_responseChannel, "ok");
pose[3], pose[4], pose[5]); } else if (_cmd[0] == '>' || _cmd[0] == '@')
} else if (argNum == 7) {
{ uint32_t freeSize = dummy.commandHandler.Push(_cmd);
dummy.SetJointSpeed(speed); Respond(_responseChannel, "%d", freeSize);
dummy.MoveL(pose[0], pose[1], pose[2], }
pose[3], pose[4], pose[5]); /*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
} }
while (dummy.IsMoving())
osDelay(10); void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
Respond(_responseChannel, "ok"); {
} /*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
} /*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
} }

View File

@ -1,44 +1,30 @@
#include "common_inc.h" #include "common_inc.h"
/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/ /*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/
extern DummyRobot dummy; extern DummyRobot dummy;
class HelperFunctions class HelperFunctions
{ {
public: public:
/*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/ /*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/
int32_t TestFunction(int32_t delta) float GetTemperatureHelper()
{ { return AdcGetChipTemperature(); }
static int cnt = 0;
return cnt += delta; } staticFunctions;
}
void SaveConfigurationHelper() // Define options that intractable with "reftool".
{} static inline auto MakeObjTree()
{
void EraseConfigurationHelper() /*--------------- 3.Add Your Protocol Variables & Functions Here ----------------*/
{} return make_protocol_member_list(
// Add Read-Only Variables
float GetTemperatureHelper() make_protocol_ro_property("serial_number", &serialNumber),
{ return AdcGetChipTemperature(); } make_protocol_function("get_temperature", staticFunctions, &HelperFunctions::GetTemperatureHelper),
make_protocol_object("robot", dummy.MakeProtocolDefinitions())
void SystemResetHelper() );
{ NVIC_SystemReset(); } }
} staticFunctions;
COMMIT_PROTOCOL
// 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

View File

@ -1,2 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" /> <module classpath="CMake" type="CPP_MODULE" version="4" />

View File

@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<project version="4"> <project version="4">
<component name="ProjectModuleManager"> <component name="ProjectModuleManager">
<modules> <modules>
<module fileurl="file://$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" filepath="$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" /> <module fileurl="file://$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" filepath="$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" />
</modules> </modules>
</component> </component>
</project> </project>