Feelyx/mv_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

can2好了

Browse Source
This commit is contained in:
Feelyx 2026-01-14 15:42:02 +08:00
parent 1f967e86f0
commit 554d930467
5 changed files with 276 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

355
User/bsp/can.c
View File

@ -37,21 +37,16 @@ static osMutexId_t queue_mutex = NULL;
static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
static bool inited = false;
static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */
/* Private function prototypes ---------------------------------------------- */
static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id);
static void BSP_CAN_RxFifo0Callback(void);
// static void BSP_CAN_RxFifo1Callback(void);
static void BSP_CAN_TxCompleteCallback(void);
static void BSP_CAN_RxFifo1Callback(void);
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
static void BSP_CAN_TxQueueInit(BSP_CAN_t can);
static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
@ -66,7 +61,7 @@ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
if (hcan->Instance == CAN1)
return BSP_CAN_1;
else if (hcan->Instance == CAN1)
else if (hcan->Instance == CAN2)
return BSP_CAN_2;
else
return BSP_CAN_ERR;
@ -126,7 +121,29 @@ static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queu
return BSP_OK;
}
/**
* @brief 删除指定CAN ID的消息队列
* @note 内部函数，已包含互斥锁保护
*/
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) {
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return BSP_ERR_TIMEOUT;
}
BSP_CAN_QueueNode_t **current = &queue_list;
while (*current != NULL) {
if ((*current)->can == can && (*current)->can_id == can_id) {
BSP_CAN_QueueNode_t *to_delete = *current;
*current = (*current)->next;
osMessageQueueDelete(to_delete->queue);
BSP_Free(to_delete);
osMutexRelease(queue_mutex);
return BSP_OK;
}
current = &(*current)->next;
}
osMutexRelease(queue_mutex);
return BSP_ERR; // 未找到
}
/**
* @brief 获取帧类型
*/
@ -146,106 +163,6 @@ static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_
return original_id;
}
/**
* @brief 初始化发送队列
*/
static void BSP_CAN_TxQueueInit(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return;
tx_queues[can].head = 0;
tx_queues[can].tail = 0;
}
/**
* @brief 向发送队列添加消息（无锁）
*/
static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
if (can >= BSP_CAN_NUM || msg == NULL) return false;
BSP_CAN_TxQueue_t *queue = &tx_queues[can];
uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE;
// 队列满
if (next_head == queue->tail) {
return false;
}
// 复制消息
queue->buffer[queue->head] = *msg;
// 更新头指针（原子操作）
queue->head = next_head;
return true;
}
/**
* @brief 从发送队列取出消息（无锁）
*/
static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
if (can >= BSP_CAN_NUM || msg == NULL) return false;
BSP_CAN_TxQueue_t *queue = &tx_queues[can];
// 队列空
if (queue->head == queue->tail) {
return false;
}
// 复制消息
*msg = queue->buffer[queue->tail];
// 更新尾指针（原子操作）
queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE;
return true;
}
/**
* @brief 检查发送队列是否为空
*/
static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return true;
return tx_queues[can].head == tx_queues[can].tail;
}
/**
* @brief 处理所有CAN实例的发送队列
*/
static void BSP_CAN_TxCompleteCallback(void) {
// 处理所有CAN实例的发送队列
for (int i = 0; i < BSP_CAN_NUM; i++) {
BSP_CAN_t can = (BSP_CAN_t)i;
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) continue;
BSP_CAN_TxMessage_t msg;
uint32_t mailbox;
// 尝试发送队列中的消息
while (!BSP_CAN_TxQueueIsEmpty(can)) {
// 检查是否有空闲邮箱
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
break; // 没有空闲邮箱，等待下次中断
}
// 从队列中取出消息
if (!BSP_CAN_TxQueuePop(can, &msg)) {
break;
}
// 发送消息
if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
// 发送失败，消息已经从队列中移除，直接丢弃
break;
}
}
}
}
/**
* @brief FIFO0接收处理函数
*/
@ -281,34 +198,34 @@ static void BSP_CAN_RxFifo0Callback(void) {
/**
* @brief FIFO1接收处理函数
*/
// static void BSP_CAN_RxFifo1Callback(void) {
// CAN_RxHeaderTypeDef rx_header;
// uint8_t rx_data[BSP_CAN_MAX_DLC];
// for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
// CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
// if (hcan == NULL) continue;
// while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) {
// if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
// uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
// BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
// uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
// osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
// if (queue != NULL) {
// BSP_CAN_Message_t msg = {0};
// msg.frame_type = frame_type;
// msg.original_id = original_id;
// msg.parsed_id = parsed_id;
// msg.dlc = rx_header.DLC;
// if (rx_header.RTR == CAN_RTR_DATA) {
// memcpy(msg.data, rx_data, rx_header.DLC);
// }
// msg.timestamp = HAL_GetTick();
// osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
// }
// }
// }
// }
// }
static void BSP_CAN_RxFifo1Callback(void) {
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[BSP_CAN_MAX_DLC];
for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
if (hcan == NULL) continue;
while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
if (queue != NULL) {
BSP_CAN_Message_t msg = {0};
msg.frame_type = frame_type;
msg.original_id = original_id;
msg.parsed_id = parsed_id;
msg.dlc = rx_header.DLC;
if (rx_header.RTR == CAN_RTR_DATA) {
memcpy(msg.data, rx_data, rx_header.DLC);
}
msg.timestamp = HAL_GetTick();
osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
}
}
}
}
}
/* HAL Callback Functions --------------------------------------------------- */
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
@ -431,11 +348,6 @@ int8_t BSP_CAN_Init(void) {
// 清零回调函数数组
memset(CAN_Callback, 0, sizeof(CAN_Callback));
// 初始化发送队列
for (int i = 0; i < BSP_CAN_NUM; i++) {
BSP_CAN_TxQueueInit((BSP_CAN_t)i);
}
// 初始化ID解析器为默认解析器
id_parser = BSP_CAN_DefaultIdParser;
@ -465,19 +377,61 @@ int8_t BSP_CAN_Init(void) {
// 自动注册CAN1接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN1中断
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断
// 初始化 CAN2 - 使用 FIFO1
can1_filter.FilterBank = 14;
can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1;
HAL_CAN_ConfigFilter(&hcan2, &can1_filter); // 通过 CAN1 配置
HAL_CAN_Start(&hcan2);
// 自动注册CAN2接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
// 激活CAN2中断
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
inited = true;
return BSP_OK;
}
int8_t BSP_CAN_DeInit(void) {
if (!inited) {
return BSP_ERR;
}
// 删除所有队列
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) == osOK) {
BSP_CAN_QueueNode_t *current = queue_list;
while (current != NULL) {
BSP_CAN_QueueNode_t *next = current->next;
osMessageQueueDelete(current->queue);
BSP_Free(current);
current = next;
}
queue_list = NULL;
osMutexRelease(queue_mutex);
}
// 删除互斥锁
if (queue_mutex != NULL) {
osMutexDelete(queue_mutex);
queue_mutex = NULL;
}
// 清零回调函数数组
memset(CAN_Callback, 0, sizeof(CAN_Callback));
// 重置ID解析器
id_parser = NULL;
inited = false;
return BSP_OK;
}
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
@ -488,7 +442,7 @@ CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
case BSP_CAN_1:
return &hcan1;
case BSP_CAN_2:
return &hcan1;
return &hcan2;
default:
return NULL;
}
@ -533,58 +487,44 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
return BSP_ERR_NULL;
}
// 准备发送消息
BSP_CAN_TxMessage_t tx_msg = {0};
CAN_TxHeaderTypeDef header = {0};
uint32_t mailbox;
switch (format) {
case BSP_CAN_FORMAT_STD_DATA:
tx_msg.header.StdId = id;
tx_msg.header.IDE = CAN_ID_STD;
tx_msg.header.RTR = CAN_RTR_DATA;
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_DATA;
break;
case BSP_CAN_FORMAT_EXT_DATA:
tx_msg.header.ExtId = id;
tx_msg.header.IDE = CAN_ID_EXT;
tx_msg.header.RTR = CAN_RTR_DATA;
header.ExtId = id;
header.IDE = CAN_ID_EXT;
header.RTR = CAN_RTR_DATA;
break;
case BSP_CAN_FORMAT_STD_REMOTE:
tx_msg.header.StdId = id;
tx_msg.header.IDE = CAN_ID_STD;
tx_msg.header.RTR = CAN_RTR_REMOTE;
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_REMOTE;
break;
case BSP_CAN_FORMAT_EXT_REMOTE:
tx_msg.header.ExtId = id;
tx_msg.header.IDE = CAN_ID_EXT;
tx_msg.header.RTR = CAN_RTR_REMOTE;
header.ExtId = id;
header.IDE = CAN_ID_EXT;
header.RTR = CAN_RTR_REMOTE;
break;
default:
return BSP_ERR;
}
tx_msg.header.DLC = dlc;
tx_msg.header.TransmitGlobalTime = DISABLE;
header.DLC = dlc;
header.TransmitGlobalTime = DISABLE;
// 复制数据
if (data != NULL && dlc > 0) {
memcpy(tx_msg.data, data, dlc);
HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
if (result != HAL_OK) {
return BSP_ERR;
}
// 尝试直接发送到邮箱
uint32_t mailbox;
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) {
HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox);
if (result == HAL_OK) {
return BSP_OK; // 发送成功
}
}
// 邮箱满，尝试放入队列
if (BSP_CAN_TxQueuePush(can, &tx_msg)) {
return BSP_OK; // 成功放入队列
}
// 队列也满，丢弃数据
return BSP_ERR; // 数据丢弃
return BSP_OK;
}
int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
@ -616,6 +556,12 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
}
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) {
if (!inited) {
return BSP_ERR_INITED;
}
return BSP_CAN_DeleteIdQueue(can, can_id);
}
int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
if (!inited) {
@ -682,6 +628,15 @@ int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
return BSP_OK;
}
int8_t BSP_CAN_UnregisterIdParser(void) {
if (!inited) {
return BSP_ERR_INITED;
}
id_parser = BSP_CAN_DefaultIdParser;
return BSP_OK;
}
uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
if (id_parser != NULL) {
return id_parser(original_id, frame_type);
@ -689,4 +644,42 @@ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
return BSP_CAN_DefaultIdParser(original_id, frame_type);
}
int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
if (!inited) {
return BSP_ERR_INITED;
}
if (can >= BSP_CAN_NUM) {
return BSP_ERR;
}
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) {
return BSP_ERR_NULL;
}
uint32_t start_time = HAL_GetTick();
// 如果超时时间为0，立即检查并返回
if (timeout == 0) {
uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan);
return (free_level > 0) ? BSP_OK : BSP_ERR_TIMEOUT;
}
// 等待至少有一个邮箱空闲
while (true) {
uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan);
if (free_level > 0) {
return BSP_OK;
}
// 检查超时
if (timeout != BSP_CAN_TIMEOUT_FOREVER) {
uint32_t elapsed = HAL_GetTick() - start_time;
if (elapsed >= timeout) {
return BSP_ERR_TIMEOUT;
}
}
osDelay(1);
}
}

