TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

!1 小板凳 存档

Browse Source 
Merge pull request !1 from TuxMonkey/dev-stool
This commit is contained in:
TuxMonkey
2026-07-15 12:17:45 +00:00
committed by Gitee
parent 7525ecdffc 14a3ab4cd0
commit 52813f45d2
11 changed files with 502 additions and 325 deletions
Download Patch File Download Diff File Expand all files Collapse all files

433
application/chassis/balance.c
View File

@@ -21,6 +21,25 @@
#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_SPIN_YAW_RATE_REF 10.0f
#define STOOL_SPIN_YAW_ACC_REF 5.0f
#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_PITCH_REF -0.10f
#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 +48,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 +68,114 @@ 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 uint8_t stool_spin_enabled;
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;
stool_spin_enabled = 0;
}
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 +189,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即可
@@ -132,9 +241,9 @@ void BalanceInit()
};
driven_conf.can_init_config.tx_id = 1;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[LD] = l_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
@@ -200,28 +309,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 +331,110 @@ 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;
stool_spin_enabled = 0;
// 根据裁判系统底盘输出电压设定底盘状态
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;
// }
}
uint8_t switch_right = rc_data[TEMP].rc.switch_right;
if (!switch_is_up(switch_right) && !switch_is_mid(switch_right))
return;
/* 腿缩回复位,只允许驱动轮电机移动 */
static void ResetChassis()
{
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f;
if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN &&
chassis_vol < CHASSIS_UNDERVOLTAGE_LIMIT)
return;
// 目标速度置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;
if (DrivenMotorIsLost())
return;
// 撞墙时前后移动保证能重新站立,执行速度输入
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_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
if (switch_is_up(switch_right))
{
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; // 退出函数不再执行关节指令
chassis_cmd_recv.offset_angle = -STOOL_SPIN_YAW_RATE_REF;
stool_spin_enabled = 1;
}
else
chassis_status = ROBOT_STOP;
// 还在复位中,关节改为位置环,执行复位
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
DMMotorOuterLoop(joint[i], ANGLE_LOOP);
DMMotorSetRef(joint[i], 0);
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()
{
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;
if (stool_spin_enabled || fabsf(stool_target_wz) > STOOL_YAW_RATE_REF)
ApproachFloat(&stool_target_wz, chassis_cmd_recv.offset_angle, STOOL_SPIN_YAW_ACC_REF * del_t);
else
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;
}
/**
@@ -390,7 +448,7 @@ static void ParamAssemble()
{
// 机体参数,视为平面刚体
chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
chassis.pitch_w = Chassis_IMU_data->Gyro[0];
chassis.pitch_w = Chassis_IMU_data->Gyro[1];
chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
chassis.wz = Chassis_IMU_data->Gyro[2];
chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
@@ -410,8 +468,41 @@ 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 pitch_error = chassis.pitch - STOOL_PITCH_REF;
float wheel_torque = -STOOL_PITCH_K * pitch_error -
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 +543,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 +552,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 +575,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 +609,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];
}
}

58
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绘制相关
@@ -153,27 +155,27 @@ static void RemoteControlSet()
}
// 右侧开关状态为[下],底盘旋转
if (switch_is_down(rc_data[TEMP].rc.switch_right))
{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
chassis_cmd_send.rotate_w = 2;
}
if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
chassis_cmd_send.rotate_w = -2;
}
chassis_cmd_send.vx = 0;
}
else
{
// if (switch_is_down(rc_data[TEMP].rc.switch_right))
// {
// chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
//
// if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
// {
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
// chassis_cmd_send.rotate_w = 2;
// }
// if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
// {
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
// chassis_cmd_send.rotate_w = -2;
// }
// chassis_cmd_send.vx = 0;
// }
// else
// {
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
chassis_cmd_send.vx = 0.003f * (-(float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1);
}
// }
// 左侧开关状态为[中]，腿长控制
if (switch_is_mid(rc_data[TEMP].rc.switch_left))
@@ -323,22 +325,26 @@ static void MouseKeySet()
*/
static void EmergencyHandler()
{
if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
// 获取遥控器在线状态 (1:在线 0:离线)
uint8_t rc_online = RemoteControlIsOnline();
if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下]或[中]
{
if ((rc_data[TEMP].rc.dial > 300 && switch_is_down(rc_data[TEMP].rc.switch_left)) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
// 急停触发条件：遥控器离线或者右侧开关打到[下]
if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_STOP;
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; // 底盘电流归零，无力化
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步
gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
}
// 遥控器右侧开关为[上],恢复正常运行
if (switch_is_up(rc_data[TEMP].rc.switch_right))
// 恢复正常运行条件：遥控器在线且右侧开关状态为[上]或[中]
else if (switch_is_up(rc_data[TEMP].rc.switch_right) || switch_is_mid(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
@@ -411,3 +417,5 @@ void RobotCMDTask()
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
}
#endif

3
application/robot_def.h
View File

@@ -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

35
bsp/can/bsp_can.c
View File

@@ -11,6 +11,11 @@
static CANInstance *can_instance[CAN_MX_REGISTER_CNT] = {NULL};
static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自增
static uint8_t CANBusIndex(CAN_HandleTypeDef *hcan)
{
return hcan == &hcan1 ? 0u : 1u;
}
/* ----------------two static function called by CANRegister()-------------------- */
/**
@@ -28,18 +33,26 @@ static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自
*/
static void CANAddFilter(CANInstance *_instance)
{
CAN_FilterTypeDef can_filter_conf;
static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用
static uint8_t all_pass_filter_configured[DEVICE_CAN_CNT] = {0};
uint8_t bus = CANBusIndex(_instance->can_handle);
can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; // 使用16位id模式,即只有低16位有效
can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
can_filter_conf.SlaveStartFilterBank = 14; // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
can_filter_conf.FilterIdLow = _instance->rx_id << 5; // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增
can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; // 启用过滤器
if (all_pass_filter_configured[bus])
return;
HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf);
CAN_FilterTypeDef accept_all_filter = {0};
accept_all_filter.FilterActivation = CAN_FILTER_ENABLE;
accept_all_filter.FilterMode = CAN_FILTERMODE_IDMASK;
accept_all_filter.FilterScale = CAN_FILTERSCALE_32BIT;
accept_all_filter.FilterIdHigh = 0x0000;
accept_all_filter.FilterIdLow = 0x0000;
accept_all_filter.FilterMaskIdHigh = 0x0000;
accept_all_filter.FilterMaskIdLow = 0x0000;
accept_all_filter.FilterBank = _instance->can_handle == &hcan1 ? 0 : 14;
accept_all_filter.FilterFIFOAssignment = CAN_RX_FIFO0;
accept_all_filter.SlaveStartFilterBank = 14;
HAL_CAN_ConfigFilter(_instance->can_handle, &accept_all_filter);
all_pass_filter_configured[bus] = 1;
return;
}
/**
@@ -163,7 +176,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例
can_instance[i]->can_module_callback(can_instance[i]); // 触发回调进行数据解析和处理
}
return;
break;
}
}
}

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)

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

@@ -1,5 +1,4 @@
#include "dmmotor.h"
#include "memory.h"
#include "general_def.h"
#include "user_lib.h"
#include "cmsis_os.h"
@@ -8,16 +7,19 @@
#include "stdlib.h"
#include "bsp_log.h"
#define DM_SEND_DELAY 0.2
static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
static osThreadId dm_task_handle[DM_MOTOR_CNT];
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
float span = x_max - x_min;
float offset = x_min;
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
@@ -25,53 +27,63 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
{
memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
memset(motor->motor_can_instace->tx_buff, 0xff, 7);
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
CANTransmit(motor->motor_can_instace, 1);
DWT_Delay(DM_SEND_DELAY);
}
static void DMMotorDecode(CANInstance *motor_can)
{
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint16_t tmp;
uint8_t *rxbuff = motor_can->rx_buff;
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
DM_Motor_Measure_s *measure = &(motor->measure);
DaemonReload(motor->motor_daemon);
measure->id = rxbuff[0];
measure->state = (rxbuff[0] >> 4) & 0xf;
measure->last_position = measure->position;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
measure->angle_single_round = measure->position / (2 * PI) * 360.0f;
if (measure->position - measure->last_position > PI)
measure->total_round--;
else if (measure->position - measure->last_position < -PI)
measure->total_round++;
measure->total_angle = measure->total_round * 360.0f + measure->position / (2 * PI) * 360.0f;
tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
measure->T_Mos = (float)rxbuff[6];
measure->T_Rotor = (float)rxbuff[7];
}
static void DMMotorLostCallback(void *motor_ptr)
{
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
DMMotorStop(motor);
}
void DMMotorCaliEncoder(DMMotorInstance *motor)
{
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
DWT_Delay(0.1);
}
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
{
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
memset(motor, 0, sizeof(DMMotorInstance));
motor->motor_type = config->motor_type;
motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
PIDInit(&motor->torque_PID, &config->controller_param_init_config.torque_PID);
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
@@ -84,13 +96,8 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
.owner_id = motor,
.reload_count = 10,
};
motor->motor_daemon = DaemonRegister(&conf);
DMMotorEnable(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DWT_Delay(0.1);
DMMotorCaliEncoder(motor);
DWT_Delay(0.1);
motor->motor_daemon = DaemonRegister(&conf);
dm_motor_instance[idx++] = motor;
return motor;
}
@@ -105,7 +112,7 @@ void DMMotorEnable(DMMotorInstance *motor)
motor->stop_flag = MOTOR_ENALBED;
}
void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈
void DMMotorStop(DMMotorInstance *motor)
{
motor->stop_flag = MOTOR_STOP;
}
@@ -115,33 +122,59 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
motor->motor_settings.outer_loop_type = type;
}
//@Todo: 目前只实现了力控，更多位控PID等请自行添加
void DMMotorTask(void const *argument)
{
float pid_ref, set;
float pid_ref, set;
DMMotorInstance *motor = (DMMotorInstance *)argument;
//DM_Motor_Measure_s *measure = &motor->measure;
Motor_Control_Setting_s *setting = &motor->motor_settings;
//CANInstance *motor_can = motor->motor_can_instace;
//uint16_t tmp;
DM_Motor_Measure_s *measure;
Motor_Control_Setting_s *setting;
DMMotor_Send_s motor_send_mailbox;
portTickType currentTime;
while (1)
{
pid_ref = motor->pid_ref;
set = pid_ref;
currentTime = xTaskGetTickCount();
set = motor->pid_ref;
measure = &motor->measure;
setting = &motor->motor_settings;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP))
{
if (setting->angle_feedback_source == OTHER_FEED)
{
set = PIDCalculate(&motor->angle_PID, *motor->other_angle_feedback_ptr, set);
}
else if (setting->angle_feedback_source == MOTOR_FEED)
{
set = PIDCalculate(&motor->angle_PID, measure->position, set);;
}
}
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (SPEED_LOOP | ANGLE_LOOP)))
{
if (setting->speed_feedback_source == OTHER_FEED)
{
set = PIDCalculate(&motor->speed_PID, *motor->other_speed_feedback_ptr, set);
}
else if (setting->speed_feedback_source == MOTOR_FEED)
{
set = PIDCalculate(&motor->speed_PID, measure->velocity, set);
}
}
if ((setting->close_loop_type & TORQUE_LOOP) && (setting->outer_loop_type & (TORQUE_LOOP | SPEED_LOOP | ANGLE_LOOP)))
{
set = PIDCalculate(&motor->torque_PID, 0, set);
}
pid_ref = set;
LIMIT_MIN_MAX(pid_ref, DM_T_MIN, DM_T_MAX);
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
motor_send_mailbox.torque_des = float_to_uint(pid_ref, DM_T_MIN, DM_T_MAX, 12);
motor_send_mailbox.Kp = 0;
motor_send_mailbox.Kd = 0;
if(motor->stop_flag == MOTOR_STOP)
if (motor->stop_flag == MOTOR_STOP)
motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
@@ -153,23 +186,38 @@ void DMMotorTask(void const *argument)
motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
CANTransmit(motor->motor_can_instace, 1);
CANTransmit(motor->motor_can_instace, 1);
osDelay(2);
osDelayUntil(&currentTime, 4); // 4ms 的绝对延时
}
}
void DMMotorControlInit()
// /**
// * @brief Keil的runtime中的stdlib.h没有提供__itoa()，此处为该函数的手动实现
// * @param n 源数据
// * @param s 目标字符串
// * @param b 目标进制 有效范围2-36
// * @attention 没加特判，注意整数溢出/字符串数组越界/进制范围限制
// **/
// void __itoa(int n,char *s,int b)
// {
// char *p = s; int sg = n < 0; n = sg ? -n : n;
// do { *p++ = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[n % b];} while(n /= b);
// sg ? (*p++ = '-') : 0;
// *p = '\0';
// for(char *h = s,*t = p-1; h < t; h++, t--)(*h ^= *t,*t ^= *h,*h ^= *t);
// }
void DMMotorControlInit(void)
{
char dm_task_name[5] = "dm";
// 遍历所有电机实例,创建任务
if (!idx)
return;
for (size_t i = 0; i < idx; i++)
{
char dm_id_buff[2] = {0};
char dm_task_name[5] = "dm", dm_id_buff[2] = {0};
__itoa(i, dm_id_buff, 10);
strcat(dm_task_name, dm_id_buff);
osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
}
}
}

