TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-24 01:52:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Compare commits

base: TuxMonkey:devSJTUheadPWRC
Branches Tags
TuxMonkey:master TuxMonkey:codex/chassis-power-control TuxMonkey:devSJTUhead TuxMonkey:devSJTUheadPWRC TuxMonkey:dev TuxMonkey:dev-visionandreferee
TuxMonkey:v1.0
..
compare: TuxMonkey:dev-visionandreferee
Branches Tags
TuxMonkey:codex/chassis-power-control TuxMonkey:master TuxMonkey:devSJTUhead TuxMonkey:devSJTUheadPWRC TuxMonkey:dev TuxMonkey:dev-visionandreferee
TuxMonkey:v1.0

2 Commits
devSJTUhea ... dev-vision

Author SHA1 Message Date
TuxMonkey
a6d9bfc2b0 vision init 2026-03-19 01:34:58 +08:00
guoxy06
3ebb57d86b 修改RectifyRCjoystick()改了左摇杆限幅 2026-03-15 16:41:00 +08:00
16 changed files with 299 additions and 566 deletions
Expand all files Collapse all files

2
Src/usart.c
View File

@@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void)
/* USER CODE END USART1_Init 1 */
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.BaudRate = 921600;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;

232
application/chassis/chassis.c
View File

@@ -1,4 +1,3 @@
/**
* @file chassis.c
* @author NeoZeng neozng1@hnu.edu.cn
@@ -26,17 +25,6 @@
#include "referee_UI.h"
#include "arm_math.h"
// =================【功率模型参数】=================
// ⚠️警告：以下参数原本是群星战队为 DJI M3508 电机拟合的。
// 你们的驱动电机是 LK9015，它的扭矩常数、内阻、空载损耗和 M3508 不同！
// 建议后期针对 LK9015 重新拟合这四个参数以达到完美的控制效果。
#define TOQUE_COEFF 1.99688994e-6f
#define K1 1.23e-07f
#define K2 1.453e-07f
#define CONSTANT_POWER 4.081f
#define WARRING_ENERGY_BUFFER 50.0f
// ==================================================
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
@@ -45,7 +33,6 @@
#define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量
#elif defined(CHASSIS_STEERING_WHEEL)
#define CHASSIS_WHEEL_OFFSET 30.0f // 机器人底盘轮子修正偏移量
//#define CHASSIS_WHEEL_OFFSET 2.0f
#define SQRT2 1.41421356f // 根号2
#elif defined(CHASSIS_OMNI_WHEEL)
#define CHASSIS_WHEEL_OFFSET 0.7071f // 机器人底盘轮子修正偏移量，根号2/2，即45度，用于修正全向轮的安装位置
@@ -66,18 +53,18 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制
static Chassis_Upload_Data_s chassis_feedback_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 SuperCapInstance *cap; // 超级电容
static LKMotorInstance *motor_rf,*motor_lb; // 轮电机9015
static DJIMotorInstance *motor_steering_rf,*motor_steering_lb; // 舵电机6020
static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015
static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020
/* 用于自旋变速策略的时间变量 */
// static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_rf, vt_lb; // 底盘速度解算后的临时输出,待进行限幅
static float at_rf, at_lb; // 底盘的角度解算后的临时输出,待进行限幅
static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅
static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅
static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID
void ChassisInit()
@@ -95,7 +82,7 @@ void ChassisInit()
.MaxOut = 12000,
},
.current_PID = {
.Kp = 0.4, // 0.4
.Kp = 1, // 0.4
.Ki = 0, // 0
.Kd = 0,
.IntegralLimit = 10500,
@@ -115,25 +102,25 @@ void ChassisInit()
chassis_motor_config.can_init_config.tx_id = 1;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lb= LKMotorInit(&chassis_motor_config);
motor_rf = LKMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 2;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rf = LKMotorInit(&chassis_motor_config);
motor_lb = LKMotorInit(&chassis_motor_config);
// 舵电机6020初始化
Motor_Init_Config_s chassis_motor_steering_config = {
.can_init_config.can_handle = &hcan1,
.controller_param_init_config = {
.angle_PID = {
.Kp = 12,
.Ki = 0.2,
.Kp = 35,
.Ki = 0.25,
.Kd = 0,
.CoefA = 3,
.CoefB = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
.IntegralLimit = 1000,
.MaxOut = 5000,
.IntegralLimit = 1500,
.MaxOut = 6000,
.Derivative_LPF_RC = 0.001,
.DeadBand = 0.5,
},
@@ -157,23 +144,23 @@ void ChassisInit()
.motor_type = GM6020,
};
chassis_motor_steering_config.can_init_config.tx_id = 2;
chassis_motor_steering_config.can_init_config.tx_id = 1;
motor_steering_rf = DJIMotorInit(&chassis_motor_steering_config);
chassis_motor_steering_config.can_init_config.tx_id = 1;
chassis_motor_steering_config.can_init_config.tx_id = 2;
motor_steering_lb = DJIMotorInit(&chassis_motor_steering_config);
PID_Init_Config_s chassis_follow_pid_conf = {
.Kp = 1.2, // 6
.Kp = 0.95, // 6
.Ki = 0,
.Kd = 0.1f, // 0.5
.DeadBand = 0.5,
.CoefA = 0.2,
.CoefB = 0.3,
.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 50, // 200
.MaxOut = 1200,
.Derivative_LPF_RC = 0.01, // 0.01
.Kd = 0.1, // 0.495
.DeadBand = 0.5f,// 启用死区，值为0.5度
//.CoefA = 0.2,
//.CoefB = 0.3,
//.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
//.IntegralLimit = 50, // 200
.MaxOut = 45,
//.Derivative_LPF_RC = 0.01, // 0.01
};
PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);
@@ -183,14 +170,14 @@ void ChassisInit()
SuperCap_Init_Config_s cap_conf = {
.can_config = {
.can_handle = &hcan2,
.tx_id = 0x001, // 超级电容默认接收id
.rx_id = 0x100, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
.can_handle = &hcan1, // 保持你原有的硬件CAN接口不变
.tx_id = 0x001, // C板发给超级电容的ID
.rx_id = 0x100, // 超级电容发给C板的ID
}};
cap = SuperCapInit(&cap_conf); // 超级电容初始化
cap = SuperCapInit(&cap_conf); // 超级电容初始化 todo 控制
// （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整
// SuperCapSetPowerOffset(cap, 5);
// （新增）设置超级电容功率补偿，例如补偿5W，可根据实际需求调整 todo 控制
SuperCapSetPowerOffset(cap, 5);
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
@@ -248,7 +235,7 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
while (norm_rotation < -180.0f) {
norm_rotation += 360.0f;
}
float threshold = 90.0f;
float threshold = 110.0f;
float target_single=fmodf(target_angle,360.0f);
if(target_single<0.0f) target_single+=360.0f;
@@ -265,6 +252,7 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
*angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f;
*speed = -(*speed);
}
// ANGLE_LIMIT_360_TO_180_ABS(*angle);
// float rotation = *angle - *last_angle;
// ANGLE_LIMIT_360_TO_180_ABS(rotation);
// if (rotation > 90) {
@@ -285,51 +273,55 @@ static void SteeringWheelCalculate()
{
float offset_rf, offset_lb; // 用于计算舵轮的角度
float at_rf_last, at_lb_last; // 上次的角度
at_rf_last = motor_steering_rf->measure.total_angle;
at_lb_last = motor_steering_lb->measure.total_angle;
// at_lf_last = motor_steering_lf->measure.total_angle;
at_rf_last = motor_steering_rf->measure.total_angle;
// at_rb_last = motor_steering_rb->measure.total_angle;
if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) {
// at_lf = at_lf_last;
// at_rf = at_rf_last;
// at_lb = at_lb_last;
// at_rb = at_rb_last;
vt_rf = 0, vt_lb = 0;
vt_lb = 0, vt_rf = 0;
} else {
// 生成预计算变量，减少计算量，空间换时间
float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
// float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
// temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
// // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
// // temp_x = chassis_vx + w; // temp_y = chassis_vx + w;与上次相同因此注释
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
// temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释
// // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+
float temp_x = chassis_vx - w, temp_y = chassis_vy + w;
arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
temp_x = chassis_vx + w;
temp_y = chassis_vy - w;
temp_y = chassis_vy + w;//与上次相同因此注释
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
// temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释
arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+
// 计算角度偏移
// offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x-
offset_rf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+
offset_lb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb: y+ , x-注意
// offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+
offset_lb = atan2f(chassis_vy + w, chassis_vx - w) * RAD_2_DEGREE; // lb: y+ , x-
offset_rf = atan2f(chassis_vy - w, chassis_vx + w) * RAD_2_DEGREE; // rf: y- , x+
at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
// at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf;
at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb;
// at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb;
// ANGLE_LIMIT_360_TO_180_ABS(at_lf);
ANGLE_LIMIT_360_TO_180_ABS(at_rf);
ANGLE_LIMIT_360_TO_180_ABS(at_lb);
// ANGLE_LIMIT_360_TO_180_ABS(at_rb);
// MinmizeRotation(&at_lf, &at_lf_last, &vt_lf);
// MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
MinmizeRotation(&at_lb, &at_lb_last, &vt_lb);
MinmizeRotation(&at_rf, &at_rf_last, &vt_rf);
MinmizeRotation(&at_lb, &at_lb_last, &vt_lb);
// MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
}
// DJIMotorSetRef(motor_steering_lf, at_lf);
DJIMotorSetRef(motor_steering_rf, at_rf);
DJIMotorSetRef(motor_steering_lb, at_lb);
// DJIMotorSetRef(motor_steering_rb, at_rb);
}
/**
@@ -348,100 +340,17 @@ static void SteeringWheelCalculate()
* @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
*
*/
/**
* @brief 根据裁判系统和电容剩余容量对输出进行限制，并应用 WJTLU 功率模型
*/
static void LimitChassisOutput()
{
float chassis_max_power = 0;
float chassis_set_power = 90.0f; // 设定的期望功率，可根据需求修改
// 功率限制待添加
// referee_data->PowerHeatData.chassis_power;
// referee_data->PowerHeatData.chassis_power_buffer;
// 获取裁判系统数据
uint16_t referee_power_limit = referee_data->GameRobotState.chassis_power_limit;
float energy_buffer = referee_data->PowerHeatData.chassis_power_buffer;
// 完成功率限制后进行电机参考输入设定
// 1. ============ 超级电容与裁判系统功率统筹 ============
if (SuperCapIsOnline(cap)) {
SuperCap_Msg_s cap_msg = SuperCapGet(cap);
if (cap_msg.ready == READY) {
// 超电正常可用，动态按比例缩放允许功率
if (cap_msg.chassis_power > chassis_set_power) {
chassis_max_power = chassis_set_power * (chassis_set_power / (float)cap_msg.chassis_power);
} else {
chassis_max_power = chassis_set_power;
}
} else {
// 超电保护中，使用裁判系统限制
if (cap_msg.chassis_power > referee_power_limit) {
chassis_max_power = referee_power_limit * ((float)referee_power_limit / (float)cap_msg.chassis_power);
} else {
chassis_max_power = referee_power_limit;
}
}
// LKMotorSetRef( motor_lf , vt_lf );
// LKMotorSetRef( motor_rb , vt_rb );
// 缓冲能量补偿逻辑
uint8_t power_offset = 0;
if (energy_buffer < WARRING_ENERGY_BUFFER) {
power_offset = (uint8_t)((WARRING_ENERGY_BUFFER - energy_buffer) * 0.2f);
}
SuperCapSetPowerOffset(cap, power_offset);
chassis_max_power -= power_offset;
} else {
// 超电离线时逻辑
chassis_max_power = (energy_buffer < WARRING_ENERGY_BUFFER) ?
(referee_power_limit * energy_buffer * 0.02f) : referee_power_limit;
}
// 2. ============ 基于 LK9015 的 WJTLU 二次功率模型限流 ============
float initial_total_power = 0;
float initial_motor_power[2];
LKMotorInstance* motors[2] = {motor_rf, motor_lb}; // 注意舵轮底盘只有两个驱动轮
float current_pid_out[2];
for (int i = 0; i < 2; i++) {
// 修正 1：LK 电机驱动中，speed_PID 是实例的直接成员
current_pid_out[i] = motors[i]->speed_PID.Output;
// 修正 2：LK 的速度单位是 rad/s，WJTLU模型要求 RPM，进行单位转换 (rad/s * 30/pi ≈ RPM)
float rpm = motors[i]->measure.speed_rads * 9.549296f;
initial_motor_power[i] = current_pid_out[i] * TOQUE_COEFF * rpm +
K1 * current_pid_out[i] * current_pid_out[i] +
K2 * rpm * rpm + CONSTANT_POWER;
if (initial_motor_power[i] > 0) initial_total_power += initial_motor_power[i];
}
// 3. 动态修改电机 PID 的 MaxOut 属性
if (initial_total_power > chassis_max_power && initial_total_power > 0) {
float scale = chassis_max_power / initial_total_power;
for (int i = 0; i < 2; i++) {
float scaled_p = initial_motor_power[i] * scale;
if (scaled_p <= CONSTANT_POWER) {
motors[i]->speed_PID.MaxOut = 0; // 该轮分配不到功率
continue;
}
// 同步修正单位
float rpm = motors[i]->measure.speed_rads * 9.549296f;
float b = TOQUE_COEFF * rpm;
float c = K2 * rpm * rpm - scaled_p + CONSTANT_POWER;
float delta = b * b - 4 * K1 * c;
if (delta >= 0) {
// 解方程获得当前允许的最大电流
float out_limit = (-b + sqrtf(delta)) / (2.0f * K1);
// 修正 3：动态限制 LK 电机 speed_PID 的最大输出（LK9015 默认最大输出为 12000）
motors[i]->speed_PID.MaxOut = fminf(out_limit, 12000.0f);
}
}
} else {
// 功率充裕时，恢复电机的默认最大输出
motor_rf->speed_PID.MaxOut = 12000.0f;
motor_lb->speed_PID.MaxOut = 12000.0f;
}
// 4. 下发目标速度（限流操作已通过改变 MaxOut 动态生效）
LKMotorSetRef( motor_rf , vt_rf );
LKMotorSetRef( motor_lb , vt_lb );
}
@@ -459,7 +368,8 @@ static void EstimateSpeed()
}
/* 机器人底盘控制核心任务 */
void ChassisTask() {
void ChassisTask()
{
// 后续增加没收到消息的处理(双板的情况)
// 获取新的控制信息
#ifdef ONE_BOARD
@@ -492,7 +402,7 @@ void ChassisTask() {
chassis_cmd_recv.wz = 0;
break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
//chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0);
// float single_round_offset = chassis_cmd_recv.offset_angle;
float single_round_offset = chassis_cmd_recv.offset_angle;
if(single_round_offset > 180.0f)
{
@@ -502,25 +412,23 @@ void ChassisTask() {
{
single_round_offset += 360.0f;
}
chassis_cmd_recv.wz = -PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/70.0f;
chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
// chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100;
break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
// chassis_cmd_recv.wz = 0.75;
chassis_cmd_recv.wz = -0.75;//实验版
chassis_cmd_recv.wz = 0.5;
break;
default:
break;
}
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东.)
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
static float sin_theta, cos_theta;
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
// chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
// chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
chassis_vy = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;//实验版
chassis_vx = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
chassis_vy = -chassis_cmd_recv.vx * sin_theta - chassis_cmd_recv.vy * cos_theta;
// 根据控制模式进行正运动学解算,计算底盘输出
SteeringWheelCalculate();
@@ -539,16 +447,6 @@ void ChassisTask() {
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
// ================== 发送超级电容控制指令 ==================
// 每帧向超电控制板发送最新状态
SuperCap_TxMsg_s cap_tx = {
.Enable = 1,
.Reserved1 = 0, // 可根据需求动态关断充电
.Powerlimit = referee_data->GameRobotState.chassis_power_limit,
.Reserved2 = 0 // PLUS版本忽略此位
};
SuperCapSend(cap, &cap_tx);
// 推送反馈消息
#ifdef ONE_BOARD
PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);

185
application/cmd/robot_cmd.c
View File

@@ -8,12 +8,10 @@
#include "message_center.h"
#include "general_def.h"
#include "dji_motor.h"
#include "referee_task.h"
// bsp
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "main.h"
#include "shoot.h"
// 私有宏,自动将编码器转换成角度值
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
@@ -46,7 +44,6 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
static Robot_Status_e robot_state; // 机器人整体工作状态
static uint8_t robot_level = 1; // 机器人等级，范围1-10，上电重置为1
void RobotCMDInit()
{
@@ -191,7 +188,7 @@ static void RemoteControlSet()
}
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
{
//gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch.yaw_motor_total_angle-around*360;
//gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch_data.yaw_motor_total_angle-around*360;
chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle - 360*around;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
@@ -206,18 +203,13 @@ static void RemoteControlSet()
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
if (switch_is_down(rc_data[TEMP].rc.switch_left) )
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_r_;
gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_r1;
}
// 云台软件限位
if (gimbal_cmd_send.pitch > PITCH_MAX_ANGLE)
gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
else if (gimbal_cmd_send.pitch < PITCH_MIN_ANGLE)
gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = (0.25f * (float)rc_data[TEMP].rc.rocker_r_); // _水平方向
chassis_cmd_send.vy = (0.25f * (float)rc_data[TEMP].rc.rocker_r1); // 1数值方向 .
chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_l_; // _水平方向
chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_l1; // 1数值方向
// // 发射参数
// if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
@@ -231,8 +223,8 @@ static void RemoteControlSet()
else
shoot_cmd_send.friction_mode = FRICTION_OFF;
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
if (rc_data[TEMP].rc.dial >500)
shoot_cmd_send.load_mode = LOAD_3_BULLET;
if (rc_data[TEMP].rc.dial < -500)
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
else
shoot_cmd_send.load_mode = LOAD_STOP;
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
@@ -245,74 +237,13 @@ static void RemoteControlSet()
*/
static void MouseKeySet()
{
// // G键控制机器人等级，每次按G键等级+1，最高10级/
// if (rc_data[TEMP].key[KEY_PRESS].g && rc_data[TEMP].key_count[KEY_PRESS][Key_G] > 0) {
// robot_level = (robot_level % 10) + 1; // 等级循环增加，1-10
// }
// G键控制机器人等级，每次按G键等级+1，最高10级
if (rc_data[TEMP].key[KEY_PRESS].g && rc_data[TEMP].key_count[KEY_PRESS][Key_G] > 0) {
robot_level = (robot_level % 10) + 1; // 等级循环增加，1-10
rc_data[TEMP].key_count[KEY_PRESS][Key_G] = 0; // 清除计数器，防止重复执行
}
// B键刷新裁判系统UI
if (rc_data[TEMP].key[KEY_PRESS].b && rc_data[TEMP].key_count[KEY_PRESS][Key_B] > 0) {
MyUIInit(); // 重新初始化UI
rc_data[TEMP].key_count[KEY_PRESS][Key_B] = 0; // 清除计数器，防止重复执行
}
// 将robot_level传递给底盘控制命令
chassis_cmd_send.robot_level = robot_level;
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 42 - rc_data[TEMP].key[KEY_PRESS].d * 42; // w向前，s向后
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 42 - rc_data[TEMP].key[KEY_PRESS].s * 42; // a向左，d向右
// switch (robot_level)
// {
// case 1:
// case 2:
// case 3:
// chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 20 - rc_data[TEMP].key[KEY_PRESS].d * 20); // w向前，s向后
// chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 20 - rc_data[TEMP].key[KEY_PRESS].s * 20); // a向左，d向右
// break;
// case 4:
// case 5:
// case 6:
// case 7:
// chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 30 - rc_data[TEMP].key[KEY_PRESS].d * 30); // w向前，s向后
// chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 30 - rc_data[TEMP].key[KEY_PRESS].s * 30); // a向左，d向右
// break;
// default:
// chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 40 - rc_data[TEMP].key[KEY_PRESS].d * 40); // w向前，s向后
// chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 40 - rc_data[TEMP].key[KEY_PRESS].s * 40); // a向左，d向右
// break;
// }
// 根据等级设置底盘速度
// if (robot_level >= 1 && robot_level <= 3) {
// chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 20 - rc_data[TEMP].key[KEY_PRESS].d * 20); // w向前，s向后
// chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 20 - rc_data[TEMP].key[KEY_PRESS].s * 20); // a向左，d向右
// } else if (robot_level >= 4 && robot_level <= 7) {
// chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 30 - rc_data[TEMP].key[KEY_PRESS].d * 30); // w向前，s向后
// chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 30 - rc_data[TEMP].key[KEY_PRESS].s * 30); // a向左，d向右
// } else {
// chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 40 - rc_data[TEMP].key[KEY_PRESS].d * 40); // w向前，s向后
// chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 40 - rc_data[TEMP].key[KEY_PRESS].s * 40); // a向左，d向右
// }
// chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
// chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
//chassis_cmd_send.vx = (rc_data[TEMP].key[KEY_PRESS].a * 27 - rc_data[TEMP].key[KEY_PRESS].d * 27); // w向前，s向后
//chassis_cmd_send.vy = (rc_data[TEMP].key[KEY_PRESS].w * 27 - rc_data[TEMP].key[KEY_PRESS].s * 27); // a向左，d向右
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 8;
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
// 云台软件限位1
if (gimbal_cmd_send.pitch > PITCH_MAX_ANGLE)
gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
else if (gimbal_cmd_send.pitch < PITCH_MIN_ANGLE)
gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速/1
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
{
case 0:
shoot_cmd_send.bullet_speed = 15;
@@ -324,45 +255,20 @@ static void MouseKeySet()
shoot_cmd_send.bullet_speed = 30;
break;
}
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
// {
// case 0:
// shoot_cmd_send.load_mode = LOAD_STOP;
// break;
// case 1:
// shoot_cmd_send.load_mode = LOAD_1_BULLET;
// break;
// case 2:
// shoot_cmd_send.load_mode = LOAD_3_BULLET;
// break;
// default:
// shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
// break;
// }
// switch (rc_data[TEMP].mouse.press_l) // 鼠标左键射击
// {
// case 0:
// shoot_cmd_send.load_mode = LOAD_STOP;
// break;
// default:
// shoot_cmd_send.load_mode = LOAD_1_BULLET; // 按鼠标进行单发
// break;
// }
// switch (rc_data[TEMP].mouse.press_r) // 鼠标右键射击
// {
// case 0:
// shoot_cmd_send.load_mode = LOAD_STOP;
// break;
// default:
// shoot_cmd_send.load_mode = LOAD_3_BULLET; // 按鼠标进行单发
// break;
// }
if (rc_data[TEMP].mouse.press_l) {
shoot_cmd_send.load_mode = LOAD_BURSTFIRE; // 左键单发
} else if (rc_data[TEMP].mouse.press_r) {
shoot_cmd_send.load_mode = LOAD_3_BULLET; // 右键三连发
} else {
shoot_cmd_send.load_mode = LOAD_STOP; // 不按鼠标不发射
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
{
case 0:
shoot_cmd_send.load_mode = LOAD_STOP;
break;
case 1:
shoot_cmd_send.load_mode = LOAD_1_BULLET;
break;
case 2:
shoot_cmd_send.load_mode = LOAD_3_BULLET;
break;
default:
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
{
@@ -397,18 +303,7 @@ static void MouseKeySet()
chassis_cmd_send.chassis_speed_buff = 100;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键设置底盘运动模式
{
case 0:
chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle - 360*around;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
break;
default:
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
around= chassis_cmd_send.offset_angle/360;
break;
}
switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量。
switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
{
case 1:
@@ -421,9 +316,6 @@ static void MouseKeySet()
}
/**
*
*
*
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
* 停止的阈值'300'待修改成合适的值,或改为开关控制.
*
@@ -437,8 +329,6 @@ static void EmergencyHandler()
{
robot_state = ROBOT_STOP;
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
gimbal_cmd_send.yaw = -gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;;//0
gimbal_cmd_send.pitch = 0;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF;
@@ -455,7 +345,8 @@ static void EmergencyHandler()
}
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask() {
void RobotCMDTask()
{
// 从其他应用获取回传数据
#ifdef ONE_BOARD
SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data);
@@ -469,25 +360,11 @@ void RobotCMDTask() {
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle();
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
if (switch_is_down(rc_data[TEMP].rc.switch_left)){ // 遥控器左侧开关状态为[下],遥控器控制
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
RemoteControlSet();
shoot_cmd_send.shoot_mode =SHOOT_ON;
shoot_cmd_send.friction_mode = FRICTION_OFF;
}
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) {
// 遥控器左侧开关状态为[上],键盘控制
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
MouseKeySet();
shoot_cmd_send.shoot_mode =SHOOT_ON;
}
else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) {
// 遥控器左侧开关状态为[中],视觉模式，使用遥控器控制底盘和云台，同时开启发射系统
RemoteControlSet();
gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
shoot_cmd_send.shoot_mode =SHOOT_ON;
shoot_cmd_send.bullet_speed = 30;
shoot_cmd_send.friction_mode = FRICTION_ON;
}
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
// 设置视觉发送数据,还需增加加速度和角速度数据

100
application/gimbal/gimbal.c
View File

@@ -14,9 +14,7 @@ static Publisher_t *gimbal_pub; // 云台应用消息发布者
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
static float last_yaw_angle = 0.0f; // 保护模式前的yaw角度
// static float last_pitch_angle = 0.0f; // 保护模式前的pitch角度
uint8_t in_protection_mode = 0; // 是否在保护模式
void GimbalInit()
{
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
@@ -28,10 +26,10 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
//.Kp = 0.685, // 8
.Kp = 0.585, // 8
//.Kp = 0.785, // 8
.Kp = 3, // 8
//.Ki = 0.315,
.Ki = 0.25,
.Ki = 0.45,
.Kd = 0.03,
//.DeadBand = 0.1
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
@@ -42,9 +40,9 @@ void GimbalInit()
.Derivative_LPF_RC = 0.001
},
.speed_PID = {
.Kp = 8000, // 50
.Kp = 500, // 50
//.Kp = 5000, // 50
.Ki = 5000, // 200
.Ki = 2000, // 200
.Kd = 0,
.CoefB = 1,
.CoefA = 10,
@@ -68,45 +66,24 @@ void GimbalInit()
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 2,//2
.tx_id = 4,//2
},
// .controller_param_init_config = {
// .angle_PID = {
// .Kp = 40, // 10
// .Ki = 0,
// .Kd = 0,
// .MaxOut = 1000,
// },
// .speed_PID = {
// .Kp = 60, // 50
// .Ki = 15, // 350
// .Kd = 0.6, // 0
// .MaxOut = 20000,//20000
// },
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.75, //0.55
.Ki = 0.05,//0.05
.Kd = 0.02,//0.02
.CoefA =0.5,//0.5
.CoefB = 0.6,//0.6
.Output_LPF_RC = 0.01,
.DeadBand = 0.005,//0.02
.Derivative_LPF_RC=0.01,//0.008
.Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter,
.IntegralLimit = 1,
.MaxOut = 600,
.Kp = 10, // 10
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 500,//22000
.Ki = 0,//
.Kd =0.0005,
// .CoefA = 0.8,
// .CoefB = 0.1,
.Output_LPF_RC = 0.002,//0.002
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter,
.IntegralLimit = 5000,
.MaxOut = 20000,
.Kp = 50, // 50
.Ki = 350, // 350
.Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.MaxOut = 8000,//20000
},
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
@@ -117,12 +94,9 @@ void GimbalInit()
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
// .outer_loop_type = SPEED_LOOP,//调试用
// .close_loop_type = SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020,
.motor_type = DM4310,
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
@@ -145,25 +119,11 @@ void GimbalTask()
{
// 停止
case GIMBAL_ZERO_FORCE:
// 进入保护模式时记录当前角度
if (!in_protection_mode) {
last_yaw_angle = gimba_IMU_data->YawTotalAngle;
// last_pitch_angle = gimba_IMU_data->Pitch;
in_protection_mode = 1;
}
DJIMotorStop(yaw_motor);
DJIMotorStop(pitch_motor);
break;
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
case GIMBAL_GYRO_MODE: // 后续只保留此模式
// 从保护模式退出时
if (in_protection_mode) {
// 计算零飘导致的角度误差
float angle_error = gimba_IMU_data->YawTotalAngle - last_yaw_angle;
// 补偿目标角度，避免突然旋转
gimbal_cmd_recv.yaw -= angle_error;
in_protection_mode = 0;
}
DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
@@ -172,6 +132,17 @@ void GimbalTask()
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
// // 根据底盘速度进行补偿，保持云台稳定
// // 这里简化处理，实际应用中可能需要更复杂的补偿算法
// float yaw_ref = gimbal_cmd_recv.yaw;
// float pitch_ref = gimbal_cmd_recv.pitch;
//
// // 当底盘移动时，云台需要保持相对稳定
// // 这里可以根据底盘速度调整云台的控制参考值
// // 例如，当底盘向前移动时，云台可能需要稍微向后调整以保持稳定
//
// DJIMotorSetRef(yaw_motor, yaw_ref); // yaw和pitch会在robot_cmd中处理好多圈和单圈
// DJIMotorSetRef(pitch_motor, pitch_ref);
break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
@@ -200,3 +171,12 @@ void GimbalTask()
// 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}
// /**
// * @brief 获取云台IMU数据指针
// *
// * @return attitude_t* 云台IMU数据指针
// */
// attitude_t *GetGimbalIMUData()
// {
// return gimba_IMU_data;
// }

6
application/gimbal/gimbal.h
View File

@@ -1,6 +1,6 @@
#ifndef GIMBAL_H
#define GIMBAL_H
#include "../modules/imu/ins_task.h"
/**
* @brief 初始化云台,会被RobotInit()调用
*
@@ -9,8 +9,8 @@ void GimbalInit();
/**
* @brief 云台任务
*
* @return attitude_t* 云台IMU数据指针
*/
void GimbalTask();
//attitude_t *GetGimbalIMUData();
#endif // GIMBAL_H

22
application/robot_def.h
View File

@@ -26,24 +26,24 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD -3595 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_CHASSIS_ALIGN_ECD 1190 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 30 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE -20 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
// 底盘参数
// #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘
// #define CHASSIS_MCNAMEE_WHEEL // 是否为麦克纳姆轮底盘
#define CHASSIS_STEERING_WHEEL // 是否为舵轮
#define STEERING_CHASSIS_ALIGN_ECD_RF 7330 // 舵电机 A 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ECD_LB 5099 // 舵电机 D 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ECD_RF 7367 // 舵电机 A 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ECD_LB 1195 // 舵电机 D 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度
#define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 51.0f // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 7 // 拨盘一圈的装载量
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
// 机器人底盘修改的参数,单位为mm(毫米)
#define WHEEL_BASE 350 // 纵向轴距(前进后退方向)
#define TRACK_WIDTH 300 // 横向轮距(左右平移方向)
@@ -154,7 +154,7 @@ typedef struct
chassis_mode_e chassis_mode;
int chassis_speed_buff;
// UI部分
uint8_t robot_level; // 机器人等级
// ...
} Chassis_Ctrl_Cmd_s;
@@ -178,7 +178,6 @@ typedef struct
Bullet_Speed_e bullet_speed; // 弹速枚举
uint8_t rest_heat;
float shoot_rate; // 连续发射的射频,unit per s,发/秒
float dead_time; // 每发弹丸的间隔时间,unit ms,毫秒
} Shoot_Ctrl_Cmd_s;
/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
@@ -208,8 +207,7 @@ typedef struct
{
attitude_t gimbal_imu_data;
uint16_t yaw_motor_single_round_angle;
//uint16_t yaw_motor_total_angle;
float yaw_motor_total_angle;
uint16_t yaw_motor_total_angle;
} Gimbal_Upload_Data_s;
typedef struct

105
application/shoot/shoot.c
View File

@@ -17,7 +17,7 @@ static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信
// dwt定时,计算冷却用
static float hibernate_time = 0, dead_time = 0;
//static float target_angle = 0;
void ShootInit()
{
// 左摩擦轮
@@ -27,16 +27,16 @@ void ShootInit()
},
.controller_param_init_config = {
.speed_PID = {
.Kp = 20, // 20
.Ki = 1, // 1
.Kp = 0, // 20
.Ki = 0, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
.MaxOut = 15000,
},
.current_PID = {
.Kp = 0.7, // 0.7
.Ki = 0.1, // 0.1
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
@@ -52,49 +52,49 @@ void ShootInit()
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = M3508};
friction_config.can_init_config.tx_id = 5,
friction_r = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 8; // 左摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_config.can_init_config.tx_id = 1,
friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_r = DJIMotorInit(&friction_config);
// 拨盘电机
Motor_Init_Config_s loader_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 7,
.tx_id = 5,
},
.controller_param_init_config = {
.angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 12, // 10
.Ki = 5,
.Kp = 0, // 10
.Ki = 0,
.Kd = 0,
.MaxOut = 10000,
.MaxOut = 200,
},
.speed_PID = {
.Kp = 3, // 10
.Ki = 10, // 1
.Kp = 0, // 10
.Ki = 0, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 250,
.MaxOut = 10000,
.IntegralLimit = 5000,
.MaxOut = 5000,
},
.current_PID = {
.Kp = 1.5, // 0.7
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 10000,
.MaxOut = 5000,
},
},
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = ANGLE_LOOP | SPEED_LOOP|CURRENT_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向
.close_loop_type = CURRENT_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
},
.motor_type = M2006 // 英雄使用m3508
};
@@ -126,8 +126,8 @@ void ShootTask()
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
if (hibernate_time + dead_time > DWT_GetTimeline_ms())
return;
// if (hibernate_time + dead_time > DWT_GetTimeline_ms())
// return;
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
switch (shoot_cmd_recv.load_mode)
@@ -138,51 +138,18 @@ void ShootTask()
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
// case LOAD_1_BULLET:
// DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
// static float last_target_angle = 0; // 存储上次的目标角度
// if (dead_time == 0) { // 第一次触发
// last_target_angle = loader->measure.total_angle;
// }
// last_target_angle += 51*36;//ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER; // 每次都在上次目标角度基础上增加固定角度
// DJIMotorSetRef(loader, last_target_angle);
// hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
// dead_time = 450;//shoot_cmd_recv.dead_time; // 完成1发弹丸发射的时间
// break;
// case LOAD_1_BULLET:
// DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环
// DJIMotorSetRef(loader, loader->measure.total_angle+51*36);
// //DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE);
// // 控制量增加一发弹丸的角度，其中90为36减速比*1.5
// hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
// dead_time = 1000; // 完成1发弹丸发射的时间
// // 记录触发指令的时间
// //dead_time = 500;
// // 完成1发弹丸发射的时间
// break;
case LOAD_1_BULLET:
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
static float target_angle = -1900;
target_angle += 51.2*36;
//float target_angle = loader->measure.total_angle + 51*36;
DJIMotorSetRef(loader, target_angle);
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 450; // 完成1发弹丸发射的时间
dead_time = 150; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
case LOAD_3_BULLET:
// DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
// DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
// hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
// dead_time = 300; // 完成3发弹丸发射的时间
// break;
DJIMotorOuterLoop(loader, ANGLE_LOOP);// 切换到角度环.
//static float target_angle = -1872;
target_angle += (51.2*36)*3;
//float target_angle = loader->measure.total_angle + 51*36;
DJIMotorSetRef(loader, target_angle);
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 700; // 完成3发弹丸发射的时间
dead_time = 300; // 完成3发弹丸发射的时间
break;
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE:
@@ -208,16 +175,16 @@ void ShootTask()
switch (shoot_cmd_recv.bullet_speed)
{
case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 50000);
DJIMotorSetRef(friction_r, 50000);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 30000);

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 = -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;
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;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)

