robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

差一个电机

Browse Source
This commit is contained in:
Robofish 2025-08-31 22:25:15 +08:00
parent e75094a41d
commit 9574929545
13 changed files with 217 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
User/device/motor_lk.c
View File

@ -76,9 +76,16 @@ static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
} }
static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 调试信息：打印接收到的数据
// printf("LK Motor ID:%d, CMD:0x%02X, Data: %02X %02X %02X %02X %02X %02X %02X %02X\n",
// motor->param.id, msg->data[0], msg->data[0], msg->data[1], msg->data[2],
// msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
// 检查命令字节是否为反馈命令 // 检查命令字节是否为反馈命令
if (msg->data[0] != LK_CMD_FEEDBACK) { if (msg->data[0] != LK_CMD_FEEDBACK) {
return; // 如果不是标准反馈命令，可能是其他格式的数据
// 临时跳过命令字节检查，直接解析数据
// return;
} }
// 解析温度 (DATA[1]) // 解析温度 (DATA[1])
@ -138,8 +145,9 @@ int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
memset(&new_motor->motor, 0, sizeof(MOTOR_t)); memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse; new_motor->motor.reverse = param->reverse;
// 计算反馈ID（假设为控制ID + 0x40） // 对于某些LK电机，反馈数据可能通过命令ID发送
uint16_t feedback_id = param->id + 0x40; // 根据实际测试，使用命令ID接收反馈数据
uint16_t feedback_id = 0x140 + param->id; // 使用命令ID作为反馈ID
// 注册CAN接收ID // 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
@ -161,8 +169,8 @@ int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
for (int i = 0; i < manager->motor_count; i++) { for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i]; MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) { if (motor && motor->param.id == param->id) {
// 计算反馈ID // 对于某些LK电机，反馈数据通过命令ID发送
uint16_t feedback_id = param->id + 0x100; uint16_t feedback_id = 0x140 + param->id;
BSP_CAN_Message_t rx_msg; BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
@ -217,9 +225,9 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
// 转矩闭环控制命令 - 将输出值转换为转矩控制值 // 转矩闭环控制命令 - 将输出值转换为转矩控制值
int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE); int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE);
// 构建CAN帧 // 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID）
BSP_CAN_StdDataFrame_t tx_frame; BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id; tx_frame.id = 0x140 + param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE; tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 设置转矩闭环控制命令数据 // 设置转矩闭环控制命令数据
@ -245,7 +253,7 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL; if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame; BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id; tx_frame.id = 0x140 + param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE; tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机运行命令 // 电机运行命令
@ -265,7 +273,7 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL; if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame; BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id; tx_frame.id = 0x140 + param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE; tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机关闭命令 // 电机关闭命令
@ -297,7 +305,7 @@ MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
} }
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
return MOTOR_LK_SetOutput(param, 0.0f); return MOTOR_LK_SetOutput(param, 0.01f);
} }
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {

2
User/device/motor_lz.c
View File

@ -36,7 +36,7 @@
MOTOR_LZ_MotionParam_t lz_recover_param = { MOTOR_LZ_MotionParam_t lz_recover_param = {
.target_angle = 0.0f, .target_angle = 0.0f,
.target_velocity = 0.0f, .target_velocity = 0.0f,
.kp = 10.0f, .kp = 5.0f,
.kd = 1.0f, .kd = 1.0f,
.torque = 0.0f, .torque = 0.0f,
}; };

53
User/module/balance_chassis.c
View File

@ -4,16 +4,15 @@
#include <math.h> #include <math.h>
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, Chassis_Action_t action) { static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */ if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode && action == c->action) return CHASSIS_OK; /* 模式未改变直接返回 */ if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
PID_Reset(&c->pid.left_wheel); PID_Reset(&c->pid.left_wheel);
PID_Reset(&c->pid.right_wheel); PID_Reset(&c->pid.right_wheel);
PID_Reset(&c->pid.follow); PID_Reset(&c->pid.follow);
PID_Reset(&c->pid.balance); PID_Reset(&c->pid.balance);
c->mode = mode; c->mode = mode;
c->action = action;
c->state = 0; c->state = 0;
return CHASSIS_OK; return CHASSIS_OK;
@ -54,9 +53,8 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
MOTOR_LZ_Update(&c->param->joint_motors[i]); MOTOR_LZ_Update(&c->param->joint_motors[i]);
} }
for (int i = 0; i < 2; i++) { MOTOR_LK_Update(&c->param->wheel_motors[0]);
MOTOR_LK_Update(&c->param->wheel_motors[i]); MOTOR_LK_Update(&c->param->wheel_motors[1]);
}
/*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/ /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
// 更新关节电机反馈并检查离线状态 // 更新关节电机反馈并检查离线状态
@ -64,13 +62,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]); MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
if (joint_motor != NULL) { if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback; c->feedback.joint[i] = joint_motor->motor.feedback;
// 检查关节电机是否离线 }
if (!joint_motor->motor.header.online) {
return CHASSIS_ERR; // 有关节电机离线，返回错误
}
} else {
return CHASSIS_ERR; // 无法获取关节电机实例，返回错误
}
} }
// 更新轮子电机反馈并检查离线状态 // 更新轮子电机反馈并检查离线状态
@ -78,25 +70,16 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]); MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
if (wheel_motor != NULL) { if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback; c->feedback.wheel[i] = wheel_motor->motor.feedback;
// 检查轮子电机是否离线
if (!wheel_motor->motor.header.online) {
return CHASSIS_ERR; // 有轮子电机离线，返回错误
}
} else {
return CHASSIS_ERR; // 无法获取轮子电机实例，返回错误
} }
} }
return 0; return 0;
} }
int8_t Chassis_UpdateIMU(Chassis_t *c, const AHRS_Eulr_t *euler, const AHRS_Gyro_t *gyro, const AHRS_Accl_t *accl){ int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){
if (c == NULL || euler == NULL || gyro == NULL || accl == NULL) { if (c == NULL) {
return -1; // 参数错误 return -1; // 参数错误
} }
c->feedback.imu_euler = *euler; c->feedback.imu = imu;
c->feedback.imu_gyro = *gyro;
c->feedback.imu_accl = *accl;
return 0; return 0;
} }
@ -108,7 +91,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
c->lask_wakeup = BSP_TIME_Get_us(); c->lask_wakeup = BSP_TIME_Get_us();
/*设置底盘模式*/ /*设置底盘模式*/
if (Chassis_SetMode(c, c_cmd->mode, c_cmd->action) != CHASSIS_OK) { if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
return CHASSIS_ERR_MODE; // 设置模式失败 return CHASSIS_ERR_MODE; // 设置模式失败
} }
@ -120,7 +103,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
MOTOR_LZ_Relax(&c->param->joint_motors[i]); MOTOR_LZ_Relax(&c->param->joint_motors[i]);
} }
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
MOTOR_LK_MotorOff(&c->param->wheel_motors[i]); MOTOR_LK_Relax(&c->param->wheel_motors[i]); // 改为Relax以保持反馈
} }
break; break;
@ -139,26 +122,27 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
case 1: case 1:
// 关节电机复位轮电机输出0； // 关节电机复位轮电机输出0；
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 1.0f); // 回到零位，最大速度1.0 rad/s MOTOR_LZ_RecoverToZero(&c->param->joint_motors[i]);
} }
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); // 设置轮子速度为0 MOTOR_LK_Relax(&c->param->wheel_motors[i]);
} }
break; break;
} }
break; break;
case CHASSIS_MODE_WHELL_BALANCE: case CHASSIS_MODE_WHELL_BALANCE:
{
// 只靠两轮平衡，关节电机锁死，通过pid实现倒立摆 // 只靠两轮平衡，关节电机锁死，通过pid实现倒立摆
// 锁定关节电机到固定位置（比如直立状态） // 锁定关节电机到固定位置（比如直立状态）
for (int i = 0; i < 4; i++) { // for (int i = 0; i < 4; i++) {
MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 0.5f); // 目标位置0，最大速度0.5 rad/s // MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 0.5f); // 目标位置0，最大速度0.5 rad/s
} // }
// 双轮平衡控制 // 双轮平衡控制
// 获取IMU pitch角度和角速度作为平衡反馈 // 获取IMU pitch角度和角速度作为平衡反馈
float pitch_angle = c->feedback.imu_euler.pit; // pitch角度 float pitch_angle = c->feedback.imu.euler.pit;
float pitch_rate = c->feedback.imu_gyro.y; // pitch角速度 float pitch_rate = c->feedback.imu.gyro.y; // pitch角速度
// 平衡PID计算 - 以直立为目标（0度） // 平衡PID计算 - 以直立为目标（0度）
float balance_output = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, pitch_rate, c->dt); float balance_output = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, pitch_rate, c->dt);
@ -197,6 +181,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_output); MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_output);
MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_output); MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_output);
break; break;
}
case CHASSIS_MODE_WHELL_LEG_BALANCE: case CHASSIS_MODE_WHELL_LEG_BALANCE:
// 轮子+腿平衡模式（暂时留空，后续实现） // 轮子+腿平衡模式（暂时留空，后续实现）

