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-10-05 13:33:46 +08:00
parent 3b1ef030c2
commit cdd7be6ad7
11 changed files with 131 additions and 715 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -75,7 +75,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lk.c
User/device/motor_lz.c
User/device/motor_rm.c
User/device/rc_can.c
User/device/virtual_chassis.c
# User/module sources

328
User/device/rc_can.c
View File

@ -1,328 +0,0 @@
/* Includes ----------------------------------------------------------------- */
#include "device/rc_can.h"
#include "bsp/time.h"
#include "device/device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private constants -------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private types ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER VARIABLE BEGIN */
/* USER VARIABLE END */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/* Private function prototypes ---------------------------------------------- */
static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param);
static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化RC CAN发送模块
* @param rc_can RC_CAN结构体指针
* @param param 初始化参数
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param) {
if (rc_can == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 参数验证
if (RC_CAN_ValidateParams(param) != DEVICE_OK) {
return DEVICE_ERR;
}
rc_can->param = *param;
// 初始化header
rc_can->header.online = false;
rc_can->header.last_online_time = 0;
// 手动初始化数据结构
rc_can->data.joy.ch_l_x = 0.0f;
rc_can->data.joy.ch_l_y = 0.0f;
rc_can->data.joy.ch_r_x = 0.0f;
rc_can->data.joy.ch_r_y = 0.0f;
rc_can->data.sw.sw_l = RC_CAN_SW_ERR;
rc_can->data.sw.sw_r = RC_CAN_SW_ERR;
rc_can->data.sw.ch_res = 0.0f;
rc_can->data.mouse.x = 0.0f;
rc_can->data.mouse.y = 0.0f;
rc_can->data.mouse.z = 0.0f;
rc_can->data.mouse.mouse_l = false;
rc_can->data.mouse.mouse_r = false;
rc_can->data.keyboard.key_value = 0;
for (int i = 0; i < 6; i++) {
rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
}
// 注册CAN ID队列（从机模式需要接收数据）
if (rc_can->param.mode == RC_CAN_MODE_SLAVE) {
return RC_CAN_RegisterIds(rc_can);
}
return DEVICE_OK;
}
/**
* @brief 更新并发送数据到CAN总线
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
if (rc_can->param.mode != RC_CAN_MODE_MASTER) {
return DEVICE_ERR;
}
BSP_CAN_StdDataFrame_t frame;
frame.dlc = 8;
// 边界裁剪宏
#define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
switch (data_type) {
case RC_CAN_DATA_JOYSTICK: {
frame.id = rc_can->param.joy_id;
float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
int16_t l_x_i = (int16_t)(l_x * 32768.0f);
int16_t l_y_i = (int16_t)(l_y * 32768.0f);
int16_t r_x_i = (int16_t)(r_x * 32768.0f);
int16_t r_y_i = (int16_t)(r_y * 32768.0f);
frame.data[0] = (uint8_t)(l_x_i & 0xFF);
frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
frame.data[2] = (uint8_t)(l_y_i & 0xFF);
frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
frame.data[4] = (uint8_t)(r_x_i & 0xFF);
frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
frame.data[6] = (uint8_t)(r_y_i & 0xFF);
frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
break;
}
case RC_CAN_DATA_SWITCH: {
frame.id = rc_can->param.sw_id;
frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
frame.data[4] = 0; // 保留字节
frame.data[5] = 0; // 保留字节
frame.data[6] = 0; // 保留字节
frame.data[7] = 0; // 保留字节
break;
}
case RC_CAN_DATA_MOUSE: {
frame.id = rc_can->param.mouse_id;
// 鼠标x/y/z一般为增量，若有极限也可加clamp
int16_t x = (int16_t)(rc_can->data.mouse.x);
int16_t y = (int16_t)(rc_can->data.mouse.y);
int16_t z = (int16_t)(rc_can->data.mouse.z);
frame.data[0] = (uint8_t)(x & 0xFF);
frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
frame.data[2] = (uint8_t)(y & 0xFF);
frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
frame.data[4] = (uint8_t)(z & 0xFF);
frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
break;
}
case RC_CAN_DATA_KEYBOARD: {
frame.id = rc_can->param.keyboard_id;
frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
for (int i = 0; i < 6; i++) {
frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
}
break;
}
default:
return DEVICE_ERR;
}
#undef RC_CAN_CLAMP
if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
frame.data, frame.dlc) != BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 接收并更新CAN数据
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Update(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
// 只有从机模式才能接收数据
if (rc_can->param.mode != RC_CAN_MODE_SLAVE) {
return DEVICE_ERR;
}
BSP_CAN_Message_t msg;
switch (data_type) {
case RC_CAN_DATA_JOYSTICK:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.joy_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
int16_t ch_l_x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
int16_t ch_l_y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
int16_t ch_r_x = (int16_t)((msg.data[5] << 8) | msg.data[4]);
int16_t ch_r_y = (int16_t)((msg.data[7] << 8) | msg.data[6]);
// 转换为浮点数（范围：-1.0到1.0）
rc_can->data.joy.ch_l_x = (float)ch_l_x / 32768.0f;
rc_can->data.joy.ch_l_y = (float)ch_l_y / 32768.0f;
rc_can->data.joy.ch_r_x = (float)ch_r_x / 32768.0f;
rc_can->data.joy.ch_r_y = (float)ch_r_y / 32768.0f;
break;
case RC_CAN_DATA_SWITCH:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.sw_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
rc_can->data.sw.sw_l = (RC_CAN_SW_t)msg.data[0];
rc_can->data.sw.sw_r = (RC_CAN_SW_t)msg.data[1];
int16_t ch_res = (int16_t)((msg.data[3] << 8) | msg.data[2]);
rc_can->data.sw.ch_res = (float)ch_res / 32768.0f;
break;
case RC_CAN_DATA_MOUSE:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.mouse_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
int16_t x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
int16_t y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
int16_t z = (int16_t)((msg.data[5] << 8) | msg.data[4]);
rc_can->data.mouse.x = (float)x;
rc_can->data.mouse.y = (float)y;
rc_can->data.mouse.z = (float)z;
rc_can->data.mouse.mouse_l = (msg.data[6] & 0x01) ? true : false;
rc_can->data.mouse.mouse_r = (msg.data[7] & 0x01) ? true : false;
break;
case RC_CAN_DATA_KEYBOARD:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.keyboard_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
if (msg.dlc < 2) {
return DEVICE_ERR;
}
// 解包数据
rc_can->data.keyboard.key_value =
(uint16_t)((msg.data[1] << 8) | msg.data[0]);
for (int i = 0; i < 6 && (i + 2) < msg.dlc; i++) {
rc_can->data.keyboard.keys[i] = (RC_CAN_Key_t)(msg.data[2 + i]);
}
// 清空未使用的按键位置
for (int i = (msg.dlc > 2 ? msg.dlc - 2 : 0); i < 6; i++) {
rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
}
break;
default:
return DEVICE_ERR;
}
// 更新header状态
rc_can->header.online = true;
rc_can->header.last_online_time = BSP_TIME_Get_us();
return DEVICE_OK;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 验证RC_CAN参数
* @param param 参数指针
* @return DEVICE_OK 成功，其他值失败
*/
static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
// 检查CAN总线有效性
if (param->can >= BSP_CAN_NUM) {
return DEVICE_ERR;
}
// 检查工作模式有效性
if (param->mode != RC_CAN_MODE_MASTER && param->mode != RC_CAN_MODE_SLAVE) {
return DEVICE_ERR;
}
// 检查CAN ID是否重复
if (param->joy_id == param->sw_id || param->joy_id == param->mouse_id ||
param->joy_id == param->keyboard_id || param->sw_id == param->mouse_id ||
param->sw_id == param->keyboard_id ||
param->mouse_id == param->keyboard_id) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 注册CAN ID
* @param rc_can RC_CAN结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can) {
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.joy_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.sw_id, 0) != BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.mouse_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.keyboard_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can){
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
rc_can->header.online = false;
return DEVICE_OK;
}
/* USER CODE BEGIN */
/* USER CODE END */

