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

419
application/chassis/balance.c
View File

@@ -21,6 +21,22 @@
#include "speed_estimation.h"
#include "fly_detection.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 float del_t;
@@ -29,8 +45,6 @@ static float del_t;
static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data; // 底盘单独调试用
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 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_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
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()
{
@@ -68,18 +184,6 @@ void BalanceInit()
.rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
}};
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 = {
// 写一个,剩下的修改方向和id即可
@@ -200,28 +304,14 @@ void BalanceInit()
// 状态初始化
l_side.target_len = r_side.target_len = 0.12;
chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY;
DWT_GetDeltaT(&balance_dwt_cnt);
}
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()
{
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
chassis_status = ROBOT_STOP;
ResetStoolRuntime();
DisableAllJointMotor();
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
if (joint[i]->motor_daemon->temp_count == 0)
return 1;
}
return 0;
DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]);
StopDrivenMotor();
DWT_GetDeltaT(&balance_dwt_cnt);
}
// 检查驱动轮电机是否离线
@@ -236,147 +326,100 @@ static uint8_t DrivenMotorIsLost()
return 0;
}
/* 切换底盘遥控器控制和云台双板控制 */
/* Chassis-board local RC control. Right switch down is hard emergency stop. */
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 (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
if (!RemoteControlIsOnline())
{
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
return;
}
// // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
// if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
// {
// if (switch_is_up(rc_data->rc.switch_left))
// {
// chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
// chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
// chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_;
// }
// else
// {
// chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
// chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
// 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_;
// }
// }
// 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;
// }
if (switch_is_down(rc_data[TEMP].rc.switch_right))
return;
float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f;
if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN &&
chassis_vol < CHASSIS_UNDERVOLTAGE_LIMIT)
return;
if (DrivenMotorIsLost())
return;
if (!switch_is_up(rc_data[TEMP].rc.switch_right))
return;
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;
}
/* 腿缩回复位,只允许驱动轮电机移动 */
/* Joint motors must remain disabled on this demo chassis. */
static void ResetChassis()
{
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
// 目标速度置0
chassis.target_v = 0;
// 复位时清空距离和腿长积累量,保证顺利站起
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;
// 还在复位中,关节改为位置环,执行复位
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
DMMotorOuterLoop(joint[i], ANGLE_LOOP);
DMMotorSetRef(joint[i], 0);
}
ClearMotionTargets();
ClearControlOutput();
l_side.target_len = r_side.target_len = 0.12f;
chassis.dist = chassis.target_dist = 0.0f;
chassis_status = ROBOT_STOP;
RefreshJointMotorDisable();
StopDrivenMotor();
}
// 工作状态设定
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();
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++)
DMMotorStop(joint[i]);
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
LKMotorStop(driven[i]);
return; // 关闭所有电机,发送的指令为零
if (chassis_cmd_recv.chassis_mode != CHASSIS_STOOL_MODE)
{
ResetChassis();
return;
}
// 运动模式
EnableAllMotor();
// 保证关节电机为开环扭矩控制
for (uint8_t i = 0; i < JOINT_CNT; i++)
DMMotorOuterLoop(joint[i], OPEN_LOOP);
EnableDrivenMotor();
chassis_status = ROBOT_READY;
l_side.target_len = r_side.target_len = 0.12f;
stool_target_wz = chassis_cmd_recv.offset_angle;
// 设置目标速度/腿长/距离
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)
if (fabsf(chassis_cmd_recv.vx) > 0.001f)
{
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;
// TODO 转向速度限幅
// TODO 最大dist误差限幅
// TODO 最大速度误差限幅
else
{
if (stool_drive_key_pressed)
stool_stop_dist = stool_dist;
ApproachFloat(&chassis.target_v, 0.0f, STOOL_BRAKE_ACC_REF * del_t);
stool_drive_key_pressed = 0;
}
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;
}
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() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{
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 ||
chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
{
@@ -452,10 +527,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
static void WattLimitSet() /* 设定运动模态的输出 */
{
DMMotorSetRef(lf, 1.0f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改
DMMotorSetRef(lb, 1.0f * -l_side.T_back);
DMMotorSetRef(rf, 1.0f * r_side.T_front);
DMMotorSetRef(rb, 1.0f * r_side.T_back);
RefreshJointMotorDisable();
LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
@@ -464,16 +536,6 @@ static void WattLimitSet() /* 设定运动模态的输出 */
// 裁判系统,双板通信,电容功率控制等
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_data.direction = chassis_cmd_recv.direction;
ui_data.friction_mode = chassis_cmd_recv.friction_mode;
@@ -497,6 +559,21 @@ void BalanceTask()
CommNPower();
// 参数组装
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(&r_side, &chassis);
@@ -516,12 +593,6 @@ void BalanceTask()
NormalForceSolve(&l_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();
}
}

28
application/chassis/lqr_calc.h
View File

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

4
application/cmd/robot_cmd.c
View File

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

7
application/robot_def.h
View File

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

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.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.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 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 = 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,处理一下方便其他功能使用(如小陀螺)
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)
{
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
DMMotorEnable(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DMMotorStop(motor);
}
void DMMotorCaliEncoder(DMMotorInstance *motor)
@@ -131,17 +130,16 @@ void DMMotorTask(void const *argument)
Motor_Control_Setting_s *setting;
DMMotor_Send_s motor_send_mailbox;
portTickType currentTime;
const portTickType xFrequency = pdMS_TO_TICKS(4);
while (1)
{
currentTime = xTaskGetTickCount();
set = motor->pid_ref;
measure = &motor->measure;
setting = &motor->motor_settings;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
measure = &motor->measure;
setting = &motor->motor_settings;
if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP))
{
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 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)
{
@@ -225,6 +225,15 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
case CHASSIS_FOLLOW_GIMBAL_YAW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
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]);
_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)
{
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:
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Cyan, 15, 2, 270, 600, "1 bull");
break;