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 16:24:06 +08:00
parent db9d9f7db5
commit 21b565eaa9
4 changed files with 35 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
User/module/balance_chassis.c
View File

@ -21,12 +21,9 @@ static Virtual_Chassis_t virtual_chassis;
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]);
// }
Virtual_Chassis_Enable(&virtual_chassis);
return CHASSIS_OK;
}
@ -35,7 +32,7 @@ static int8_t Chassis_MotorRelax(Chassis_t *c) {
return CHASSIS_ERR_NULL;
float relax_torque[4] = {0.0f, 0.0f, 0.0f, 0.0f};
Virtual_Chassis_SendJointTorque(&virtual_chassis, relax_torque);
Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.1f, 0.1f);
Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f);
return CHASSIS_OK;
}
@ -221,7 +218,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
case CHASSIS_MODE_RECOVER: {
float fn;
fn = -25.0f;
fn = -15.0f;
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
@ -264,6 +261,11 @@ void Chassis_Output(Chassis_t *c) {
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
// // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
// }
float out[4] = {
c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]};
Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]);
// for (int i = 0; i < 2; i++) {
}
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {

3
User/task/ctrl_chassis.c
View File

@ -55,6 +55,9 @@ void Task_ctrl_chassis(void *argument) {
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) != osOK) {
// 没有新命令，保持上次命令
}
Chassis_UpdateFeedback(&chassis);

98
User/task/ctrl_virtual_chassis.c
View File

@ -1,98 +0,0 @@
/*
ctrl_virtual_chassis Task - 使用虚拟底盘设备的示例任务
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/virtual_chassis.h"
#include "module/config.h"
#include "bsp/time.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
static Virtual_Chassis_t virtual_chassis;
static bool chassis_enabled = false;
static float test_torques[VIRTUAL_CHASSIS_MOTOR_COUNT] = {0.0f, 0.0f, 0.0f, 0.0f};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_ctrl_virtual_chassis(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / 100.0f; // 100Hz频率
osDelay(500); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
// 初始化虚拟底盘
if (Virtual_Chassis_Init(&virtual_chassis, &Config_GetRobotParam()->virtual_chassis_param) != DEVICE_OK) {
// 初始化失败处理
return;
}
// 使能底盘电机
Virtual_Chassis_Enable(&virtual_chassis);
chassis_enabled = true;
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// 更新虚拟底盘数据（接收反馈）
Virtual_Chassis_Update(&virtual_chassis);
// 检查设备在线状态
if (Virtual_Chassis_IsOnline(&virtual_chassis)) {
// 发送力矩控制命令（这里可以根据实际需要设置力矩值）
if (chassis_enabled) {
Virtual_Chassis_SendTorqueCommand(&virtual_chassis, test_torques);
}
// 获取电机反馈数据
for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) {
Virtual_Chassis_MotorFeedback_t *motor_fb = Virtual_Chassis_GetMotorFeedback(&virtual_chassis, i);
if (motor_fb != NULL && motor_fb->online) {
// 这里可以使用电机反馈数据
// 例如：发送到其他任务或用于控制算法
// motor_fb->current_angle, motor_fb->current_velocity, motor_fb->current_torque
}
}
// 获取IMU反馈数据
Virtual_Chassis_IMUFeedback_t *imu_fb = Virtual_Chassis_GetIMUFeedback(&virtual_chassis);
if (imu_fb != NULL && imu_fb->online) {
// 这里可以使用IMU数据替换原有的陀螺仪数据
// 例如：将数据发送给底盘控制任务
Chassis_IMU_t chassis_imu = {
.accl = imu_fb->accl,
.gyro = imu_fb->gyro,
.euler = imu_fb->euler,
};
// 发送IMU数据到底盘控制任务
osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu, 0, 0);
}
} else {
// 设备离线，重新使能
if (chassis_enabled) {
Virtual_Chassis_Enable(&virtual_chassis);
}
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

22
User/task/rc.c
View File

@ -9,6 +9,7 @@
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "module/config.h"
#include "module/balance_chassis.h"
// #include "module/shoot.h"
// #include "module/gimbal.h"
#include <stdbool.h>
@ -21,6 +22,7 @@
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
DR16_t dr16;
Chassis_CMD_t cmd_for_chassis;
// RC_CAN_t rc_can;
// Shoot_CMD_t for_shoot;
// Gimbal_CMD_t cmd_for_gimbal;
@ -46,6 +48,26 @@ void Task_rc(void *argument) {
} else {
DR16_Offline(&dr16);
}
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
break;
case DR16_SW_DOWN:
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
osMessageQueueReset(task_runtime.msgq.Chassis_cmd); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0, 0); // 非阻塞发送底盘控制命令
// for_shoot.online = dr16.header.online;
// switch (dr16.data.sw_r) {
// case DR16_SW_UP: