MaRuihan
/
STM32_BHBF_SWING
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

03131702

master
L1NG\42961 4 hours ago
parent
fd0da8647c
commit
5a78ebe4bc
3 changed files with 233 additions and 179 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 400
      Core/FSM/Src/Handset_Status_Setting.c
  2. 10
      Core/FSM/Src/fsm_state_control.c
  3. 2
      Core/FSM/Src/robot_move_actions.c

400
Core/FSM/Src/Handset_Status_Setting.c
View File

@ -1,36 +1,50 @@
/*
* Handset_Status_Setting.c
*
* Created on: 2026年1月15日
* Author: bm673
* @file Handset_Status_Setting.c
* @brief 遥控器状态处理、模式管理、按键/摇杆检测、参数补偿驱动文件
* @author bm673
* @date 2026年1月15日
* @details 实现机器人遥控模式切换、急停检测、IO状态获取、姿态补偿等功能
*/
#include "Handset_Status_Setting.h"
#include "BHBF_ROBOT.h"
#include <math.h>
#define ROCKER_THRESHOLD_DEFAULT 300 // 默认遥杆阈值
#define ANGLE_DEAD_ZONE 20 // 角度死区（度）
#define ANGLE_FORWARD_LEFT_MIN 70 // 前进左转最小角度
#define ANGLE_FORWARD_LEFT_MAX 110 // 前进左转最大角度
#define ANGLE_FORWARD_RIGHT_MIN -110 // 前进右转最小角度
#define ANGLE_FORWARD_RIGHT_MAX -70 // 前进右转最大角度
#define ANGLE_REVERSE_MIN 160 // 反向最小角度
#define ANGLE_FORWARD_LIMIT1 70
#define ANGLE_FORWARD_LIMIT2 110
#define ANGLE_LEFT_LIMIT1 160
#define ANGLE_LEFT_LIMIT2 -160
#define ANGLE_RIGHT_LIMIT1 -20
#define ANGLE_RIGHT_LIMIT2 20
#define ANGLE_BACK_LIMIT1 -70
#define ANGLE_BACK_LIMIT2 -110
// ============ 类型定义 ============
/*=========================== 宏定义配置 ===========================*/
// 摇杆阈值配置
#define ROCKER_THRESHOLD_DEFAULT 300U // 摇杆死区阈值
#define ANGLE_DEAD_ZONE 20U // 角度死区(度)
// 机器人运动角度判断阈值
#define ANGLE_FORWARD_LEFT_MIN 70
#define ANGLE_FORWARD_LEFT_MAX 110
#define ANGLE_FORWARD_RIGHT_MIN -110
#define ANGLE_FORWARD_RIGHT_MAX -70
#define ANGLE_REVERSE_MIN 160
// 摇杆方向角度区间定义
#define ANGLE_FORWARD_LIMIT1 70
#define ANGLE_FORWARD_LIMIT2 110
#define ANGLE_LEFT_LIMIT1 160
#define ANGLE_LEFT_LIMIT2 -160
#define ANGLE_RIGHT_LIMIT1 -20
#define ANGLE_RIGHT_LIMIT2 20
#define ANGLE_BACK_LIMIT1 -70
#define ANGLE_BACK_LIMIT2 -110
// 姿态补偿参数配置
#define COMP_DEADZONE 300 // 遥控器补偿生效阈值
#define COMP_COUNT_THRESH 200 // 补偿计数阈值(400ms累加一次)
#define COMP_STEP 10 // 补偿步长
#define COMP_LIMIT_MAX 1000 // 正向补偿最大值
#define COMP_LIMIT_MIN -1000 // 负向补偿最大值
/*=========================== 静态函数声明 ===========================*/
typedef InputEvent (*ModeEventHandler)(void);
// 急停检测
static inline InputEvent CheckEmergencyStop(void);
// 各模式事件处理函数
static InputEvent GetHaltModeEvents(void);
static InputEvent GetManualModeEvents(void);
static InputEvent GetHorizontalModeEvents(void);
@ -39,193 +53,235 @@ static InputEvent GetVerticalLeftModeEvents(void);
static InputEvent GetVerticalRightModeEvents(void);
static InputEvent GetRegionalFlatTaskEvents(void);
static InputEvent GetRegionalHorizontalTaskEvents(void);
static InputEvent GetVerticalRightModeEvents(void);
static const ModeEventHandler modeEventHandlers[MODE_COUNT];
// 辅助功能函数
static InputEvent CheckCommonKeys(void);
static InputEvent CheckAllKeys(void);
static InputEvent CalculateRockerEvent(void);
static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt);
/*=========================== 全局变量 ===========================*/
// 模式-事件处理函数映射表
static const ModeEventHandler modeEventHandlers[MODE_COUNT] = {
[Halt_Mode] = GetHaltModeEvents,
[Manual_Mode] = GetManualModeEvents,
[Horizontal_Mode] = GetHorizontalModeEvents,
[Flat_Mode] = GetFlatModeEvents,
[Vertical_Mode_Left] = GetVerticalLeftModeEvents,
[Vertical_Mode_Right] = GetVerticalRightModeEvents,
[Regional_Horizontal_Automatic_Task] = GetRegionalHorizontalTaskEvents,
[Regional_Flat_Automatic_Task] = GetRegionalFlatTaskEvents,
};
// ============ 模式状态获取 ============
/*=========================== 函数实现 ===========================*/
/**
* @brief 获取机器人当前运行模式
* @param 无
* @return Robot_Mode 当前机器人模式枚举
*/
Robot_Mode RobotRockerState(void)
{
if (GV.PV.Robot_Operation_Mode <= 0|| GV.PV.Robot_Operation_Mode >= MODE_COUNT) {
if (GV.PV.Robot_Operation_Mode <= 0 || GV.PV.Robot_Operation_Mode >= MODE_COUNT)
{
return Halt_Mode;
}
switch (GV.PV.Robot_Operation_Mode) {
case 0: return Halt_Mode; // RunMode0 → 枚举0
case 1: return Manual_Mode; // RunMode1 → 枚举1
case 2: return Horizontal_Mode; // RunMode2 → 枚举2
case 3: return Flat_Mode; // RunMode3 → 枚举3（补全Flat_Mode）
case 4: return Vertical_Mode_Left; // RunMode4 → 枚举4
case 5: return Vertical_Mode_Right; // RunMode5 → 枚举5
case 6: return Regional_Horizontal_Automatic_Task; // RunMode6 → 枚举6
case 7: return Regional_Flat_Automatic_Task; // RunMode7 → 枚举7
default: return Halt_Mode; // 兜底：非法值返回停机，而非手动
switch (GV.PV.Robot_Operation_Mode)
{
case 0: return Halt_Mode;
case 1: return Manual_Mode;
case 2: return Horizontal_Mode;
case 3: return Flat_Mode;
case 4: return Vertical_Mode_Left;
case 5: return Vertical_Mode_Right;
case 6: return Regional_Horizontal_Automatic_Task;
case 7: return Regional_Flat_Automatic_Task;
default: return Halt_Mode;
}
}
// ============ 主按键检测函数 ============
/**
* @brief 遥控器按键/事件主检测函数
* @param current_mode 当前机器人模式
* @return InputEvent 触发的输入事件
*/
InputEvent RemoteControl_GetKeyIndex(Robot_Mode current_mode)
{
// 安全检查
if (current_mode < 0 || current_mode >= MODE_COUNT) {
// 安全校验
if (current_mode < 0 || current_mode >= MODE_COUNT)
{
return INPUT_NONE;
}
// 获取并调用对应的处理函数
// 获取对应模式的事件处理函数并执行
ModeEventHandler handler = modeEventHandlers[current_mode];
return (handler != NULL) ? handler() : INPUT_NONE;
}
// ============ 全局变量 ============
static const ModeEventHandler modeEventHandlers[MODE_COUNT] = {
[Halt_Mode] = GetHaltModeEvents,
[Manual_Mode] = GetManualModeEvents,
[Horizontal_Mode] = GetHorizontalModeEvents,
[Flat_Mode] = GetFlatModeEvents,
[Vertical_Mode_Left] = GetVerticalLeftModeEvents,
[Vertical_Mode_Right] = GetVerticalRightModeEvents,
[Regional_Horizontal_Automatic_Task] = GetRegionalHorizontalTaskEvents,
[Regional_Flat_Automatic_Task] = GetRegionalFlatTaskEvents,
};
// ============ 辅助函数 ============
// 检查常用按键
/**
* @brief 通用常用按键检测（手动/水平/垂直等模式共用）
* @param 无
* @return InputEvent 按键事件
*/
static inline InputEvent CheckCommonKeys(void)
{
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
if (P_MK32->CH4_SA == -1000) return INPUT_KEY_LANE_CHANGE_UP;
if (P_MK32->CH5_SB == -1000) return INPUT_KEY_FORWARD_CRUISE;
if (P_MK32->CH5_SB == 1000) return INPUT_KEY_BACKWARD_CRUISE;
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
if (P_MK32->CH4_SA == -1000) return INPUT_KEY_LANE_CHANGE_UP;
if (P_MK32->CH5_SB == -1000) return INPUT_KEY_FORWARD_CRUISE;
if (P_MK32->CH5_SB == 1000) return INPUT_KEY_BACKWARD_CRUISE;
return INPUT_NONE;
}
/**
* @brief 全按键检测（停机模式专用）
* @param 无
* @return InputEvent 按键事件
*/
static inline InputEvent CheckAllKeys(void)
{
// 按枚举顺序检查所有按键，匹配到则立即返回对应枚举值
if (P_MK32->CH4_SA == -1000) return INPUT_KEY_LANE_CHANGE_UP; // 换道上
if (P_MK32->CH4_SA == 1000) return INPUT_KEY_LANE_CHANGE_DOWN; // 换道下
if (P_MK32->CH5_SB == -1000) return INPUT_KEY_FORWARD_CRUISE; // 前进巡航
if (P_MK32->CH5_SB == 1000) return INPUT_KEY_BACKWARD_CRUISE; // 后退巡航
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP; // 自动作业上
if (P_MK32->CH7_SD == 1000) return INPUT_KEY_AUTO_WORK_DOWN; // 自动作业下
if (P_MK32->CH4_SA == -1000) return INPUT_KEY_LANE_CHANGE_UP;
if (P_MK32->CH4_SA == 1000) return INPUT_KEY_LANE_CHANGE_DOWN;
if (P_MK32->CH5_SB == -1000) return INPUT_KEY_FORWARD_CRUISE;
if (P_MK32->CH5_SB == 1000) return INPUT_KEY_BACKWARD_CRUISE;
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
if (P_MK32->CH7_SD == 1000) return INPUT_KEY_AUTO_WORK_DOWN;
// 无任何按键按下时返回默认值
return INPUT_NONE;
}
// 摇杆事件计算
/**
* @brief 遥控器摇杆方向计算与事件转换
* @param 无
* @return InputEvent 摇杆方向事件
*/
static InputEvent CalculateRockerEvent(void)
{
// 检查摇杆是否在中位
int16_t vert = P_MK32->CH2_LY_V;
int16_t hori = P_MK32->CH3_LY_H;
if ((fabs(vert) < ROCKER_THRESHOLD_DEFAULT) &&
(fabs(hori) < ROCKER_THRESHOLD_DEFAULT)) {
// 摇杆中位判断
if ((fabs(vert) < ROCKER_THRESHOLD_DEFAULT) && (fabs(hori) < ROCKER_THRESHOLD_DEFAULT))
{
return INPUT_ROCKER_STOP;
}
// 计算角度
// 角度计算
double angle_rad = atan2(vert, hori);
int16_t angle_deg = (int16_t)(angle_rad * 180.0 / M_PI);
// 判断方向
if (angle_deg <= ANGLE_FORWARD_LIMIT2 && angle_deg >= ANGLE_FORWARD_LIMIT1) {
return INPUT_ROCKER_FORWARD;
}
if (angle_deg >= ANGLE_LEFT_LIMIT1 || angle_deg <= ANGLE_LEFT_LIMIT2) {
return INPUT_ROCKER_TURN_LEFT;
}
if (angle_deg >= ANGLE_RIGHT_LIMIT1 && angle_deg <= ANGLE_RIGHT_LIMIT2) {
return INPUT_ROCKER_TURN_RIGHT;
}
if (angle_deg >= ANGLE_BACK_LIMIT2 && angle_deg <= ANGLE_BACK_LIMIT1) {
return INPUT_ROCKER_BACKWARD;
}
// 方向判断
if (angle_deg <= ANGLE_FORWARD_LIMIT2 && angle_deg >= ANGLE_FORWARD_LIMIT1)
{
return INPUT_ROCKER_FORWARD;
}
if (angle_deg >= ANGLE_LEFT_LIMIT1 || angle_deg <= ANGLE_LEFT_LIMIT2)
{
return INPUT_ROCKER_TURN_LEFT;
}
if (angle_deg >= ANGLE_RIGHT_LIMIT1 && angle_deg <= ANGLE_RIGHT_LIMIT2)
{
return INPUT_ROCKER_TURN_RIGHT;
}
if (angle_deg >= ANGLE_BACK_LIMIT2 && angle_deg <= ANGLE_BACK_LIMIT1)
{
return INPUT_ROCKER_BACKWARD;
}
return INPUT_ROCKER_STOP;
}
// 急停检测
/**
* @brief 急停检测（最高优先级）
* @param 无
* @return InputEvent 急停事件/无事件
*/
static inline InputEvent CheckEmergencyStop(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
return INPUT_NONE;
if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
{
return EMERGENCE_STOP;
}
return INPUT_NONE;
}
// ============ 各模式事件获取函数 ============
/*-------------------------- 各模式事件处理函数 --------------------------*/
static InputEvent GetHaltModeEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
key = CheckAllKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
key = CheckAllKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetManualModeEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetHorizontalModeEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetFlatModeEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetVerticalLeftModeEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetVerticalRightModeEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetRegionalHorizontalTaskEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
return CalculateRockerEvent();
}
static InputEvent GetRegionalFlatTaskEvents(void)
{
InputEvent key = CheckEmergencyStop();
if(key!=INPUT_NONE) return key;
InputEvent key = CheckEmergencyStop();
if (key != INPUT_NONE) return key;
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
return CalculateRockerEvent();
}
// ============ IO状态获取函数 ============
/**
* @brief IO状态检测（升降控制）
* @param 无
* @return IO_State IO状态枚举
*/
IO_State GetIOState(void)
{
if (P_MK32->CH6_SC == -1000) return IO_STATE_RISE;
@ -233,88 +289,86 @@ IO_State GetIOState(void)
return IO_STATE_NONE;
}
// ============ 参数控制函数 ============
/**
* @brief PV参数更新控制
* @param 无
* @return 无
*/
void PV_control(void)
{
GV.PV = decoded_PV_variable;
GV.Robot_Move_Speed = GV.PV.Robot_Move_Speed / 10;
GV.LaneChangeDistance = GV.PV.Robot_Change_Lane_Distance ; /* 1-100cm */
GV.Vertical_Adjust=((float)GV.PV.Robot_Vertical_Adjust)/10;
GV.PV = decoded_PV_variable;
GV.Robot_Move_Speed = GV.PV.Robot_Move_Speed / 10;
GV.LaneChangeDistance = GV.PV.Robot_Change_Lane_Distance;
GV.Vertical_Adjust = ((float)GV.PV.Robot_Vertical_Adjust) / 10;
}
// ============ 输入变量更新函数 ============
/**
* @brief IV状态参数更新
* @param 无
* @return 无
*/
void IV_control(void)
{
IV.Robot_Move_Deri_Speed = 0;
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
IV.Robot_Gyro = GV.TL720DParameters.RF_Angle_Roll;
IV.Is_Online = GV.P_MK32.IsOnline;
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
IV.Robot_Move_Deri_Speed = GV.Left_Speed_M_min*10;
IV.LeftCompensation = GV.Left_Compensation;
IV.RightCompensation = GV.Right_Compensation;
// IV.Left_Motor_Err = GV.LeftMotor.Motor_Fault;
// IV.Right_Motor_Err = GV.RightMotor.Motor_Fault;
// IV.TimeStamp = SystemTimeMiliCount;
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
IV.Robot_Gyro = GV.TL720DParameters.RF_Angle_Roll;
IV.Is_Online = GV.P_MK32.IsOnline;
IV.Robot_Move_Deri_Speed = GV.Left_Speed_M_min * 10;
IV.LeftCompensation = GV.Left_Compensation;
IV.RightCompensation = GV.Right_Compensation;
}
/* 补偿阈值常量（建议用宏定义，便于维护） */
#define COMP_DEADZONE 300 /* 遥控器生效阈值 */
#define COMP_COUNT_THRESH 200 /* 200*2=400ms累加一个书 */
#define COMP_STEP 10 /* 步长，根据实际需求调整（原为10）*/
#define COMP_LIMIT_MAX 1000 /* 正向角度最大值---10du*/
#define COMP_LIMIT_MIN -1000 /* 步长，根据实际需求调整（原为10）*/
/**
* @brief 单通道补偿更新核心（类型参数与全局变量一致）
* @param chVal 遥控器通道值（int16_t 或 int32_t）
* @param pComp 指向补偿变量的指针（int32_t*）
* @param pCnt 指向计数器的指针（int*）
* @brief 单通道姿态补偿更新
* @param chVal 遥控器通道值
* @param pComp 补偿变量指针
* @param pCnt 补偿计数器指针
* @return 无
*/
static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt)
{
if (chVal > COMP_DEADZONE) {
if (chVal > COMP_DEADZONE)
{
(*pCnt)++;
if (*pCnt > COMP_COUNT_THRESH) {
if (*pCnt > COMP_COUNT_THRESH)
{
*pComp += COMP_STEP;
if (*pComp > COMP_LIMIT_MAX) *pComp = COMP_LIMIT_MAX;
*pComp = (*pComp > COMP_LIMIT_MAX) ? COMP_LIMIT_MAX : *pComp;
*pCnt = 0;
}
}
else if (chVal < -COMP_DEADZONE) {
else if (chVal < -COMP_DEADZONE)
{
(*pCnt)--;
if (*pCnt < -COMP_COUNT_THRESH) {
if (*pCnt < -COMP_COUNT_THRESH)
{
*pComp -= COMP_STEP;
if (*pComp < COMP_LIMIT_MIN) *pComp = COMP_LIMIT_MIN;
*pComp = (*pComp < COMP_LIMIT_MIN) ? COMP_LIMIT_MIN : *pComp;
*pCnt = 0;
}
}
else {
else
{
*pCnt = 0;
}
}
void Compensation_Update()
/**
* @brief 左右轮姿态补偿总更新函数
* @param 无
* @return 无
*/
void Compensation_Update(void)
{
static int leftCnt = 0; /* 左通道计数器 */
static int rightCnt = 0; /* 右通道计数器 */
static int leftCnt = 0;
static int rightCnt = 0;
static float Left_Compensation_Degre_100 = 0;
static float Right_Compensation_Degre_100 = 0;
GV.Left_Compensation = Left_Compensation_Degre_100 / 100.0f;
GV.Right_Compensation = Right_Compensation_Degre_100 / 100.0f;
Update_Single_Compensation(P_MK32->CH14_LT, &GV.Left_Compensation, &leftCnt);
Update_Single_Compensation(P_MK32->CH15_RT, &GV.Right_Compensation, &rightCnt);
}

10
Core/FSM/Src/fsm_state_control.c
View File

@ -150,13 +150,13 @@ static void manual_right_group(void)
static void horizontal_forward_group(void)
{
GV.Robot_Desired_Speed=GV.Robot_Move_Speed;
//horizontal_forward();
horizontal_forward();
/* 若需喷枪控制，可在此添加 */
}
static void horizontal_backward_group(void)
{
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
//horizontal_forward();
horizontal_forward();
/* 若需喷枪控制，可在此添加 */
}
static void horizontal_auto_group(void)
@ -167,7 +167,7 @@ static void horizontal_auto_group(void)
}
static void change_Road_group(void)
{
//Change_Road_Func();
Change_Road_Func();
/* 若需喷枪控制，可在此添加 */
}
@ -175,13 +175,13 @@ static void change_Road_group(void)
static void vertical_forward_group(void)
{
GV.Robot_Desired_Speed=GV.Robot_Move_Speed;
//vertical_forward();
vertical_forward();
}
static void vertical_backward_group(void)
{
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
//vertical_forward();
vertical_forward();
}
static void Emergency_Stop_group(void)

2
Core/FSM/Src/robot_move_actions.c
View File

@ -64,7 +64,7 @@ enum lane_change_auto_state {
};
/**
* 换道自动执行状态枚举
* 摆臂状态枚举
*/
enum swing_state {
SWING_START_STATE = 0, /* 起始状态 */

Write Preview
Loading…
Cancel
Save