157
User/device/rc_can.h
View File

@ -1,157 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/can.h"
#include "device/device.h"
#include <stdint.h>
#include <stdbool.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
RC_CAN_SW_ERR = 0,
RC_CAN_SW_UP = 1,
RC_CAN_SW_MID = 3,
RC_CAN_SW_DOWN = 2,
} RC_CAN_SW_t;
typedef enum {
RC_CAN_MODE_MASTER = 0, // 主机模式
RC_CAN_MODE_SLAVE = 1, // 从机模式
} RC_CAN_Mode_t;
typedef enum {
RC_CAN_DATA_JOYSTICK = 0,
RC_CAN_DATA_SWITCH,
RC_CAN_DATA_MOUSE,
RC_CAN_DATA_KEYBOARD
} RC_CAN_DataType_t;
typedef enum {
RC_CAN_KEY_NONE = 0xFF, // 无按键
RC_CAN_KEY_W = 0,
RC_CAN_KEY_S,
RC_CAN_KEY_A,
RC_CAN_KEY_D,
RC_CAN_KEY_SHIFT,
RC_CAN_KEY_CTRL,
RC_CAN_KEY_Q,
RC_CAN_KEY_E,
RC_CAN_KEY_R,
RC_CAN_KEY_F,
RC_CAN_KEY_G,
RC_CAN_KEY_Z,
RC_CAN_KEY_X,
RC_CAN_KEY_C,
RC_CAN_KEY_V,
RC_CAN_KEY_B,
RC_CAN_KEY_NUM,
} RC_CAN_Key_t;
// 遥杆数据包
typedef struct {
float ch_l_x;
float ch_l_y;
float ch_r_x;
float ch_r_y;
} RC_CAN_JoyData_t;
// 拨杆数据包
typedef struct {
RC_CAN_SW_t sw_l; // 左拨杆状态
RC_CAN_SW_t sw_r; // 右拨杆状态
float ch_res; // 第五通道
} RC_CAN_SwitchData_t;
// 鼠标数据包
typedef struct {
float x; // 鼠标X轴移动
float y; // 鼠标Y轴移动
float z; // 鼠标Z轴(滚轮)
bool mouse_l; // 鼠标左键
bool mouse_r; // 鼠标右键
} RC_CAN_MouseData_t;
// 键盘数据包
typedef struct {
uint16_t key_value; // 键盘按键位映射
RC_CAN_Key_t keys[16];
} RC_CAN_KeyboardData_t;
typedef struct {
RC_CAN_JoyData_t joy;
RC_CAN_SwitchData_t sw;
RC_CAN_MouseData_t mouse;
RC_CAN_KeyboardData_t keyboard;
} RC_CAN_Data_t;
// RC_CAN 参数结构
typedef struct {
BSP_CAN_t can; // 使用的CAN总线
RC_CAN_Mode_t mode; // 工作模式
uint16_t joy_id; // 遥杆CAN ID
uint16_t sw_id; // 拨杆CAN ID
uint16_t mouse_id; // 鼠标CAN ID
uint16_t keyboard_id; // 键盘CAN ID
} RC_CAN_Param_t;
// RC_CAN 主结构
typedef struct {
DEVICE_Header_t header;
RC_CAN_Param_t param;
RC_CAN_Data_t data;
} RC_CAN_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化RC CAN发送模块
* @param rc_can RC_CAN结构体指针
* @param param 初始化参数
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param);
/**
* @brief 更新并发送数据到CAN总线
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type);
/**
* @brief 接收并更新CAN数据
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Update(RC_CAN_t *rc_can , RC_CAN_DataType_t data_type);
int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

11
User/device/virtual_chassis.c
View File

@ -339,16 +339,5 @@ int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left
return (result1 == BSP_OK && result2 == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 检查虚拟底盘是否在线
*/
bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis) {
if (chassis == NULL) {
return false;
}
return chassis->header.online;
}
/* Private functions -------------------------------------------------------- */

