From 35cf664580bb30717d81b2471a5594fbdf6cb1aa Mon Sep 17 00:00:00 2001 From: Feelyx Date: Mon, 12 Jan 2026 21:44:09 +0800 Subject: [PATCH] =?UTF-8?q?can2=E6=88=91c=E4=BD=A0=E7=9A=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/arm.cpp | 122 ++++++++++++++++++++++++++++++++++++++++ User/module/arm.hpp | 10 +++- User/module/chassis.cpp | 7 +-- User/task/ctrl_arm.cpp | 8 +-- User/task/init.c | 4 +- 5 files changed, 138 insertions(+), 13 deletions(-) diff --git a/User/module/arm.cpp b/User/module/arm.cpp index e69de29..1fd4292 100644 --- a/User/module/arm.cpp +++ b/User/module/arm.cpp @@ -0,0 +1,122 @@ +#include "module/arm.hpp" +#include "bsp/time.h" + +Arm::Arm(){ + dm_motor_params[0] = MOTOR_DM_Param_t{ + .can = BSP_CAN_1, + .master_id = 0x11, + .can_id = 1, + .module = MOTOR_DM_J4310, + .reverse = false + }; + dm_motor_params[1] = MOTOR_DM_Param_t{ + .can = BSP_CAN_1, + .master_id = 0x12, + .can_id = 1, + .module = MOTOR_DM_J4310, + .reverse = false + }; + dm_motor_params[2] = MOTOR_DM_Param_t{ + .can = BSP_CAN_1, + .master_id = 0x13, + .can_id = 1, + .module = MOTOR_DM_J4310, + .reverse = false + }; + lz_motor_params[0] = MOTOR_LZ_Param_t{ + .can = BSP_CAN_1, + .motor_id = 127, + .host_id = 0xff, + .module= MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION + }; + lz_motor_params[1] = MOTOR_LZ_Param_t{ + .can = BSP_CAN_1, + .motor_id = 126, + .host_id = 0xff, + .module= MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION + }; + lz_motor_params[2] = MOTOR_LZ_Param_t{ + .can = BSP_CAN_1, + .motor_id = 125, + .host_id = 0xff, + .module= MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION + }; +} + +void Arm::init(){ + MOTOR_LZ_Init(); + MOTOR_LZ_Register(&lz_motor_params[0]); + MOTOR_LZ_Enable(&lz_motor_params[0]); + BSP_TIME_Delay(2); + MOTOR_LZ_Register(&lz_motor_params[1]); + MOTOR_LZ_Enable(&lz_motor_params[1]); + BSP_TIME_Delay(2); + MOTOR_LZ_Register(&lz_motor_params[2]); + MOTOR_LZ_Enable(&lz_motor_params[2]); + BSP_TIME_Delay(2); + + MOTOR_DM_Register(&dm_motor_params[0]); + MOTOR_DM_Enable(&dm_motor_params[0]); + BSP_TIME_Delay(2); + MOTOR_DM_Register(&dm_motor_params[1]); + MOTOR_DM_Enable(&dm_motor_params[1]); + BSP_TIME_Delay(2); + MOTOR_DM_Register(&dm_motor_params[2]); + MOTOR_DM_Enable(&dm_motor_params[2]); + BSP_TIME_Delay(2); + +} + +void Arm::operator()(){ + MOTOR_DM_Relax(&dm_motor_params[0]); + BSP_TIME_Delay(2); + MOTOR_DM_Relax(&dm_motor_params[1]); + BSP_TIME_Delay(2); + MOTOR_DM_Relax(&dm_motor_params[2]); + BSP_TIME_Delay(2); + + MOTOR_DM_UpdateAll(); + + MOTOR_DM_t *motor_dm = NULL; + motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[0]); + arm_dm_motor[0] = *motor_dm; + motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[1]); + arm_dm_motor[1] = *motor_dm; + motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[2]); + arm_dm_motor[2] = *motor_dm; + + arm_output[0].angle = 1.0f; + arm_output[0].velocity = 1.0f; + arm_output[0].kp = 0.5f; + arm_output[0].kd = 1.0f; + // MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_output[0]); + + arm_state = MOTOR_LZ_Relax(&lz_motor_params[0]); + BSP_TIME_Delay(2); + MOTOR_LZ_Relax(&lz_motor_params[1]); + BSP_TIME_Delay(2); + MOTOR_LZ_Relax(&lz_motor_params[2]); + BSP_TIME_Delay(2); + + MOTOR_LZ_UpdateAll(); + + MOTOR_LZ_t *motor_lz = NULL; + motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[0]); + arm_lz_motor[0] = *motor_lz; + motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[1]); + arm_lz_motor[1] = *motor_lz; + motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[2]); + arm_lz_motor[2] = *motor_lz; + // arm_lz_motion[0].target_angle = 1.0f; + arm_lz_motion[0].target_velocity = 1.0f; + // arm_lz_motion[0].kp = 0.5f; + arm_lz_motion[0].kd = 0.5f; + // MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_motion[0]); + +} diff --git a/User/module/arm.hpp b/User/module/arm.hpp index 66342ea..05eff6f 100644 --- a/User/module/arm.hpp +++ b/User/module/arm.hpp @@ -6,6 +6,7 @@ #include "device/motor_lz.h" #include "device/motor_rm.h" #include "device/motor_dm.h" +#include class Arm { public: @@ -13,6 +14,11 @@ public: void init(); void operator()(); private: - MOTOR_DM_Param_t motors_param[4]; - MOTOR_t arm_motors[4]; + MOTOR_DM_Param_t dm_motor_params[3]; + MOTOR_DM_t arm_dm_motor[3]; + MOTOR_LZ_Param_t lz_motor_params[3]; + MOTOR_LZ_t arm_lz_motor[3]; + MOTOR_LZ_MotionParam_t arm_lz_motion[3]; + MOTOR_MIT_Output_t arm_output[3]; + int8_t arm_state = 1; }; \ No newline at end of file diff --git a/User/module/chassis.cpp b/User/module/chassis.cpp index d08fb8a..62fe6ae 100644 --- a/User/module/chassis.cpp +++ b/User/module/chassis.cpp @@ -1,13 +1,11 @@ #include "module/chassis.hpp" -#include "bsp/can.h" -#include "device/motor_rm.h" Chassis::Chassis() { motors_param[0] = { .can = BSP_CAN_1, - .id = 0x201, - .module = MOTOR_M3508, + .id = 0x206, + .module = MOTOR_GM6020, .reverse = false, .gear = true }; @@ -35,7 +33,6 @@ Chassis::Chassis() { } void Chassis::init() { - BSP_CAN_Init(); MOTOR_RM_Register(&motors_param[0]); MOTOR_RM_Register(&motors_param[1]); MOTOR_RM_Register(&motors_param[2]); diff --git a/User/task/ctrl_arm.cpp b/User/task/ctrl_arm.cpp index ab23378..466800b 100644 --- a/User/task/ctrl_arm.cpp +++ b/User/task/ctrl_arm.cpp @@ -6,7 +6,7 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "module/arm.hpp" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +14,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ - +Arm arm; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,13 +30,13 @@ void Task_ctrl_arm(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - + arm.init(); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - + arm(); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/init.c b/User/task/init.c index 3beaad3..54a7901 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -7,7 +7,7 @@ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "bsp/can.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -25,7 +25,7 @@ void Task_Init(void *argument) { (void)argument; /* 未使用argument，消除警告 */ /* USER CODE INIT BEGIN */ - + BSP_CAN_Init(); /* USER CODE INIT END */ osKernelLock(); /* 锁定内核，防止任务切换 */