1
modules/imu/ins_task.c
View File

@@ -135,6 +135,7 @@ void INS_Task(void)
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);

2
modules/master_machine/master_process.c
View File

@@ -5,7 +5,7 @@
* @version beta
* @date 2022-11-03
* @todo 增加对串口调试助手协议的支持,包括vofa和serial debug
* @copyright Copyright (c) 2022
* @copyright Copyright (c) 2023
*
*/
#include "master_process.h"

14
modules/motor/DJImotor/dji_motor.c
View File

@@ -24,7 +24,7 @@ static CANInstance sender_assignment[6] = {
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
};
@@ -321,15 +321,25 @@ void DJIMotorControl()
// 获取最终输出
set = (int16_t)pid_ref;
// 分组填入发送数据
group = motor->sender_group;
num = motor->message_num;
if (i == 1)
{
sender_assignment[4].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位
sender_assignment[4].tx_buff[7] = (uint8_t)(set >> 8); // 低八位
}
else
{
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
}
// 若该电机处于停止状态,直接将buff置零
if (motor->stop_flag == MOTOR_STOP)
memset(sender_assignment[group].tx_buff + 2 * num, 0, sizeof(uint16_t));
memset(sender_assignment[group].tx_buff + 2 * num, 0, 2u);
}
// 遍历flag,检查是否要发送这一帧报文