7
User/device/virtual_chassis.h
View File

@ -113,13 +113,6 @@ int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques
*/
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]);
/**
* @brief 检查虚拟底盘是否在线
* @param chassis 虚拟底盘设备结构体指针
* @return true 在线，false 离线
*/
bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis);
#ifdef __cplusplus
}
#endif

121
User/module/balance_chassis.c
View File

@ -1,15 +1,18 @@
#include "device/motor_lz.h"
#define _USE_MATH_DEFINES
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/kinematics.h"
#include "component/limiter.h"
#include "component/user_math.h"
#include "device/virtual_chassis.h"
#include "module/balance_chassis.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
static Virtual_Chassis_t virtual_chassis;
/**
* @brief 使能所有电机
* @param c 底盘结构体指针
@ -18,24 +21,24 @@
static int8_t Chassis_MotorEnable(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) {
// MOTOR_LZ_Enable(&c->param->joint_motors[i]);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
}
// for (int i = 0; i < 4; i++) {
// // MOTOR_LZ_Enable(&c->param->joint_motors[i]);
// }
// for (int i = 0; i < 2; i++) {
// MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
// }
return CHASSIS_OK;
}
static int8_t Chassis_MotorRelax(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 2; i++) {
// MOTOR_LK_Relax(&c->param->wheel_motors[i]);
}
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Relax(&c->param->joint_motors[i]);
}
// for (int i = 0; i < 2; i++) {
// // MOTOR_LK_Relax(&c->param->wheel_motors[i]);
// }
// for (int i = 0; i < 4; i++) {
// MOTOR_LZ_Relax(&c->param->joint_motors[i]);
// }
return CHASSIS_OK;
}
@ -110,12 +113,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
/*初始化和注册所有电机*/
MOTOR_LZ_Init();
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Register(&c->param->joint_motors[i]);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Register(&c->param->wheel_motors[i]);
}
Virtual_Chassis_Init(&virtual_chassis, &c->param->virtual_chassis_param);
MOTOR_DM_Register(&c->param->yaw_motor);
/*初始化VMC控制器*/
@ -154,35 +152,22 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
if (c == NULL) {
return -1; // 参数错误
}
/*更新电机反馈*/
Virtual_Chassis_Update(&virtual_chassis);
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Update(&c->param->joint_motors[i]);
}
MOTOR_LK_Update(&c->param->wheel_motors[0]);
MOTOR_LK_Update(&c->param->wheel_motors[1]);
/*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
// 更新关节电机反馈并检查离线状态
for (int i = 0; i < 4; i++) {
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
if (joint_motor != NULL) {
if (joint_motor->motor.feedback.rotor_abs_angle > M_PI) {
joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
joint_motor->motor.feedback.rotor_abs_angle =
-joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整
c->feedback.joint[i] = joint_motor->motor.feedback;
c->feedback.joint[i] = virtual_chassis.data.joint[i];
if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
c->feedback.joint[i].rotor_abs_angle -= M_2PI;
}
c->feedback.joint[i].rotor_abs_angle =
-c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
}
// 更新轮子电机反馈并检查离线状态
for (int i = 0; i < 2; i++) {
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback;
}
c->feedback.wheel[i] = virtual_chassis.data.wheel[i];
}
c->feedback.imu.accl = virtual_chassis.data.imu.accl;
c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
c->feedback.imu.euler = virtual_chassis.data.imu.euler;
/* 更新云台电机反馈数据（yaw轴） */
MOTOR_DM_Update(&(c->param->yaw_motor));
@ -240,11 +225,11 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
fn = -25.0f;
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
&c->output.joint[1].torque);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
&c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
&c->output.joint[2].torque);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
&c->output.joint[2]);
// Chassis_MotorEnable(c);
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
@ -270,16 +255,16 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
void Chassis_Output(Chassis_t *c) {
if (c == NULL)
return;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_MotionParam_t param = {0};
param.torque = c->output.joint[i].torque;
// MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
}
BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
}
// for (int i = 0; i < 4; i++) {
// // MOTOR_LZ_MotionParam_t param = {0};
// // param.torque = c->output.joint[i];
// // MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
// }
// BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
// for (int i = 0; i < 2; i++) {
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
// // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
// }
}
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
@ -395,12 +380,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
&c->output.joint[1].torque);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
&c->output.joint[1]);
} else {
// VMC失败，设为0力矩
c->output.joint[0].torque = 0.0f;
c->output.joint[1].torque = 0.0f;
c->output.joint[0]= 0.0f;
c->output.joint[1]= 0.0f;
}
}
@ -416,12 +401,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
&c->output.joint[2].torque);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
&c->output.joint[2]);
} else {
// VMC失败，设为0力矩
c->output.joint[2].torque = 0.0f;
c->output.joint[3].torque = 0.0f;
c->output.joint[2] = 0.0f;
c->output.joint[3] = 0.0f;
}
}
@ -433,9 +418,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
}
for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i].torque) > 20.0f) {
c->output.joint[i].torque =
(c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
if (fabsf(c->output.joint[i]) > 20.0f) {
c->output.joint[i] =
(c->output.joint[i] > 0) ? 20.0f : -20.0f;
}
}

5
User/module/balance_chassis.h
View File

@ -62,7 +62,7 @@ typedef struct {
}Chassis_Feedback_t;
typedef struct {
MOTOR_LZ_MotionParam_t joint[4]; /* 四个关节电机输出 */
float joint[4]; /* 四个关节电机输出 */
float wheel[2]; /* 两个轮子电机输出 */
}Chassis_Output_t;
@ -70,8 +70,7 @@ typedef struct {
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
Virtual_Chassis_Param_t virtual_chassis_param; // 虚拟底盘参数
MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */
VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */

125
User/module/config.c
View File

@ -6,7 +6,7 @@
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor_dm.h"
#include "device/rc_can.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
@ -85,54 +85,7 @@ Config_RobotParam_t robot_config = {
.in = 30.0f,
.out = 30.0f,
},
.joint_motors = {
{ // 左髋关节
.can = BSP_CAN_1,
.motor_id = 124,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 左膝关节
.can = BSP_CAN_1,
.motor_id = 125,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右膝关节
.can = BSP_CAN_1,
.motor_id = 126,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右髋关节
.can = BSP_CAN_1,
.motor_id = 127,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
},
.wheel_motors = {
{ // 左轮电机
.can = BSP_CAN_1,
.id = 0x141,
.module = MOTOR_LK_MF9025,
.reverse = false,
},
{ // 右轮电机
.can = BSP_CAN_1,
.id = 0x142,
.module = MOTOR_LK_MF9025,
.reverse = true,
},
},
.yaw_motor = { // 云台Yaw轴电机
.can = BSP_CAN_2,
.can_id = 0x1,
@ -159,50 +112,42 @@ Config_RobotParam_t robot_config = {
.hip_length = 0.0f // 髋宽 (L5) (m)
}
},
.lqr_gains = {
.k11_coeff = { -1.916376919295156e+02f, 2.009487240966917e+02f, -9.481460086781939e+01f, -4.748704486775178e+00f }, // theta
.k12_coeff = { -1.794538872095484e+00f, -1.557720009681701e-02f, -7.253705624763870e+00f, -9.066344876912042e-01f }, // d_theta
.k13_coeff = { -5.530128764310525e+01f, 5.441066051951745e+01f, -1.855725101721958e+01f, -1.532320658646968e+00f }, // x
.k14_coeff = { -5.226956984912729e+01f, 5.134367619659140e+01f, -1.878250555345751e+01f, -2.194040977657715e+00f }, // d_x
.k15_coeff = { -8.041587662748894e+00f, 1.194985309170939e+01f, -6.358536570979702e+00f, 8.475914154881636e-01f }, // phi
.k16_coeff = { -1.171602430557222e+01f, 1.424681188601595e+01f, -6.769563511869035e+00f, 1.358696951640962e+00f }, // d_phi
.k21_coeff = { -1.589916277171124e+01f, 3.688717311486668e+01f, -2.693106123880470e+01f, 8.468880380216705e+00f }, // theta
.k22_coeff = { -4.056650578339882e+00f, 5.627051302682392e+00f, -2.802037254431724e+00f, 1.258712322539219e+00f }, // d_theta
.k23_coeff = { -4.615940205649521e+01f, 5.298830026615487e+01f, -2.321905486010759e+01f, 4.733000143959848e+00f }, // x
.k24_coeff = { -6.198443605956307e+01f, 6.867018097135634e+01f, -2.873375258895661e+01f, 5.632056979434964e+00f }, // d_x
.k25_coeff = { -1.149934231218892e+01f, 9.960303123832579e+00f, -2.826210920406189e+00f, 4.384038656352771e+00f }, // phi
.k26_coeff = { 1.452344963148120e+01f, -1.451377402367933e+01f, 5.020766559053281e+00f, 1.099371055071753e+00f }, // d_phi
},
.lqr_gains = {
.k11_coeff = { -1.916376919295156e+02f, 2.009487240966917e+02f, -9.481460086781939e+01f, -4.748704486775178e+00f }, // theta
.k12_coeff = { -1.794538872095484e+00f, -1.557720009681701e-02f, -7.253705624763870e+00f, -9.066344876912042e-01f }, // d_theta
.k13_coeff = { -5.530128764310525e+01f, 5.441066051951745e+01f, -1.855725101721958e+01f, -1.532320658646968e+00f }, // x
.k14_coeff = { -5.226956984912729e+01f, 5.134367619659140e+01f, -1.878250555345751e+01f, -2.194040977657715e+00f }, // d_x
.k15_coeff = { -8.041587662748894e+00f, 1.194985309170939e+01f, -6.358536570979702e+00f, 8.475914154881636e-01f }, // phi
.k16_coeff = { -1.171602430557222e+01f, 1.424681188601595e+01f, -6.769563511869035e+00f, 1.358696951640962e+00f }, // d_phi
.k21_coeff = { -1.589916277171124e+01f, 3.688717311486668e+01f, -2.693106123880470e+01f, 8.468880380216705e+00f }, // theta
.k22_coeff = { -4.056650578339882e+00f, 5.627051302682392e+00f, -2.802037254431724e+00f, 1.258712322539219e+00f }, // d_theta
.k23_coeff = { -4.615940205649521e+01f, 5.298830026615487e+01f, -2.321905486010759e+01f, 4.733000143959848e+00f }, // x
.k24_coeff = { -6.198443605956307e+01f, 6.867018097135634e+01f, -2.873375258895661e+01f, 5.632056979434964e+00f }, // d_x
.k25_coeff = { -1.149934231218892e+01f, 9.960303123832579e+00f, -2.826210920406189e+00f, 4.384038656352771e+00f }, // phi
.k26_coeff = { 1.452344963148120e+01f, -1.451377402367933e+01f, 5.020766559053281e+00f, 1.099371055071753e+00f }, // d_phi
},
.virtual_chassis_param = {
.motors = {
.can = BSP_CAN_1,
.enable_id = 0x121,
.joint_cmd_id = 0x122,
.joint_feedback_base_id = 124,
.wheel_left_id = 0x128,
.wheel_right_id = 0x129,
.wheel_left_feedback_id = 0x82,
.wheel_right_feedback_id = 0x83,
},
.imu = {
.can = BSP_CAN_1,
.accl_id = 0x301,
.gyro_id = 0x302,
.euler_id = 0x303,
.quat_id = 0x304,
},
},
},
.rc_can_param = {
.can = BSP_CAN_2,
.mode = RC_CAN_MODE_SLAVE,
.joy_id = 0x30,
.sw_id = 0x31,
.mouse_id = 0x32,
.keyboard_id = 0x33,
},
.virtual_chassis_param = {
.motors = {
.can = BSP_CAN_1,
.enable_id = 0x121,
.joint_cmd_id = 0x122,
.joint_feedback_base_id = 124,
.wheel_left_id = 0x128,
.wheel_right_id = 0x129,
.wheel_left_feedback_id = 0x82,
.wheel_right_feedback_id = 0x83,
},
.imu = {
.can = BSP_CAN_1,
.accl_id = 0x301,
.gyro_id = 0x302,
.euler_id = 0x303,
.quat_id = 0x304,
},
},
};
/* Private function prototypes ---------------------------------------------- */

2
User/module/config.h
View File

@ -13,13 +13,11 @@ extern "C" {
#include "device/motor_lz.h"
#include "device/motor_lk.h"
#include "module/balance_chassis.h"
#include "device/rc_can.h"
#include "device/virtual_chassis.h"
typedef struct {
DM_IMU_Param_t imu_param;
Chassis_Params_t chassis_param;
RC_CAN_Param_t rc_can_param;
Virtual_Chassis_Param_t virtual_chassis_param; // 虚拟底盘参数
} Config_RobotParam_t;

9
User/task/ctrl_chassis.c
View File

@ -31,7 +31,7 @@ Chassis_CMD_t chassis_cmd = {
.height = 0.0f,
};
Chassis_IMU_t chassis_imu;
Virtual_Chassis_t virtual_chassis; // 添加虚拟底盘设备
// Virtual_Chassis_t virtual_chassis; // 添加虚拟底盘设备
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -50,19 +50,16 @@ void Task_ctrl_chassis(void *argument) {
Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
Virtual_Chassis_Init(&virtual_chassis, &Config_GetRobotParam()->virtual_chassis_param);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
Virtual_Chassis_Update(&virtual_chassis);
// Chassis_UpdateFeedback(&chassis);
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

80
User/task/rc.c
View File

@ -1,15 +1,18 @@
/*
rc Task
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/rc_can.h"
#include "module/balance_chassis.h"
#include "device/dr16.h"
#include "module/config.h"
// #include "module/shoot.h"
// #include "module/gimbal.h"
#include <stdbool.h>
// #include
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -17,8 +20,10 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
RC_CAN_t rc_can;
Chassis_CMD_t cmd_to_chassis;
DR16_t dr16;
// RC_CAN_t rc_can;
// Shoot_CMD_t for_shoot;
// Gimbal_CMD_t cmd_for_gimbal;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -26,50 +31,41 @@ Chassis_CMD_t cmd_to_chassis;
void Task_rc(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_CAN_Init();
RC_CAN_Init(&rc_can, &Config_GetRobotParam()->rc_can_param);
DR16_Init(&dr16);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
RC_CAN_Update(&rc_can, RC_CAN_DATA_JOYSTICK);
RC_CAN_Update(&rc_can, RC_CAN_DATA_SWITCH);
if (rc_can.header.online) {
switch (rc_can.data.sw.sw_l) {
case RC_CAN_SW_UP: // 上位
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
case RC_CAN_SW_MID: // 中位
cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
break;
case RC_CAN_SW_DOWN: // 下位
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
default:
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
cmd_to_chassis.move_vec.vx = rc_can.data.joy.ch_l_y;
cmd_to_chassis.move_vec.vy = rc_can.data.joy.ch_l_x;
cmd_to_chassis.move_vec.wz = rc_can.data.joy.ch_r_x;
cmd_to_chassis.height = rc_can.data.sw.ch_res;
DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) {
DR16_ParseData(&dr16);
} else {
RC_CAN_OFFLINE(&rc_can);
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
DR16_Offline(&dr16);
}
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
// for_shoot.online = dr16.header.online;
// switch (dr16.data.sw_r) {
// case DR16_SW_UP:
// for_shoot.ready = false;
// for_shoot.firecmd = false;
// break;
// case DR16_SW_MID:
// for_shoot.ready = true;
// for_shoot.firecmd = false;
// break;
// case DR16_SW_DOWN:
// for_shoot.ready = true;
// for_shoot.firecmd = true;
// break;
// default:
// for_shoot.ready = false;
// for_shoot.firecmd = false;
// break;
// }
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}