87
User/module/arm.cpp
View File

@ -20,6 +20,9 @@
#define AXIS_6_MIN_ANGLE -0.25f
#define AXIS_6_MAX_ANGLE 0.25f
#define DJI_MOTOR_OPEN 60
#define DJI_MOTOR_CLOSE 0
Arm::Arm(){
//dm电机参数设置
dm_motor_params[0] = MOTOR_DM_Param_t{
@ -68,6 +71,13 @@ Arm::Arm(){
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION
};
motor_dji_param = {
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M3508,
.reverse = false,
.gear = true
};
//dm电机控制参数初始化
for (int i = 0; i < 3; i++) {
arm_dm_output[i].velocity = 0;
@ -205,10 +215,30 @@ Arm::Arm(){
.d_cutoff_freq=100,
.range = -1,
};
pid_params[6] = KPID_Params_t{
.k=1,
.p=0.2,
.i=0,
.d=0,
.i_limit=0.5,
.out_limit=1,
.d_cutoff_freq=100,
.range = -1,
};
pid_v_params[6] = KPID_Params_t{
.k=1,
.p=0.2,
.i=0,
.d=0,
.i_limit=0.5,
.out_limit=1,
.d_cutoff_freq=100,
.range = -1,
};
}
void Arm::init(){
for(int i = 0; i < 6; i++){
for(int i = 0; i < 7; i++){
PID_Init(&pid[i], KPID_MODE_NO_D, CTRL_ARM_FREQ, &pid_params[i]);
PID_Init(&pid_v[i], KPID_MODE_NO_D, CTRL_ARM_FREQ, &pid_v_params[i]);
}
@ -223,8 +253,10 @@ void Arm::init(){
MOTOR_DM_Enable(&dm_motor_params[i]);
}
MOTOR_RM_Register(&motor_dji_param);
BSP_TIME_Delay(1000); //等待电机上线
read_adta();
read_data();
joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
@ -233,7 +265,7 @@ void Arm::init(){
joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
arm_relax();
BSP_TIME_Delay(1000); //等待电机上线
read_adta();
read_data();
joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
@ -255,15 +287,15 @@ void Arm::operator()(){
dt = time - last_time;
// arm_relax();
//读取反馈数据
read_adta();
read_data();
//根据遥控器计算关节角度
calc_motor_angle();
//根据关节角度计算电机角度
// calc_motor_angle();
calc_pid();
//发送控制命令
send_control();
// arm_relax();
// send_control();
arm_relax();
// //控制数据
// for (int i = 1; i < 3; i++) {
// arm_dm_output[i].angle = arm_dm_motor[i].motor.feedback.rotor_abs_angle + STEP;
@ -283,7 +315,16 @@ void Arm::arm_relax(){
MOTOR_LZ_Relax(&lz_motor_params[2]);
}
void Arm::read_adta(){
void Arm::arm_enable(){
MOTOR_DM_Enable(&dm_motor_params[0]);
MOTOR_DM_Enable(&dm_motor_params[1]);
MOTOR_DM_Enable(&dm_motor_params[2]);
MOTOR_LZ_Enable(&lz_motor_params[0]);
MOTOR_LZ_Enable(&lz_motor_params[1]);
MOTOR_LZ_Enable(&lz_motor_params[2]);
}
void Arm::read_data(){
//读取dm数据
MOTOR_DM_UpdateAll();
@ -305,6 +346,12 @@ void Arm::read_adta(){
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[2]);
arm_lz_motor[2] = *motor_lz;
MOTOR_RM_UpdateAll();
MOTOR_RM_t *motor_dji = NULL;
motor_dji = MOTOR_RM_GetMotor(&motor_dji_param);
arm_motor_dji = *motor_dji;
current_pos[0] = arm_lz_motor[0].lz_feedback.current_angle;
current_pos[1] = arm_lz_motor[1].lz_feedback.current_angle;
current_pos[2] = arm_lz_motor[2].lz_feedback.current_angle;
@ -408,6 +455,7 @@ void Arm::calc_pid(){
}
void Arm::send_control(){
MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
MOTOR_LZ_MotionControl(&lz_motor_params[1], &arm_lz_output[1]);
MOTOR_LZ_MotionControl(&lz_motor_params[2], &arm_lz_output[2]);
@ -415,29 +463,4 @@ void Arm::send_control(){
MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);
MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]);
// if(dr16.data.sw_r == DR16_SW_MID){
// if(dr16.data.ch_r_x >0.4f || dr16.data.ch_r_x < -0.4f){
// MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
// }if(dr16.data.ch_r_y >0.4f || dr16.data.ch_r_y < -0.4f){
// MOTOR_LZ_MotionControl(&lz_motor_params[1], &arm_lz_output[1]);
// }if(dr16.data.ch_l_y > 0.4f || dr16.data.ch_l_y < -0.4f || dr16.data.ch_r_y > 0.4f || dr16.data.ch_r_y < -0.4f){
// MOTOR_LZ_MotionControl(&lz_motor_params[2], &arm_lz_output[2]);
// }if(dr16.data.ch_l_x >0.4f || dr16.data.ch_l_x < -0.4f){
// MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_dm_output[0]);
// }
// }else if(dr16.data.sw_r == DR16_SW_UP){
// if(dr16.data.ch_r_y >0.4f || dr16.data.ch_r_y < -0.4f){
// MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);
// }if(dr16.data.ch_r_x >0.4f || dr16.data.ch_r_x < -0.4f){
// MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]);
// }
// }
}

10
User/module/arm.hpp
View File

@ -14,11 +14,12 @@ public:
void init();
void operator()();
private:
void read_adta();
void read_data();
void calc_joint_angle();
void calc_motor_angle();
void send_control();
void arm_relax();
void arm_enable();
void calc_pid();
float joint_angle[6] = {0}; //关节角度
// float motor_angle[6] = {0}; //电机角度
@ -28,8 +29,8 @@ private:
MOTOR_LZ_t arm_lz_motor[3];
MOTOR_LZ_MotionParam_t arm_lz_output[3];
MOTOR_MIT_Output_t arm_dm_output[3];
KPID_Params_t pid_params[6];
KPID_Params_t pid_v_params[6];
KPID_Params_t pid_params[7];
KPID_Params_t pid_v_params[7];
float output[6] = {0};
KPID_t pid[6];
KPID_t pid_v[6];
@ -38,5 +39,6 @@ private:
uint64_t time = 0;
uint64_t last_time = 0;
uint64_t dt = 0;
MOTOR_RM_Param_t motor_dji_param;
MOTOR_RM_t arm_motor_dji;
};

46
User/module/chassis.cpp
View File

@ -1,5 +1,5 @@
#include "module/chassis.hpp"
#include "module/dr16.h"
Chassis::Chassis() {
motors_param[0] = {
@ -40,20 +40,42 @@ void Chassis::init() {
}
void Chassis::operator()() {
MOTOR_RM_UpdateAll();
MOTOR_RM_t *motor = NULL;
motor = MOTOR_RM_GetMotor(&motors_param[0]);
chassis_motors[0] = motor->motor;
motor = MOTOR_RM_GetMotor(&motors_param[1]);
chassis_motors[1] = motor->motor;
motor = MOTOR_RM_GetMotor(&motors_param[2]);
chassis_motors[2] = motor->motor;
motor = MOTOR_RM_GetMotor(&motors_param[3]);
chassis_motors[3] = motor->motor;
//更新数据
update_motors();
//解算底盘数据
//计算pid
//发送控制
// MOTOR_RM_SetOutput(&motors_param[0], 0.1f);
// MOTOR_RM_Ctrl(&motors_param[0]);
}
void Chassis::update_motors(){
MOTOR_RM_UpdateAll();
MOTOR_RM_t *motor = NULL;
motor = MOTOR_RM_GetMotor(&motors_param[0]);
chassis_motors[0] = *motor;
motor = MOTOR_RM_GetMotor(&motors_param[1]);
chassis_motors[1] = *motor;
motor = MOTOR_RM_GetMotor(&motors_param[2]);
chassis_motors[2] = *motor;
motor = MOTOR_RM_GetMotor(&motors_param[3]);
chassis_motors[3] = *motor;
}
void Chassis::calc_chassis(){
if(dr16.data.sw_r == DR16_SW_DOWN){
}
}
void Chassis::calc_pid(){
}
void Chassis::send_control(){
}

6
User/module/chassis.hpp
View File

@ -11,7 +11,11 @@ public:
void init();
void operator()();
private:
void update_motors();
void calc_chassis();
void calc_pid();
void send_control();
MOTOR_RM_Param_t motors_param[4];
MOTOR_t chassis_motors[4];
MOTOR_RM_t chassis_motors[4];
};