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

halfsteering

Browse Source
This commit is contained in:
TuxMonkey
2026-03-03 21:44:39 +08:00
parent 6d7d23ebba
commit 596d3d4c65
3 changed files with 205 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

169
User_Code/user_task/half_steering/halfsteering_cmd.c
View File

@@ -1,5 +1,166 @@
//
// Created by esqwt on 2026/3/2.
//
#include "halfsteering_cmd.h"
#include "halfsteering_def.h"
#include "chassis_half_steer.h" // 包含底盘应用层结构体
#include "pid.h"
#include "rc.h"
#include "user_lib.h"
#include <string.h>
#include <math.h>
// --- 外部依赖 ---
extern Chassis_HalfSteer_t chassis_half_steer; // 假设在app层定义好的底盘实例
// --- 本地任务数据 ---
static HalfSteer_Task_Data_t task_data;
static const RC_ctrl_t *local_rc_ctrl;
// ================= PID 参数定义与实例化 =================
static PIDInstance chassis_follow_pid;
// 按你的格式直接在这里定义并初始化配置结构体
static PID_Init_Config_s chassis_follow_pid_conf = {
.Kp = 6.0f,
.Ki = 0.0f,
.Kd = 0.495f,
// .DeadBand = 0.5f,
//.CoefA = 0.2f,
//.CoefB = 0.3f,
//.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit,
//.IntegralLimit = 50.0f,
.MaxOut = 45.0f,
//.Derivative_LPF_RC = 0.01f,
};
// --- 私有函数声明 ---
static void ModeSelection(void);
static void RemoteControlSet(Chassis_Ctrl_Cmd_s *cmd);
/**
* @brief 任务初始化
*/
void RobotCMDInit(void)
{
// 1. 获取遥控器指针
local_rc_ctrl = RC_Get_RC_Pointer(); // 请替换为你实际获取遥控器数据的接口
// 2. 初始化底盘跟随 PID
PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf);
// 3. 任务数据清零
memset(&task_data, 0, sizeof(HalfSteer_Task_Data_t));
task_data.current_mode = MODE_RELAX;
task_data.init_done = 1;
}
/**
* @brief 任务核心循环 (放置于 RTOS Task 中)
*/
void RobotCMDTask(void)
{
if (!task_data.init_done) return;
Chassis_Ctrl_Cmd_s chassis_cmd_send;
memset(&chassis_cmd_send, 0, sizeof(Chassis_Ctrl_Cmd_s));
// 1. 状态机选择
ModeSelection();
// 2. 根据状态设定底层指令
RemoteControlSet(&chassis_cmd_send);
// 3. 将计算完成的指令发送给底盘应用层
Chassis_HalfSteer_Update(&chassis_half_steer, &chassis_cmd_send);
}
/**
* @brief 状态机模式选择 (复刻你的多拨杆判断逻辑)
*/
static void ModeSelection(void)
{
// 左侧[中], 右侧[下] -> 视觉模式
if (switch_is_mid(local_rc_ctrl->rc.s[1]) && switch_is_down(local_rc_ctrl->rc.s[0]))
{
task_data.current_mode = MODE_AUTO_VISION;
}
// 左侧[中], 右侧[中] -> 遥控器不跟随
else if (switch_is_mid(local_rc_ctrl->rc.switch[1])
&&
switch_is_mid(local_rc_ctrl->rc.switch[0])
) {
task_data.current_mode = MODE_NO_FOLLOW;
}
// 左侧[下], 右侧[下] -> 底盘小陀螺
else
if (switch_is_down(local_rc_ctrl->rc.s[1]) && switch_is_down(local_rc_ctrl->rc.s[0]))
{
task_data.current_mode = MODE_REMOTE_SPIN;
}
// 左侧[下], 右侧[中] -> 遥控器不跟随 (你原代码中的独立判断)
else if (switch_is_down(local_rc_ctrl->rc.s[1]) && switch_is_mid(local_rc_ctrl->rc.s[0]))
{
task_data.current_mode = MODE_NO_FOLLOW;
}
// 左侧[下] (单边条件兜底) -> 默认跟随模式
else if (switch_is_down(local_rc_ctrl->rc.s[1]))
{
task_data.current_mode = MODE_REMOTE_FOLLOW;
}
else
{
task_data.current_mode = MODE_RELAX;
}
}
/**
* @brief 遥控器映射与 PID 计算
*/
static void RemoteControlSet(Chassis_Ctrl_Cmd_s *cmd)
{
if (task_data.current_mode == MODE_RELAX)
{
cmd->chassis_mode = CHASSIS_ZERO_FORCE;
return;
}
// --- 1. 底盘基础平移 (水平和竖直方向) ---
cmd->vx = RC_CHASSIS_SPEED_SCALE * ((float) local_rc_ctrl->rc.ch[1] / 660.0f); // 摇杆前进
cmd->vy = RC_CHASSIS_SPEED_SCALE * ((float) local_rc_ctrl->rc.ch[0] / 660.0f); // 摇杆平移
// 摇杆死区处理
if (fabsf(local_rc_ctrl->rc.ch[1]) < RC_DEADBAND) cmd->vx = 0;
if (fabsf(local_rc_ctrl->rc.ch[0]) < RC_DEADBAND) cmd->vy = 0;
// --- 2. 旋转量与模式映射 ---
switch (task_data.current_mode)
{
case MODE_REMOTE_FOLLOW:
cmd->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
// 注意：此处需要你获取到底盘和云台的实际偏差角 (例如从 Gimbal 结构体或电机 feedback 中拿)
// 假设获取到的夹角叫 angle_error，并已转换到 [-180, 180] 之间
float angle_error = 0.0f; /* 替换为获取真实误差的代码 */
// 使用在文件头部实例化的 chassis_follow_pid 计算 wz 输出
cmd->wz = PIDCalculate(&chassis_follow_pid, angle_error, 0.0f) / 100.0f;
break;
case MODE_REMOTE_SPIN:
cmd->chassis_mode = CHASSIS_ROTATE;
cmd->wz = RC_ROTATE_SPEED_SCALE;
break;
case MODE_AUTO_VISION:
// 在你给的代码中，24赛季检录视觉模式时让底盘转小陀螺
cmd->chassis_mode = CHASSIS_ROTATE; // CHASSIS_RE_ROTATE 对应底层小陀螺逻辑
cmd->wz = RC_ROTATE_SPEED_SCALE;
break;
case MODE_NO_FOLLOW:
cmd->chassis_mode = CHASSIS_NO_FOLLOW;
cmd->wz = 0.0f; // 如果需要拨杆控制旋转，可以在这里加入摇杆映射
break;
default:
cmd->chassis_mode = CHASSIS_ZERO_FORCE;
break;
}
}

