diff --git a/User/bsp/can.c b/User/bsp/can.c index 1613b73..193dd2d 100644 --- a/User/bsp/can.c +++ b/User/bsp/can.c @@ -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) { @@ -430,12 +347,7 @@ 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); + } +} diff --git a/User/module/arm.cpp b/User/module/arm.cpp index 979a97a..fbd0dc1 100644 --- a/User/module/arm.cpp +++ b/User/module/arm.cpp @@ -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]); } @@ -222,9 +252,11 @@ void Arm::init(){ MOTOR_DM_Register(&dm_motor_params[i]); 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]); - - // } - // } } diff --git a/User/module/arm.hpp b/User/module/arm.hpp index 5103d5f..241ce34 100644 --- a/User/module/arm.hpp +++ b/User/module/arm.hpp @@ -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; }; \ No newline at end of file diff --git a/User/module/chassis.cpp b/User/module/chassis.cpp index 9c69b98..04d38a4 100644 --- a/User/module/chassis.cpp +++ b/User/module/chassis.cpp @@ -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(){ + +} diff --git a/User/module/chassis.hpp b/User/module/chassis.hpp index 196af55..400ed99 100644 --- a/User/module/chassis.hpp +++ b/User/module/chassis.hpp @@ -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]; };