119 lines
4.5 KiB
C
119 lines
4.5 KiB
C
/*
|
|
四足机器人12个关节驱动
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "bsp/uart.h"
|
|
#include "go.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
|
|
|
|
#include "component/user_math.h"
|
|
|
|
#include "cmsis_os2.h"
|
|
|
|
static osSemaphoreId_t sem_left_rx = NULL;
|
|
static osSemaphoreId_t sem_right_rx = NULL;
|
|
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private variables -------------------------------------------------------- */
|
|
// static RIS_MotorData_t raw_left __attribute__ ((section(".AXI_SRAM")));
|
|
static uint8_t raw_left_buff[16] __attribute__ ((section(".AXI_SRAM")));
|
|
static uint8_t raw_right_buff[16] __attribute__ ((section(".AXI_SRAM")));
|
|
// static RIS_MotorData_t raw_right __attribute__ ((section(".AXI_SRAM")));
|
|
static MotorCmd_t motor_cmd __attribute__ ((section(".AXI_SRAM")));
|
|
|
|
static osThreadId_t thread_alert;
|
|
static GO_t *go_rx;
|
|
static bool inited = false;
|
|
|
|
/* Private function -------------------------------------------------------- */
|
|
|
|
// static void GO_LeftRxCpltCallback(void) {
|
|
// osMessageQueuePut(go_rx->msgq_raw, &raw_left_buff, 0, 0);
|
|
// osSemaphoreRelease(sem_left_rx); // 通知接收完成
|
|
// memset(raw_left_buff, 0, sizeof(raw_left_buff)); // 清空缓存区
|
|
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
|
|
// (uint8_t *)&raw_left, sizeof(RIS_MotorData_t));
|
|
// }
|
|
// static void GO_RightRxCpltCallback(void) {
|
|
// osMessageQueuePut(go_rx->msgq_raw, &raw_right_buff, 0, 0);
|
|
// osSemaphoreRelease(sem_right_rx); // 通知接收完成
|
|
// memset(raw_right_buff, 0, sizeof(raw_right_buff)); // 清空缓存区
|
|
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
|
|
// (uint8_t *)&raw_right, sizeof(RIS_MotorData_t));
|
|
// }
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
|
|
int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param){
|
|
if (go == NULL) return DEVICE_ERR_NULL;
|
|
if (inited) return DEVICE_ERR_INITED;
|
|
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
|
|
|
go_rx->msgq_raw = osMessageQueueNew(36, 16u, NULL);
|
|
|
|
go->param = param;
|
|
sem_left_rx = osSemaphoreNew(1, 0, NULL);
|
|
sem_right_rx = osSemaphoreNew(1, 0, NULL);
|
|
// BSP_UART_RegisterCallback(BSP_UART_LEFT_LEG, BSP_UART_RX_CPLT_CB,
|
|
// GO_LeftRxCpltCallback);
|
|
// BSP_UART_RegisterCallback(BSP_UART_RIGHT_LEG, BSP_UART_RX_CPLT_CB,
|
|
// GO_RightRxCpltCallback);
|
|
|
|
inited = true;
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t GO_Motor_Control(GO_MotorOutput_t *output, GO_t *go){
|
|
if (go == NULL) return DEVICE_ERR_NULL;
|
|
if (output == NULL) return DEVICE_ERR_NULL;
|
|
memset(raw_right_buff, 0, sizeof(raw_right_buff));
|
|
memset(raw_left_buff, 0, sizeof(raw_left_buff));
|
|
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
|
|
// (uint8_t *)&raw_left_buff, 16);
|
|
// HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
|
|
// (uint8_t *)&raw_right_buff, 16);
|
|
|
|
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
|
motor_cmd.id = i;
|
|
motor_cmd.mode = GO_MOTOR_MODE_FOC;
|
|
motor_cmd.T = output->as_array[i].tor_des;
|
|
motor_cmd.W = output->as_array[i].spd_des;
|
|
motor_cmd.Pos = output->as_array[i].pos_des;
|
|
motor_cmd.K_P = output->as_array[i].k_pos;
|
|
motor_cmd.K_W = output->as_array[i].k_spd;
|
|
|
|
modify_data(&motor_cmd);
|
|
|
|
if (i <= GO_MOTOR_LF_CALF || (i >= GO_MOTOR_LR_HIP && i <= GO_MOTOR_LR_CALF)) {
|
|
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
|
|
(uint8_t *)&motor_cmd.motor_send_data,
|
|
sizeof(RIS_ControlData_t));
|
|
|
|
HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
|
|
(uint8_t *)&raw_left_buff, 16, 5);
|
|
} else {
|
|
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
|
|
(uint8_t *)&motor_cmd.motor_send_data,
|
|
sizeof(RIS_ControlData_t));
|
|
|
|
HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
|
|
(uint8_t *)&raw_right_buff, 16, 5);
|
|
}
|
|
|
|
}
|
|
return DEVICE_OK;
|
|
}
|
|
|
|
int8_t GO_StoreMsg(GO_t *go, RIS_MotorData_t *go_rx){
|
|
|
|
return DEVICE_OK;
|
|
}
|