22
User/module/balance_chassis.h
View File

@ -40,25 +40,22 @@ typedef enum {
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
} Chassis_Mode_t; } Chassis_Mode_t;
typedef enum {
CHASSIS_ACTION_JUMP,
CHASSIS_ACTION_STAND,
}Chassis_Action_t;
typedef struct { typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */ Chassis_Mode_t mode; /* 底盘模式 */
Chassis_Action_t action; /* 底盘动作 */
MoveVector_t move_vec; /* 底盘运动向量 */ MoveVector_t move_vec; /* 底盘运动向量 */
float height; /* 目标高度 */ float height; /* 目标高度 */
} Chassis_CMD_t; } Chassis_CMD_t;
typedef struct {
AHRS_Accl_t accl; /* IMU加速度计 */
AHRS_Gyro_t gyro; /* IMU陀螺仪 */
AHRS_Eulr_t euler; /* IMU欧拉角 */
}Chassis_IMU_t;
typedef struct { typedef struct {
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */ MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */ MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
AHRS_Accl_t imu_accl; /* IMU加速度计 */ Chassis_IMU_t imu; /* IMU数据 */
AHRS_Gyro_t imu_gyro; /* IMU陀螺仪 */
AHRS_Eulr_t imu_euler; /* IMU欧拉角 */
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */ MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
}Chassis_Feedback_t; }Chassis_Feedback_t;
@ -90,7 +87,7 @@ typedef struct {
* 包含了初始化参数，中间变量，输出变量 * 包含了初始化参数，中间变量，输出变量
*/ */
typedef struct { typedef struct {
uint32_t lask_wakeup; uint64_t lask_wakeup;
float dt; float dt;
Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */ Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
@ -98,7 +95,6 @@ typedef struct {
/* 模块通用 */ /* 模块通用 */
Chassis_Mode_t mode; /* 底盘模式 */ Chassis_Mode_t mode; /* 底盘模式 */
Chassis_Action_t action; /* 底盘动作 */
/* 反馈信息 */ /* 反馈信息 */
Chassis_Feedback_t feedback; Chassis_Feedback_t feedback;
@ -152,7 +148,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
int8_t Chassis_UpdateFeedback(Chassis_t *c); int8_t Chassis_UpdateFeedback(Chassis_t *c);
int8_t Chassis_UpdateIMU(Chassis_t *c, const AHRS_Eulr_t *euler, const AHRS_Gyro_t *gyro, const AHRS_Accl_t *accl); int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
/** /**
* \brief 运行底盘控制逻辑 * \brief 运行底盘控制逻辑
* *

80
User/module/config.c
View File

@ -55,16 +55,92 @@ Config_RobotParam_t robot_config = {
}, },
.wheel_motors[0] = { // 左轮电机 .wheel_motors[0] = { // 左轮电机
.can = BSP_CAN_1, .can = BSP_CAN_1,
.id = 0x141, .id = 1, // 电机ID为1，对应命令ID=0x141，反馈ID=0x181
.module = MOTOR_LK_MF9025, .module = MOTOR_LK_MF9025,
.reverse = false, .reverse = false,
}, },
.wheel_motors[1] = { // 右轮电机 .wheel_motors[1] = { // 右轮电机
.can = BSP_CAN_1, .can = BSP_CAN_1,
.id = 0x142, .id = 2, // 电机ID为2，对应命令ID=0x142，反馈ID=0x182
.module = MOTOR_LK_MF9025, .module = MOTOR_LK_MF9025,
.reverse = true, .reverse = true,
}, },
.chassis_param = {
.follow_pid_param = {
.k = 1.0f,
.p = 20.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 500.0f,
.d_cutoff_freq = 30.0f,
.range = 0.0f,
},
.motor_pid_param = {
.k = 1.0f,
.p = 0.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 12.0f,
.d_cutoff_freq = 30.0f,
.range = 0.0f,
},
.low_pass_cutoff_freq = {
.in = 30.0f,
.out = 30.0f,
},
.joint_motors = {
{ // 左髋关节
.can = BSP_CAN_1,
.motor_id = 124,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 左膝关节
.can = BSP_CAN_1,
.motor_id = 125,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右膝关节
.can = BSP_CAN_1,
.motor_id = 126,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右髋关节
.can = BSP_CAN_1,
.motor_id = 127,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
},
.wheel_motors = {
{ // 左轮电机
.can = BSP_CAN_1,
.id = 1,
.module = MOTOR_LK_MF9025,
.reverse = false,
},
{ // 右轮电机
.can = BSP_CAN_1,
.id = 2,
.module = MOTOR_LK_MF9025,
.reverse = true,
},
},
.mech_zero_yaw = 0.0f,
}
}; };
/* Private function prototypes ---------------------------------------------- */ /* Private function prototypes ---------------------------------------------- */

2
User/module/config.h
View File

@ -12,12 +12,14 @@ extern "C" {
#include "device/dm_imu.h" #include "device/dm_imu.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
#include "device/motor_lk.h" #include "device/motor_lk.h"
#include "module/balance_chassis.h"
typedef struct { typedef struct {
DM_IMU_Param_t imu_param; DM_IMU_Param_t imu_param;
MOTOR_LZ_Param_t joint_motors[4]; MOTOR_LZ_Param_t joint_motors[4];
MOTOR_LK_Param_t wheel_motors[2]; MOTOR_LK_Param_t wheel_motors[2];
Chassis_Params_t chassis_param;
} Config_RobotParam_t; } Config_RobotParam_t;
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */

8
User/task/atti_esti.c
View File

@ -9,6 +9,7 @@
#include "bsp/can.h" #include "bsp/can.h"
#include "device/dm_imu.h" #include "device/dm_imu.h"
#include "module/config.h" #include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,6 +18,7 @@
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
DM_IMU_t dm_imu; DM_IMU_t dm_imu;
Chassis_IMU_t chassis_imu_send;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -42,7 +44,11 @@ void Task_atti_esti(void *argument) {
/* USER CODE BEGIN */ /* USER CODE BEGIN */
if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
chassis_imu_send.accl = dm_imu.data.accl;
chassis_imu_send.gyro = dm_imu.data.gyro;
chassis_imu_send.euler = dm_imu.data.euler;
osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
} }
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

2
User/task/config.yaml
View File

@ -25,4 +25,4 @@
frequency: 500.0 frequency: 500.0
function: Task_ctrl_chassis function: Task_ctrl_chassis
name: ctrl_chassis name: ctrl_chassis
stack: 256 stack: 512

76
User/task/ctrl_chassis.c
View File

@ -10,6 +10,8 @@
#include "device/motor_lk.h" #include "device/motor_lk.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
#include "module/config.h" #include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,25 +19,20 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
MOTOR_LZ_MotionParam_t joint0_motion_params = { Chassis_t chassis;
.target_angle = 0.0f, Chassis_CMD_t chassis_cmd = {
.target_velocity = 0.0f, .mode = CHASSIS_MODE_RECOVER, // 改为RECOVER模式，让电机先启动
.kp = 1.0f, .move_vec = {
.kd = 1.0f, .vx = 0.0f,
.torque = 0.0f, .vy = 0.0f,
.wz = 0.0f,
},
.height = 0.0f,
}; };
Chassis_IMU_t chassis_imu;
MOTOR_LZ_MotionParam_t joint1_motion_params = { MOTOR_Feedback_t left_wheel_fb;
.target_angle = 0.0f, MOTOR_Feedback_t right_wheel_fb;
.target_velocity = 0.0f,
.kp = 1.0f,
.kd = 1.0f,
.torque = 0.0f,
};
bool motor_free = false;
MOTOR_t joint0;
MOTOR_t joint1;
// MOTOR_LZ_Feedback_t lz_feedback;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -51,47 +48,26 @@ void Task_ctrl_chassis(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
BSP_CAN_Init();
// 初始化电机 Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
MOTOR_LZ_Init();
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Register(&Config_GetRobotParam()->joint_motors[i]);
MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_motors[i]);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Register(&Config_GetRobotParam()->wheel_motors[i]);
MOTOR_LK_MotorOn(&Config_GetRobotParam()->wheel_motors[i]);
}
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
MOTOR_LZ_Update(&Config_GetRobotParam()->joint_motors[0]); if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
MOTOR_LZ_Update(&Config_GetRobotParam()->joint_motors[1]); chassis.feedback.imu = chassis_imu;
// 实时获取电机数据并更新 joint0 和 joint1
MOTOR_LZ_t* joint0_instance = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_motors[0]);
MOTOR_LZ_t* joint1_instance = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_motors[1]);
if (joint0_instance != NULL) {
joint0 = joint0_instance->motor;
} }
if (joint1_instance != NULL) { if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
joint1 = joint1_instance->motor; // 成功接收到命令，更新底盘命令
} }
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
if (motor_free) {
// MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_motors[0]);
// MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_motors[1]);
MOTOR_LZ_RecoverToZero(&Config_GetRobotParam()->joint_motors[0]);
MOTOR_LZ_RecoverToZero(&Config_GetRobotParam()->joint_motors[1]);
} else {
MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_motors[0], &joint0_motion_params);
MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_motors[1], &joint1_motion_params);
}
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}} }}

