TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Codex generated changes:

Browse Source 
1.stool-mode
2.INS direction changes
not tested,not ok
This commit is contained in:
TuxMonkey
2026-07-14 01:07:48 +08:00
parent a4ebc4b452
commit 8a13444e48
7 changed files with 286 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

415
application/chassis/balance.c
View File

@@ -21,6 +21,22 @@
#include "speed_estimation.h" #include "speed_estimation.h"
#include "fly_detection.h" #include "fly_detection.h"
#include "buzzer.h" #include "buzzer.h"
#include "math.h"
#include "string.h"
#define STOOL_SPEED_REF 1.5f
#define STOOL_YAW_RATE_REF 3.5f
#define STOOL_ACC_REF 4.0f
#define STOOL_BRAKE_ACC_REF 8.0f
#define STOOL_PITCH_K 55.0f
#define STOOL_PITCH_W_K 7.0f
#define STOOL_VEL_K 10.0f
#define STOOL_STOP_DIST_K 8.0f
#define STOOL_VEL_LPF_RC 0.03f
#define STOOL_WHEEL_TORQUE_LIMIT 25.0f
#define STOOL_RC_DEADBAND 40
#define CHASSIS_UNDERVOLTAGE_LIMIT 15.0f
#define REFEREE_VOLTAGE_VALID_MIN 5.0f
// 计时变量 // 计时变量
static uint32_t balance_dwt_cnt; static uint32_t balance_dwt_cnt;
static float del_t; static float del_t;
@@ -29,8 +45,6 @@ static float del_t;
static INS_t *Chassis_IMU_data; static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data; // 底盘单独调试用 static RC_ctrl_t *rc_data; // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
static Chassis_Can_Comm chassis_can_recv;
// 四个关节电机和两个驱动轮电机 // 四个关节电机和两个驱动轮电机
static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2]; static LKMotorInstance *l_driven, *r_driven, *driven[2];
@@ -51,10 +65,112 @@ static Robot_Status_e chassis_status;
static referee_info_t *referee_data; // 用于获取裁判系统的数据 static referee_info_t *referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
static SuperCapInstance *cap; // 超级电容 static SuperCapInstance *cap; // 超级电容
static uint16_t DataSend2Cap[4] = {0, 0, 0, 0};
static uint8_t stool_drive_key_pressed;
static float stool_vel;
static float stool_dist;
static float stool_stop_dist;
static float stool_target_wz;
static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
static void ClearMotionTargets(void)
{
chassis.target_v = 0.0f;
chassis.target_dist = chassis.dist;
stool_target_wz = 0.0f;
}
static void ClearControlOutput(void)
{
l_side.F_leg = 0.0f;
l_side.T_hip = 0.0f;
l_side.T_back = 0.0f;
l_side.T_front = 0.0f;
l_side.T_wheel = 0.0f;
r_side.F_leg = 0.0f;
r_side.T_hip = 0.0f;
r_side.T_back = 0.0f;
r_side.T_front = 0.0f;
r_side.T_wheel = 0.0f;
}
static void ClearJointOutput(void)
{
l_side.F_leg = 0.0f;
l_side.T_hip = 0.0f;
l_side.T_back = 0.0f;
l_side.T_front = 0.0f;
r_side.F_leg = 0.0f;
r_side.T_hip = 0.0f;
r_side.T_back = 0.0f;
r_side.T_front = 0.0f;
}
static void ApproachFloat(float *value, float target, float step)
{
if (*value < target)
{
*value += step;
if (*value > target)
*value = target;
}
else if (*value > target)
{
*value -= step;
if (*value < target)
*value = target;
}
}
static float RCChannelToRatio(int16_t ch)
{
if (ch > -STOOL_RC_DEADBAND && ch < STOOL_RC_DEADBAND)
return 0.0f;
return float_constrain((float)ch / 660.0f, -1.0f, 1.0f);
}
static void ResetStoolRuntime(void)
{
stool_drive_key_pressed = 0;
stool_vel = 0.0f;
stool_dist = chassis.dist;
stool_stop_dist = stool_dist;
ClearMotionTargets();
ClearControlOutput();
}
static void DisableAllJointMotor(void)
{
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
DMMotorSetRef(joint[i], 0.0f);
DMMotorOuterLoop(joint[i], OPEN_LOOP);
DMMotorStop(joint[i]);
}
}
static void RefreshJointMotorDisable(void)
{
DisableAllJointMotor();
}
static void EnableDrivenMotor(void)
{
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
LKMotorEnable(driven[i]);
}
static void StopDrivenMotor(void)
{
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
{
LKMotorSetRef(driven[i], 0.0f);
LKMotorStop(driven[i]);
}
}
void BalanceInit() void BalanceInit()
{ {
@@ -68,18 +184,6 @@ void BalanceInit()
.rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
}}; }};
cap = SuperCapInit(&cap_conf); // ww超级电容初始化 cap = SuperCapInit(&cap_conf); // ww超级电容初始化
CANComm_Init_Config_s comm_conf = {
.can_config = {
.can_handle = &hcan2,
.tx_id = 0x311,
.rx_id = 0x312,
},
.daemon_count = 100,
.recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
.send_data_len = sizeof(Chassis_Upload_Data_s),
};
cmd_can_comm = CANCommInit(&comm_conf);
// 关节电机 // 关节电机
Motor_Init_Config_s joint_conf = { Motor_Init_Config_s joint_conf = {
// 写一个,剩下的修改方向和id即可 // 写一个,剩下的修改方向和id即可
@@ -200,28 +304,14 @@ void BalanceInit()
// 状态初始化 // 状态初始化
l_side.target_len = r_side.target_len = 0.12; l_side.target_len = r_side.target_len = 0.12;
chassis.vel_cov = 100; // 速度协方差初始化 chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY; chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
DWT_GetDeltaT(&balance_dwt_cnt); chassis_status = ROBOT_STOP;
} ResetStoolRuntime();
DisableAllJointMotor();
static void EnableAllMotor() /* 打开所有电机 */
{
for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
DMMotorEnable(joint[i]);
for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
LKMotorEnable(driven[i]);
}
// 检查关节电机是否离线
static uint8_t JointMotorIsLost()
{
for (uint8_t i = 0; i < JOINT_CNT; i++) for (uint8_t i = 0; i < JOINT_CNT; i++)
{ DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]);
if (joint[i]->motor_daemon->temp_count == 0) StopDrivenMotor();
return 1; DWT_GetDeltaT(&balance_dwt_cnt);
}
return 0;
} }
// 检查驱动轮电机是否离线 // 检查驱动轮电机是否离线
@@ -236,147 +326,100 @@ static uint8_t DrivenMotorIsLost()
return 0; return 0;
} }
/* 切换底盘遥控器控制和云台双板控制 */ /* Chassis-board local RC control. Right switch down is hard emergency stop. */
static void ControlSwitch() static void ControlSwitch()
{ {
float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001; memset(&chassis_cmd_recv, 0, sizeof(chassis_cmd_recv));
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
chassis_cmd_recv.direction = CHASSIS_ALIGN;
chassis_cmd_recv.friction_mode = FRICTION_OFF;
chassis_cmd_recv.loader_mode = LOAD_STOP;
chassis_cmd_recv.vision_mode = UNLOCK;
chassis_cmd_recv.ui_mode = UI_KEEP;
// 根据裁判系统底盘输出电压设定底盘状态 if (!RemoteControlIsOnline())
if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
{ {
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
return; return;
} }
// // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 if (switch_is_down(rc_data[TEMP].rc.switch_right))
// if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) return;
// {
// if (switch_is_up(rc_data->rc.switch_left)) float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f;
// { if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN &&
// chassis_cmd_recv.chassis_mode = CHASSIS_RESET; chassis_vol < CHASSIS_UNDERVOLTAGE_LIMIT)
// chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s return;
// chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_;
// } if (DrivenMotorIsLost())
// else return;
// {
// chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 if (!switch_is_up(rc_data[TEMP].rc.switch_right))
// chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s return;
// chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
// chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_; chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
// } chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
// } chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
// else
// {
// chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
// }
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
// if (abs(l_side.theta) > (30.0f * DEGREE_2_RAD) || abs(r_side.theta) > (30.0f * DEGREE_2_RAD))
// {
// chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
// }
} }
/* 腿缩回复位,只允许驱动轮电机移动 */ /* Joint motors must remain disabled on this demo chassis. */
static void ResetChassis() static void ResetChassis()
{ {
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身 ClearMotionTargets();
ClearControlOutput();
// 目标速度置0 l_side.target_len = r_side.target_len = 0.12f;
chassis.target_v = 0; chassis.dist = chassis.target_dist = 0.0f;
// 复位时清空距离和腿长积累量,保证顺利站起
chassis.dist = chassis.target_dist = 0;
l_side.target_len = r_side.target_len = 0.12;
// 角度输入为当前角度
// chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_round) < 0.05 &&
abs(lb->measure.total_round) < 0.05 &&
abs(rf->measure.total_round) < 0.05 &&
abs(rb->measure.total_round) < 0.05)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_round) <= 0.03 &&
abs(lb->measure.total_round) <= 0.03 &&
abs(rf->measure.total_round) <= 0.03 &&
abs(rb->measure.total_round) <= 0.03)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
for (uint8_t i = 0; i < JOINT_CNT; i++)
DMMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
return; // 退出函数不再执行关节指令
}
else
chassis_status = ROBOT_STOP; chassis_status = ROBOT_STOP;
RefreshJointMotorDisable();
// 还在复位中,关节改为位置环,执行复位 StopDrivenMotor();
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
DMMotorOuterLoop(joint[i], ANGLE_LOOP);
DMMotorSetRef(joint[i], 0);
}
} }
// 工作状态设定 // 工作状态设定
static void WokingStateSet() static void WokingStateSet()
{ {
if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式 RefreshJointMotorDisable();
if (last_chassis_mode != chassis_cmd_recv.chassis_mode)
{
if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
ResetStoolRuntime();
else
ClearControlOutput();
last_chassis_mode = chassis_cmd_recv.chassis_mode;
}
if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{ {
ResetChassis(); ResetChassis();
return; return;
} }
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
{
// 目标速度置0
chassis.target_v = 0;
// 清空腿长和距离
l_side.target_len = r_side.target_len = 0.12;
chassis.dist = chassis.target_dist = 0;
// 角度输入为当前角度
// chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
for (uint8_t i = 0; i < JOINT_CNT; i++) if (chassis_cmd_recv.chassis_mode != CHASSIS_STOOL_MODE)
DMMotorStop(joint[i]); {
for (uint8_t i = 0; i < DRIVEN_CNT; i++) ResetChassis();
LKMotorStop(driven[i]); return;
return; // 关闭所有电机,发送的指令为零
} }
// 运动模式 EnableDrivenMotor();
EnableAllMotor(); chassis_status = ROBOT_READY;
// 保证关节电机为开环扭矩控制 l_side.target_len = r_side.target_len = 0.12f;
for (uint8_t i = 0; i < JOINT_CNT; i++) stool_target_wz = chassis_cmd_recv.offset_angle;
DMMotorOuterLoop(joint[i], OPEN_LOOP);
// 设置目标速度/腿长/距离 if (fabsf(chassis_cmd_recv.vx) > 0.001f)
l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
r_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
// 腿长限幅
VAL_LIMIT(l_side.target_len, 0.12, 0.30);
VAL_LIMIT(r_side.target_len, 0.12, 0.30);
// 加速度限幅,防止键盘控制摔倒
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
// VAL_LIMIT(chassis.target_v, 0.002, 2.0);
// 角度输入
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{ {
chassis.target_yaw = chassis_cmd_recv.offset_angle; ApproachFloat(&chassis.target_v, chassis_cmd_recv.vx, STOOL_ACC_REF * del_t);
stool_stop_dist = stool_dist;
stool_drive_key_pressed = 1;
} }
chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle * DEGREE_2_RAD; else
{
// TODO 转向速度限幅 if (stool_drive_key_pressed)
stool_stop_dist = stool_dist;
// TODO 最大dist误差限幅 ApproachFloat(&chassis.target_v, 0.0f, STOOL_BRAKE_ACC_REF * del_t);
stool_drive_key_pressed = 0;
// TODO 最大速度误差限幅 }
VAL_LIMIT(chassis.target_v, -STOOL_SPEED_REF, STOOL_SPEED_REF);
chassis.target_dist = chassis.dist;
} }
/** /**
@@ -410,8 +453,40 @@ static void ParamAssemble()
r_side.w_ecd = -r_driven->measure.speed_rads; r_side.w_ecd = -r_driven->measure.speed_rads;
} }
static void StoolModeBalanceControl(void)
{
float raw_vel = WHEEL_RADIUS * (l_side.w_ecd + r_side.w_ecd) * 0.5f;
float vel_lpf_alpha = del_t / (STOOL_VEL_LPF_RC + del_t);
VAL_LIMIT(vel_lpf_alpha, 0.0f, 1.0f);
stool_vel += vel_lpf_alpha * (raw_vel - stool_vel);
stool_dist += stool_vel * del_t;
if (fabsf(chassis.target_v) > 0.001f)
stool_stop_dist = stool_dist;
float stop_dist_error = stool_dist - stool_stop_dist;
float wheel_torque = -STOOL_PITCH_K * chassis.pitch -
STOOL_PITCH_W_K * chassis.pitch_w +
STOOL_VEL_K * (stool_vel - chassis.target_v) +
STOOL_STOP_DIST_K * stop_dist_error;
VAL_LIMIT(wheel_torque, -STOOL_WHEEL_TORQUE_LIMIT, STOOL_WHEEL_TORQUE_LIMIT);
l_side.T_wheel = wheel_torque;
r_side.T_wheel = wheel_torque;
}
static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{ {
if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
{
PIDCalculate(&steer_v_pid, chassis.wz, stool_target_wz);
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
return;
}
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG ||
chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随 chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
{ {
@@ -452,10 +527,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
static void WattLimitSet() /* 设定运动模态的输出 */ static void WattLimitSet() /* 设定运动模态的输出 */
{ {
DMMotorSetRef(lf, 1.0f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改 RefreshJointMotorDisable();
DMMotorSetRef(lb, 1.0f * -l_side.T_back);
DMMotorSetRef(rf, 1.0f * r_side.T_front);
DMMotorSetRef(rb, 1.0f * r_side.T_back);
LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel); LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel); LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
@@ -464,16 +536,6 @@ static void WattLimitSet() /* 设定运动模态的输出 */
// 裁判系统,双板通信,电容功率控制等 // 裁判系统,双板通信,电容功率控制等
static void CommNPower() static void CommNPower()
{ {
//static uint8_t supercap_send_cnt = 0;
// CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
// supercap_send_cnt++;
// if (supercap_send_cnt % 5 == 0)
// {
// DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送
// DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit;
// SuperCapSend(cap, (uint8_t *)&DataSend2Cap);
// supercap_send_cnt = 0;
// }
/* 更新ui数据 */ /* 更新ui数据 */
ui_data.direction = chassis_cmd_recv.direction; ui_data.direction = chassis_cmd_recv.direction;
ui_data.friction_mode = chassis_cmd_recv.friction_mode; ui_data.friction_mode = chassis_cmd_recv.friction_mode;
@@ -497,6 +559,21 @@ void BalanceTask()
CommNPower(); CommNPower();
// 参数组装 // 参数组装
ParamAssemble(); ParamAssemble();
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return;
if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
{
StoolModeBalanceControl();
SynthesizeMotion();
ClearJointOutput();
WattLimitSet();
return;
}
// 将五连杆映射成单杆 // 将五连杆映射成单杆
Link2Leg(&l_side, &chassis); Link2Leg(&l_side, &chassis);
Link2Leg(&r_side, &chassis); Link2Leg(&r_side, &chassis);
@@ -516,12 +593,6 @@ void BalanceTask()
NormalForceSolve(&l_side, Chassis_IMU_data); NormalForceSolve(&l_side, Chassis_IMU_data);
NormalForceSolve(&r_side, Chassis_IMU_data); NormalForceSolve(&r_side, Chassis_IMU_data);
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
// 运动模态,电机输出映射和限幅 // 运动模态,电机输出映射和限幅
WattLimitSet(); WattLimitSet();
} }

26
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,20 @@
*/ */
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis) static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{ {
static float k[12][3] = {91.443258,-113.149301,-8.780431, static float k[12][3] = {
2.186397,-6.908826,-0.171192, {91.443258f, -113.149301f, -8.780431f},
47.943203,-39.423913,-13.297140, {2.186397f, -6.908826f, -0.171192f},
30.671413,-28.308799,-9.363969, {47.943203f, -39.423913f, -13.297140f},
231.778706,-224.742110,68.974495, {30.671413f, -28.308799f, -9.363969f},
18.910995,-20.079072,7.490403, {231.778706f, -224.742110f, 68.974495f},
50.114072,-58.670210,23.856863, {18.910995f, -20.079072f, 7.490403f},
3.273746,-3.321150,1.267603, {50.114072f, -58.670210f, 23.856863f},
140.969395,-137.481822,42.507159, {3.273746f, -3.321150f, 1.267603f},
93.859327,-90.754295,27.811839, {140.969395f, -137.481822f, 42.507159f},
-283.581279,232.017305,87.865897, {93.859327f, -90.754295f, 27.811839f},
-28.193619,23.802666,3.963794,}; {-283.581279f, 232.017305f, 87.865897f},
{-28.193619f, 23.802666f, 3.963794f},
};
float T[2] = {0}; // 0 T_wheel 1 T_hip float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len; float l = p->leg_len;
float lsqr = l * l; float lsqr = l * l;

4
application/cmd/robot_cmd.c
View File

@@ -16,6 +16,8 @@
#include "bsp_log.h" #include "bsp_log.h"
#include "string.h" #include "string.h"
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
@@ -415,3 +417,5 @@ void RobotCMDTask()
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
} }
#endif

5
application/robot_def.h
View File

@@ -17,8 +17,8 @@
#include "stdint.h" #include "stdint.h"
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
#define ONE_BOARD // 单板控制整车 // #define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板 #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板 // #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据
@@ -92,6 +92,7 @@ typedef enum
CHASSIS_RESET, // 底盘重置,双腿缩回 CHASSIS_RESET, // 底盘重置,双腿缩回
CHASSIS_FREE_DEBUG, // 底盘单独调试模式 CHASSIS_FREE_DEBUG, // 底盘单独调试模式
CHASSIS_ROTATE_REVERSE, CHASSIS_ROTATE_REVERSE,
CHASSIS_STOOL_MODE, // 小板凳模式
} chassis_mode_e; } chassis_mode_e;