48
modules/referee/referee_task.c
View File

@@ -57,7 +57,7 @@ static Graph_Data_t UI_shoot_line[10]; // 射击准线
static Graph_Data_t UI_Energy[3]; // 电容能量条
static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change
static uint32_t shoot_line_location[10] = {525, 950, 490, 515, 565};
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
void MyUIInit()
{
@@ -66,19 +66,19 @@ void MyUIInit()
while (referee_recv_info->GameRobotState.robot_id == 0)
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
DeterminRobotID(); // 确定ui要发送到的目标客户端.
DeterminRobotID(); // 确定ui要发送到的目标客户端
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
// 绘制发射基准线
//UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 860, 475, 1060, 475);
//UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);
// 绘制车辆状态标志指示
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "level:");
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
@@ -91,7 +91,7 @@ void MyUIInit()
// 绘制车辆状态标志，动态
// 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "1");
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
@@ -116,7 +116,7 @@ void MyUIInit()
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
}
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常.
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
static uint8_t count = 0;
static uint16_t count1 = 0;
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
@@ -176,15 +176,27 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
{
UIChangeCheck(_Interactive_data);
// robot level
if (_Interactive_data->Referee_Interactive_Flag.robot_level_flag == 1)
// chassis
if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
{
char level_str[2];
level_str[0] = '0' + _Interactive_data->robot_level;
level_str[1] = '\0';
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, level_str);
switch (_Interactive_data->chassis_mode)
{
case CHASSIS_ZERO_FORCE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
break;
case CHASSIS_ROTATE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate ");
// 此处注意字数对齐问题，字数相同才能覆盖掉
break;
case CHASSIS_NO_FOLLOW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
break;
case CHASSIS_FOLLOW_GIMBAL_YAW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
break;
}
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
_Interactive_data->Referee_Interactive_Flag.robot_level_flag = 0;
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
}
// gimbal
if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
@@ -249,12 +261,6 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
*/
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
{
if (_Interactive_data->robot_level != _Interactive_data->robot_last_level)
{
_Interactive_data->Referee_Interactive_Flag.robot_level_flag = 1;
_Interactive_data->robot_last_level = _Interactive_data->robot_level;
}
if (_Interactive_data->chassis_mode != _Interactive_data->chassis_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 1;

3
modules/referee/rm_referee.h
View File

@@ -54,7 +54,6 @@ typedef struct
uint32_t lid_flag : 1;
uint32_t friction_flag : 1;
uint32_t Power_flag : 1;
uint32_t robot_level_flag : 1;
} Referee_Interactive_Flag_t;
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
@@ -62,7 +61,6 @@ typedef struct
{
Referee_Interactive_Flag_t Referee_Interactive_Flag;
// 为UI绘制以及交互数据所用
uint8_t robot_level; // 机器人等级
chassis_mode_e chassis_mode; // 底盘模式
gimbal_mode_e gimbal_mode; // 云台模式
shoot_mode_e shoot_mode; // 发射模式设置
@@ -71,7 +69,6 @@ typedef struct
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
// 上一次的模式，用于flag判断
uint8_t robot_last_level;
chassis_mode_e chassis_last_mode;
gimbal_mode_e gimbal_last_mode;
shoot_mode_e shoot_last_mode;

29
modules/remote/remote_control.c
View File

@@ -20,13 +20,32 @@ static DaemonInstance *rc_daemon_instance;
* @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
*
*/
static void RectifyRCjoystick()
// static void RectifyRCjoystick()
// {
// for (uint8_t i = 0; i < 5; ++i)
// if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
// *(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
// }
static void RectifyRCjoystick()//左遥杆水平向右限幅不够，因为下面的+3，将+3删除就用上面的
{
int16_t *rocker_ptr = &rc_ctrl[TEMP].rc.rocker_l_;
for (uint8_t i = 0; i < 5; ++i)
if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
*(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
{
if (i == 0) // 左遥杆水平方向
{
// 增加右向最大值到700，左向保持660不变
if (rocker_ptr[i] > 700)
rocker_ptr[i] = 700;
else if (rocker_ptr[i] < -660)
rocker_ptr[i] = -660;
}
else // 其他通道保持原限制
{
if (abs(rocker_ptr[i]) > 660)
rocker_ptr[i] = 0;
}
}
}
/**
* @brief 遥控器数据解析
*
@@ -40,7 +59,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf)
rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3
rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+2.5;
rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+3;
// 左侧拨轮
RectifyRCjoystick();
// 开关,0左1右

20
modules/super_cap/super_cap.c
View File

@@ -5,7 +5,6 @@
#include "super_cap.h"
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h" // 【新增】引入获取时间的头文件，或者是 "main.h" 用 HAL_GetTick()
static SuperCapInstance *super_cap_instance = NULL;
@@ -16,9 +15,6 @@ static void SuperCapRxCallback(CANInstance *_instance)
rxbuff = _instance->rx_buff;
Msg = &super_cap_instance->cap_msg;
// 【新增】更新最后一次收到消息的时间戳
super_cap_instance->last_update_tick = DWT_GetTimeline_ms(); // 如果没有DWT，用 HAL_GetTick() 替换
// 按照新协议解析接收数据
Msg->ready = (SuperCapReadyEnum)rxbuff[0];
Msg->state = (SuperCapStateEnum)rxbuff[1];
@@ -28,18 +24,6 @@ static void SuperCapRxCallback(CANInstance *_instance)
Msg->bat_power = rxbuff[5];
}
// 【新增】在线判断函数
uint8_t SuperCapIsOnline(SuperCapInstance *instance)
{
if (instance == NULL) return 0;
// 如果当前时间减去上次接收时间超过 1000ms (1秒)，则认为离线
if (DWT_GetTimeline_ms() - instance->last_update_tick > 2000) {
return 0; // 离线
}
return 1; // 在线
}
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
{
super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
@@ -56,9 +40,9 @@ void SuperCapSend(SuperCapInstance *instance, SuperCap_TxMsg_s *tx_data)
// 按照新协议装载发送数据
txbuff[0] = tx_data->Enable;
txbuff[1] = tx_data->Reserved1;
txbuff[1] = tx_data->Charge;
txbuff[2] = tx_data->Powerlimit - instance->power_offset; // 计算功率补偿
txbuff[3] = tx_data->Reserved2;
txbuff[3] = tx_data->ChargePower;
CANTransmit(instance->can_ins, 1);
}

8
modules/super_cap/super_cap.h
View File

@@ -44,9 +44,9 @@ typedef struct
/* 需要发送给超级电容的控制信息结构体 */
typedef struct {
uint8_t Enable; // 超级电容使能。1使能，0失能
uint8_t Reserved1; // Reserved 超级电容充电。1充电，0放电
uint8_t Charge; // 超级电容充电。1充电，0放电
uint8_t Powerlimit; // 裁判系统功率限制
uint8_t Reserved2; // Reserved 超级电容充电功率
uint8_t ChargePower; // 超级电容充电功率
} SuperCap_TxMsg_s;
#pragma pack()
@@ -56,7 +56,6 @@ typedef struct
CANInstance *can_ins; // CAN实例
SuperCap_Msg_s cap_msg; // 超级电容接收信息
uint8_t power_offset; // 功率补偿值
uint32_t last_update_tick; // 【新增】上次收到数据的时间戳
} SuperCapInstance;
/* 超级电容初始化配置 */
@@ -70,9 +69,6 @@ typedef struct
*/
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
// 【新增】函数声明
uint8_t SuperCapIsOnline(SuperCapInstance *instance);
/**
* @brief 发送超级电容控制信息
*/