154 lines
6.6 KiB
C
154 lines
6.6 KiB
C
/*
|
||
四足机器人12个关节驱动 TODO:改为DMA
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "bsp/uart.h"
|
||
#include "go.h"
|
||
|
||
#include <stdbool.h>
|
||
#include <string.h>
|
||
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private variables -------------------------------------------------------- */
|
||
static bool inited = false;
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param){
|
||
if (go == NULL) return DEVICE_ERR_NULL;
|
||
if (param == NULL) return DEVICE_ERR_NULL;
|
||
if (inited) return DEVICE_ERR_INITED;
|
||
|
||
go->uart_param = param;
|
||
memset(&go->motor.output, 0, sizeof(go->motor.output));
|
||
memset(&go->motor.feedback, 0, sizeof(go->motor.feedback));
|
||
|
||
|
||
inited = true;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
|
||
int8_t GO_Motor_Control(GO_t *go){
|
||
if (go == NULL) return DEVICE_ERR_NULL;
|
||
if (!inited) return DEVICE_ERR_INITED;
|
||
|
||
// 发送控制命令
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
go->motor.output.id[i].id = i; // 设置电机ID
|
||
go->motor.output.id[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
|
||
modify_data(&go->motor.output.id[i]);
|
||
if (i < GO_MOTOR_NUM / 4 || i >= GO_MOTOR_NUM/2 && i < GO_MOTOR_NUM * 3 / 4) {
|
||
// if (i < GO_MOTOR_NUM / 4) {
|
||
HAL_UART_Transmit(BSP_UART_GetHandle(go->uart_param->left_leg),
|
||
(uint8_t *)&go->motor.output.id[i].motor_send_data,
|
||
sizeof(go->motor.output.id[i].motor_send_data), 1);
|
||
HAL_UART_Receive(BSP_UART_GetHandle(go->uart_param->left_leg),
|
||
(uint8_t *)&go->motor.feedback.id[i].motor_recv_data,
|
||
sizeof(go->motor.feedback.id[i].motor_recv_data), 1);
|
||
} else {
|
||
HAL_UART_Transmit(BSP_UART_GetHandle(go->uart_param->right_leg),
|
||
(uint8_t *)&go->motor.output.id[i].motor_send_data,
|
||
sizeof(go->motor.output.id[i].motor_send_data), 1);
|
||
HAL_UART_Receive(BSP_UART_GetHandle(go->uart_param->right_leg),
|
||
(uint8_t *)&go->motor.feedback.id[i].motor_recv_data,
|
||
sizeof(go->motor.feedback.id[i].motor_recv_data), 1);
|
||
}
|
||
extract_data(&go->motor.feedback.id[i]); // 解析接收到的数据
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
// int8_t GO_Half_Init(GO_Half_t *go_half, const GO_UART_Params_t *param) {
|
||
// if (go_half == NULL) return DEVICE_ERR_NULL;
|
||
// if (param == NULL) return DEVICE_ERR_NULL;
|
||
|
||
// go_half->uart_param = param;
|
||
// memset(&go_half->motor.output, 0, sizeof(go_half->motor.output));
|
||
// memset(&go_half->motor.feedback, 0, sizeof(go_half->motor.feedback));
|
||
|
||
// return DEVICE_OK;
|
||
// }
|
||
|
||
// int8_t GO_Control_Left(GO_Half_t *go_half) {
|
||
// if (go_half == NULL) return DEVICE_ERR_NULL;
|
||
|
||
// // 发送控制命令
|
||
// for (uint8_t i = 0; i < GO_MOTOR_NUM / 2; i++) {
|
||
// // 0,1,2 -> id 0,1,2；3,4,5 -> id 6,7,8
|
||
// if (i < 3) {
|
||
// go_half->motor.output.cmd[i].id = i;
|
||
// } else {
|
||
// go_half->motor.output.cmd[i].id = i + 3; // 3->6, 4->7, 5->8
|
||
// }
|
||
// go_half->motor.output.cmd[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
|
||
// modify_data(&go_half->motor.output.cmd[i]); // 修改数据
|
||
// HAL_UART_Transmit(BSP_UART_GetHandle(go_half->uart_param->left_leg),
|
||
// (uint8_t *)&go_half->motor.output.cmd[i].motor_send_data,
|
||
// sizeof(go_half->motor.output.cmd[i].motor_send_data), 1);
|
||
// HAL_UART_Receive(BSP_UART_GetHandle(go_half->uart_param->left_leg),
|
||
// (uint8_t *)&go_half->motor.feedback.data[i].motor_recv_data,
|
||
// sizeof(go_half->motor.feedback.data[i].motor_recv_data), 1);
|
||
// extract_data(&go_half->motor.feedback.data[i]); // 解析接收到的数据
|
||
// }
|
||
// return DEVICE_OK;
|
||
// }
|
||
|
||
// int8_t GO_Control_Right(GO_Half_t *go_half) {
|
||
// if (go_half == NULL) return DEVICE_ERR_NULL;
|
||
|
||
// // 发送控制命令
|
||
// for (uint8_t i = 0; i < GO_MOTOR_NUM / 2; i++) {
|
||
// // 0,1,2 -> id 3,4,5；3,4,5 -> id 9,10,11
|
||
// if (i < 3) {
|
||
// go_half->motor.output.cmd[i].id = i + 3; // 0->3, 1->4, 2->5
|
||
// } else {
|
||
// go_half->motor.output.cmd[i].id = i + 6; // 3->9, 4->10, 5->11
|
||
// }
|
||
// go_half->motor.output.cmd[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
|
||
// modify_data(&go_half->motor.output.cmd[i]); // 修改数据
|
||
// HAL_UART_Transmit(BSP_UART_GetHandle(go_half->uart_param->right_leg),
|
||
// (uint8_t *)&go_half->motor.output.cmd[i].motor_send_data,
|
||
// sizeof(go_half->motor.output.cmd[i].motor_send_data), 1);
|
||
// HAL_UART_Receive(BSP_UART_GetHandle(go_half->uart_param->right_leg),
|
||
// (uint8_t *)&go_half->motor.feedback.data[i].motor_recv_data,
|
||
// sizeof(go_half->motor.feedback.data[i].motor_recv_data), 1);
|
||
// extract_data(&go_half->motor.feedback.data[i]); // 解析接收到的数据
|
||
// }
|
||
// return DEVICE_OK;
|
||
// }
|
||
|
||
int8_t GO_ParseCmd(GO_t *go, const GO_ChassisCMD_t *chassis_cmd){
|
||
if (go == NULL) return DEVICE_ERR_NULL;
|
||
if (chassis_cmd == NULL) return DEVICE_ERR_NULL;
|
||
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
go->motor.output.id[i].K_P = chassis_cmd->id[i].K_P; // 设置位置控制增益
|
||
go->motor.output.id[i].K_W = chassis_cmd->id[i].K_W;
|
||
go->motor.output.id[i].Pos = chassis_cmd->id[i].Pos; // 设置目标位置
|
||
go->motor.output.id[i].W = chassis_cmd->id[i].W; // 设置
|
||
go->motor.output.id[i].T = chassis_cmd->id[i].T; // 设置目标扭矩
|
||
}
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t GO_ParseFeedback(GO_t *go, GO_ChassisFeedback_t *chassis_feedback){
|
||
if (go == NULL) return DEVICE_ERR_NULL;
|
||
if (chassis_feedback == NULL) return DEVICE_ERR_NULL;
|
||
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
chassis_feedback->id[i].Pos = go->motor.feedback.id[i].Pos; // 设置位置反馈
|
||
chassis_feedback->id[i].W = go->motor.feedback.id[i].W; // 设置速度反馈
|
||
chassis_feedback->id[i].T = go->motor.feedback.id[i].T; // 设置扭矩反馈
|
||
}
|
||
return DEVICE_OK;
|
||
}
|