18
User_Code/user_task/half_steering/halfsteering_cmd.h
View File

@@ -1,8 +1,14 @@
//
// Created by esqwt on 2026/3/2.
//
#ifndef HALFSTEERING_CMD_H
#define HALFSTEERING_CMD_H
#ifndef TRONONEH7_SCAFFOLD_HALFSTEERING_CMD_H
#define TRONONEH7_SCAFFOLD_HALFSTEERING_CMD_H
/**
* @brief 机器人核心控制任务初始化,会被RobotInit()调用
*/
void RobotCMDInit(void);
#endif // TRONONEH7_SCAFFOLD_HALFSTEERING_CMD_H
/**
* @brief 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率)
*/
void RobotCMDTask(void);
#endif // HALFSTEERING_CMD_H

34
User_Code/user_task/half_steering/halfsteering_def.h
View File

@@ -1,8 +1,30 @@
//
// Created by esqwt on 2026/3/3.
//
#ifndef HALF_STEER_DEF_H
#define HALF_STEER_DEF_H
#ifndef TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H
#define TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H
#include "stdint.h"
#endif // TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H
// ================= 遥控器映射系数 =================
#define RC_CHASSIS_SPEED_SCALE 80.0f // 摇杆转平移速度比例
#define RC_ROTATE_SPEED_SCALE 1.0f // 小陀螺模式固定自旋速度
#define RC_DEADBAND 10 // 摇杆死区
// ================= 机器人状态枚举 =================
typedef enum
{
MODE_RELAX = 0, // 急停/掉线模式
MODE_REMOTE_FOLLOW, // 纯遥控器：底盘跟随云台
MODE_REMOTE_SPIN, // 纯遥控器：底盘小陀螺
MODE_AUTO_VISION, // 视觉辅助模式
MODE_NO_FOLLOW, // 云台底盘分离 (不跟随)
} Robot_State_e;
// ================= 内部控制对象结构体 =================
typedef struct
{
Robot_State_e current_mode; // 当前状态机模式
uint8_t init_done; // 初始化完成标志
// 如果有视觉数据或云台数据需要跨函数传递，也可以加在这里
} HalfSteer_Task_Data_t;
#endif // HALF_STEER_DEF_H