diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b8a186..be7b042 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/User/device/rc_can.c b/User/device/rc_can.c deleted file mode 100644 index 77d9733..0000000 --- a/User/device/rc_can.c +++ /dev/null @@ -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 */ diff --git a/User/device/rc_can.h b/User/device/rc_can.h deleted file mode 100644 index e5c6d90..0000000 --- a/User/device/rc_can.h +++ /dev/null @@ -1,157 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include "bsp/can.h" -#include "device/device.h" -#include -#include - -/* 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 diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c index c21cb3a..95a94d2 100644 --- a/User/device/virtual_chassis.c +++ b/User/device/virtual_chassis.c @@ -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 -------------------------------------------------------- */ diff --git a/User/device/virtual_chassis.h b/User/device/virtual_chassis.h index 3b6eddd..51d8c25 100644 --- a/User/device/virtual_chassis.h +++ b/User/device/virtual_chassis.h @@ -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 \ No newline at end of file diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 2e6f1b3..1993efd 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -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 #include #include + +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], ¶m); - } - 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], ¶m); + // } + // 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; } } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 88bb825..63c67d0 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -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增益矩阵参数 */ diff --git a/User/module/config.c b/User/module/config.c index c716e68..2cdd2cf 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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 ---------------------------------------------- */ diff --git a/User/module/config.h b/User/module/config.h index ef1adc5..9b6e362 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -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; diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 369d0a9..d18cc04 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -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); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/rc.c b/User/task/rc.c index 5a13b90..3f9738d 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -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 +// #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); /* 运行结束，等待下一次唤醒 */ } - } \ No newline at end of file