37
modules/motor/DMmotor/dmmotor.h
View File

@@ -6,7 +6,7 @@
#include "motor_def.h"
#include "daemon.h"
#define DM_MOTOR_CNT 4
#define DM_MOTOR_CNT 5
#define DM_P_MIN (-12.5f)
#define DM_P_MAX 12.5f
@@ -15,18 +15,22 @@
#define DM_T_MIN (-18.0f)
#define DM_T_MAX 18.0f
typedef struct
#define DM_REDUCE 9
typedef struct
{
uint8_t id;
uint8_t state;
float velocity;
float last_position;
float position;
float angle_single_round;
float total_angle;
float torque;
float T_Mos;
float T_Rotor;
int32_t total_round;
}DM_Motor_Measure_s;
} DM_Motor_Measure_s;
typedef struct
{
@@ -35,12 +39,14 @@ typedef struct
uint16_t torque_des;
uint16_t Kp;
uint16_t Kd;
}DMMotor_Send_s;
typedef struct
} DMMotor_Send_s;
typedef struct
{
Motor_Type_e motor_type;
DM_Motor_Measure_s measure;
Motor_Control_Setting_s motor_settings;
PIDInstance current_PID;
PIDInstance torque_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float *other_angle_feedback_ptr;
@@ -50,27 +56,26 @@ typedef struct
float pid_ref;
Motor_Working_Type_e stop_flag;
CANInstance *motor_can_instace;
DaemonInstance* motor_daemon;
DaemonInstance *motor_daemon;
uint32_t lost_cnt;
}DMMotorInstance;
} DMMotorInstance;
typedef enum
{
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
DM_CMD_RESET_MODE = 0xfd, // 停止
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
DM_CMD_RESET_MODE = 0xfd, // 停止
DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
}DMMotor_Mode_e;
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
} DMMotor_Mode_e;
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e closeloop_type);
void DMMotorEnable(DMMotorInstance *motor);
void DMMotorStop(DMMotorInstance *motor);
void DMMotorCaliEncoder(DMMotorInstance *motor);
void DMMotorControlInit();
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
void DMMotorControlInit(void);
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
#endif // !DMMOTOR

