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 2026-01-11 16:24:35 +08:00
parent 93759801cc
commit 4aab1a909d
9 changed files with 5741 additions and 5702 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -165,6 +165,11 @@
<WinNumber>1</WinNumber>
<ItemText>shoot</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>dr16</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

11389
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

2
User/component/vmc.c
View File

@ -80,7 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
if (vmc == NULL || !vmc->initialized) {
return -1;
}
body_pitch = -body_pitch; // 机体俯仰角取反
// body_pitch = body_pitch; // 机体俯仰角取反
VMC_Leg_t *leg = &vmc->leg;

1
User/component/vmc.h
View File

@ -8,7 +8,6 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/* Exported types ----------------------------------------------------------- */
/**

14
User/module/balance_chassis.c
View File

@ -3,6 +3,7 @@
#include "bsp/time.h"
#include "component/filter.h"
#include "component/user_math.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include <math.h>
#include <stddef.h>
@ -248,6 +249,15 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
}
}
for (int i = 0; i < 4; 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 < 4; i++) {
// c->feedback.joint[i] = virtual_chassis.data.joint[i];
// if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
@ -394,6 +404,7 @@ void Chassis_Output(Chassis_t *c) {
// 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);
MOTOR_LZ_MotionParam_t motion_param = {
.torque=0.0f,
.target_angle=0.0f,
@ -402,12 +413,15 @@ void Chassis_Output(Chassis_t *c) {
.kd=0.0f,
};
for (int i = 0; i < 4; i++) {
motion_param.torque = c->output.joint[i];
MOTOR_LZ_MotionControl(&c->param->joint_param[i],&motion_param);
}
c->output.wheel[0] =
LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]);
c->output.wheel[1] =
LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
// c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,

8
User/module/config.c
View File

@ -98,7 +98,7 @@ Config_RobotParam_t robot_config = {
.chassis_param = {
.yaw={
.k=1.0f,
.k=0.0f,
.p=1.0f,
.i=0.0f,
.d=0.3f,
@ -109,7 +109,7 @@ Config_RobotParam_t robot_config = {
},
.roll={
.k=1.0f,
.k=0.2f,
.p=0.5f, /* 横滚角比例系数 */
.i=0.01f, /* 横滚角积分系数 */
.d=0.01f, /* 横滚角微分系数 */
@ -203,13 +203,13 @@ Config_RobotParam_t robot_config = {
.wheel_param = {
{ // 左轮电机
.can = BSP_CAN_3,
.id = 0x141,
.id = 0x142,
.module = MOTOR_LK_MF9025,
.reverse = false,
},
{ // 右轮电机
.can = BSP_CAN_3,
.id = 0x142,
.id = 0x141,
.module = MOTOR_LK_MF9025,
.reverse = true,
},

15
User/task/atti_esit.c
View File

@ -82,9 +82,18 @@ void Task_atti_esit(void *argument) {
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
imu_for_chassis.accl = bmi088.accl;
imu_for_chassis.gyro = bmi088.gyro;
imu_for_chassis.euler = eulr_to_send;
/* 加速度计数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
imu_for_chassis.accl.x = bmi088.accl.y;
imu_for_chassis.accl.y = -bmi088.accl.x;
imu_for_chassis.accl.z = bmi088.accl.z;
/* 陀螺仪数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
imu_for_chassis.gyro.x = bmi088.gyro.y;
imu_for_chassis.gyro.y = -bmi088.gyro.x;
imu_for_chassis.gyro.z = bmi088.gyro.z;
/* 欧拉角绕z轴旋转270度 */
imu_for_chassis.euler.rol = eulr_to_send.pit;
imu_for_chassis.euler.pit = -eulr_to_send.rol;
imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度
osMessageQueueReset(
task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.imu, &imu_for_chassis, 0,

9
User/task/rc.c
View File

@ -105,13 +105,14 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;