4
modules/algorithm/QuaternionEKF.c
View File

@@ -199,8 +199,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
// 利用四元数反解欧拉角 // 利用四元数反解欧拉角
QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f; QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺) // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f) if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)

8
modules/motor/DMmotor/dmmotor.c
View File

@@ -65,8 +65,7 @@ static void DMMotorDecode(CANInstance *motor_can)
static void DMMotorLostCallback(void *motor_ptr) static void DMMotorLostCallback(void *motor_ptr)
{ {
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
DMMotorEnable(motor); DMMotorStop(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
} }
void DMMotorCaliEncoder(DMMotorInstance *motor) void DMMotorCaliEncoder(DMMotorInstance *motor)
@@ -131,17 +130,16 @@ void DMMotorTask(void const *argument)
Motor_Control_Setting_s *setting; Motor_Control_Setting_s *setting;
DMMotor_Send_s motor_send_mailbox; DMMotor_Send_s motor_send_mailbox;
portTickType currentTime; portTickType currentTime;
const portTickType xFrequency = pdMS_TO_TICKS(4);
while (1) while (1)
{ {
currentTime = xTaskGetTickCount(); currentTime = xTaskGetTickCount();
set = motor->pid_ref; set = motor->pid_ref;
measure = &motor->measure;
setting = &motor->motor_settings;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1; set *= -1;
measure = &motor->measure;
setting = &motor->motor_settings;
if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP)) if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP))
{ {
if (setting->angle_feedback_source == OTHER_FEED) if (setting->angle_feedback_source == OTHER_FEED)

14
modules/referee/referee_task.c
View File

@@ -38,7 +38,7 @@ static void DeterminRobotID()
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data); static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测 static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化 static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) __attribute__((unused)); // 测试用函数，实现模式自动变化
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data) referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
{ {
@@ -225,6 +225,15 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
case CHASSIS_FOLLOW_GIMBAL_YAW: case CHASSIS_FOLLOW_GIMBAL_YAW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
break; break;
case CHASSIS_STOOL_MODE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "stool ");
break;
case CHASSIS_FREE_DEBUG:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "debug ");
break;
case CHASSIS_ROTATE_REVERSE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "revrot ");
break;
} }
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 0; _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
@@ -263,6 +272,9 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
{ {
switch (_Interactive_data->loader_mode) switch (_Interactive_data->loader_mode)
{ {
case LOAD_REVERSE:
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Purplish_red, 15, 2, 270, 600, "rev ");
break;
case LOAD_1_BULLET: case LOAD_1_BULLET:
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Cyan, 15, 2, 270, 600, "1 bull"); UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Cyan, 15, 2, 270, 600, "1 bull");
break; break;