85
modules/motor/LKmotor/LK9025.c
View File

@@ -1,5 +1,6 @@
#include "LK9025.h"
#include "stdlib.h"
#include "string.h"
#include "general_def.h"
#include "daemon.h"
#include "bsp_dwt.h"
@@ -7,33 +8,30 @@
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
// 后续考虑兼容单电机和多电机指令.
/**
* @brief 电机反馈报文解析
*
* @param _instance 发生中断的caninstance
*/
static uint32_t LKMotorSingleCanID(uint32_t id)
{
return id < 0x140 ? 0x140 + id : id;
}
static void LKMotorDecode(CANInstance *_instance)
{
LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
LKMotorInstance *motor = (LKMotorInstance *)_instance->id;
LKMotor_Measure_t *measure = &motor->measure;
uint8_t *rx_buff = _instance->rx_buff;
DaemonReload(motor->daemon); // 喂狗
DaemonReload(motor->daemon);
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads +
DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
measure->real_current = (int16_t)((1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])));
measure->temperature = rx_buff[1];
@@ -41,7 +39,7 @@ static void LKMotorDecode(CANInstance *_instance)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -32768)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
measure->total_angle = measure->total_round * 360.0f + measure->angle_single_round;
}
static void LKMotorLostCallback(void *motor_ptr)
@@ -53,7 +51,6 @@ static void LKMotorLostCallback(void *motor_ptr)
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
{
LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
memset(motor, 0, sizeof(LKMotorInstance));
motor->motor_settings = config->controller_setting_init_config;
@@ -65,16 +62,10 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
config->can_init_config.id = motor;
config->can_init_config.can_module_callback = LKMotorDecode;
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
config->can_init_config.tx_id = LKMotorSingleCanID(config->can_init_config.tx_id);
config->can_init_config.rx_id = config->can_init_config.tx_id;
motor->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0) // 用第一个电机的can instance发送数据
{
sender_instance = motor->motor_can_ins;
sender_instance->tx_id = 0x280; // 修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
}
LKMotorEnable(motor);
DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
lkmotor_instance[idx++] = motor;
@@ -82,14 +73,29 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
Daemon_Init_Config_s daemon_config = {
.callback = LKMotorLostCallback,
.owner_id = motor,
.reload_count = 50, // 50ms
.reload_count = 50,
};
motor->daemon = DaemonRegister(&daemon_config);
return motor;
}
/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
static void LKMotorSendTorque(LKMotorInstance *motor, int16_t set)
{
uint16_t raw_set = (uint16_t)set;
uint8_t *tx_buff = motor->motor_can_ins->tx_buff;
tx_buff[0] = 0xA1;
tx_buff[1] = 0x00;
tx_buff[2] = 0x00;
tx_buff[3] = 0x00;
tx_buff[4] = (uint8_t)(raw_set & 0xff);
tx_buff[5] = (uint8_t)(raw_set >> 8);
tx_buff[6] = 0x00;
tx_buff[7] = 0x00;
CANTransmit(motor->motor_can_ins, 0.2);
}
void LKMotorControl()
{
float pid_measure, pid_ref;
@@ -105,48 +111,41 @@ void LKMotorControl()
setting = &motor->motor_settings;
pid_ref = motor->pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref *= -1;
pid_ref *= -1.0f;
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_measure = measure->total_angle;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (setting->angle_feedback_source == OTHER_FEED)
if (setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
}
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = (int16_t)pid_ref;
if (pid_ref > I_MAX)
pid_ref = I_MAX;
else if (pid_ref < I_MIN)
pid_ref = I_MIN;
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
}
set = motor->stop_flag == MOTOR_STOP ? 0 : (int16_t)pid_ref;
LKMotorSendTorque(motor, set);
}
if (idx) // 如果有电机注册了
CANTransmit(sender_instance, 0.2);
}
void LKMotorStop(LKMotorInstance *motor)

2
modules/motor/motor_def.h
View File

@@ -27,6 +27,7 @@ typedef enum
CURRENT_LOOP = 0b0001,
SPEED_LOOP = 0b0010,
ANGLE_LOOP = 0b0100,
TORQUE_LOOP = 0b1000,
// only for checking
SPEED_AND_CURRENT_LOOP = 0b0011,
@@ -125,6 +126,7 @@ typedef struct
PID_Init_Config_s current_PID;
PID_Init_Config_s speed_PID;
PID_Init_Config_s angle_PID;
PID_Init_Config_s torque_PID;
} Motor_Controller_Init_s;
/* 用于初始化CAN电机的结构体,各类电机通用 */

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;