6
User/task/init.c
View File

@ -7,7 +7,9 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "component/ahrs.h"
#include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -38,6 +40,8 @@ void Task_Init(void *argument) {
// 创建消息队列 // 创建消息队列
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
/* USER MESSAGE END */ /* USER MESSAGE END */
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核

47
User/task/rc.c
View File

@ -7,6 +7,7 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "device/dr16.h" #include "device/dr16.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -15,6 +16,7 @@
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
DR16_t dr16; DR16_t dr16;
Chassis_CMD_t cmd_to_chassis;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -37,7 +39,50 @@ void Task_rc(void *argument) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
DR16_StartDmaRecv(&dr16); DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) {
// 根据左拨杆设置底盘模式
switch (dr16.data.sw_l) {
case 1: // 上位
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
case 3: // 中位
cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
break;
case 2: // 下位
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
break;
default:
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
// 解析遥控器摇杆数据为运动向量
// 将遥控器通道值从[364, 1684]映射到[-1.0, 1.0]
float ch_l_y_norm = (float)(dr16.data.ch_l_y - 1024) / 660.0f; // 前后
float ch_l_x_norm = (float)(dr16.data.ch_l_x - 1024) / 660.0f; // 左右
float ch_r_x_norm = (float)(dr16.data.ch_r_x - 1024) / 660.0f; // 旋转
// 设置运动向量（根据需要调整增益）
cmd_to_chassis.move_vec.vx = ch_l_y_norm * 2.0f; // 前后运动，增益可调
cmd_to_chassis.move_vec.vy = ch_l_x_norm * 2.0f; // 左右运动，增益可调
cmd_to_chassis.move_vec.wz = ch_r_x_norm * 3.0f; // 旋转运动，增益可调
// 设置目标高度（可根据右拨杆或其他输入调整）
cmd_to_chassis.height = 0.0f;
// 发送命令到底盘控制任务
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
} else {
// 超时处理，发送安全命令
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
cmd_to_chassis.move_vec.vx = 0.0f;
cmd_to_chassis.move_vec.vy = 0.0f;
cmd_to_chassis.move_vec.wz = 0.0f;
cmd_to_chassis.height = 0.0f;
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
}
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }

2
User/task/user_task.c
View File

@ -27,5 +27,5 @@ const osThreadAttr_t attr_atti_esti = {
const osThreadAttr_t attr_ctrl_chassis = { const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis", .name = "ctrl_chassis",
.priority = osPriorityNormal, .priority = osPriorityNormal,
.stack_size = 256 * 4, .stack_size = 512 * 8,
}; };

4
User/task/user_task.h
View File

@ -42,6 +42,10 @@ typedef struct {
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
struct { struct {
osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
osMessageQueueId_t chassis_imu;
osMessageQueueId_t Chassis_cmd;
} msgq; } msgq;
/* USER MESSAGE END */ /* USER MESSAGE END */