LiZongdi
/
BingooRobot_pro
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
仓库提交练习
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
5 Commits
1 Branch
0 Tags
2.6 MiB
 
 
 
Tree: e73e756c37
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'e73e756c37'
${ noResults }
BingooRobot_pro/Bingoo/base/robot_move_actions.c

1074 lines
31 KiB
Raw Blame History

/*
* robot_move_actions.c
*
* Created on: 2025年7月14日
* Author: akeguo
*
* @note 本文件包含机器人水平/垂直方向自动移动的状态机实现，
* 以及换道状态机。所有状态机均通过函数指针表驱动。
*/
/*=========================== 1.头文件包含 ===========================*/
#include "robot_move_actions.h"
#include "swing_action.h"
#include "fsm_state.h"
#include "fsm_state_control.h"
#include "paint_gun_action.h"
#include "MSP/msp_PID.h"
#include "bsp_slide_averager.h"
#include <math.h>
/*=========================== 2.宏定义 ===========================*/
/* 姿态控制常量（用于 Robot_Posture_Adjus_Gravity） */
#define TT_One_Deg_Count2 11014
#define POSTURE_ERROR_THRESHOLD 10.0f /* 姿态调整误差阈值（度） */
#define ANGLE_NORMALIZE_BOUNDARY 180.0f /* 角度归一化边界 */
#define SPEED_FACTOR_LARGE 1.0f /* 大误差速度系数 */
#define SPEED_FACTOR_SMALL 0.15f /* 小误差速度系数 */
#define SPEED_BASE 4.0f /* 电机速度基准 (m/min) */
#define K_OF_DIS_VER 1.011189f /* 最小二乘拟合的k */
#define B_OF_DIS_VER 0.387616f /* 最小二乘拟合的b */
#define K_OF_DIS_HOR 1.013714f /* 最小二乘拟合的k */
#define B_OF_DIS_HOR 3.226667f /* 最小二乘拟合的b */
/* 换道相关常量 */
#define LANE_CHANGE_SPEED 3.0f /* 换道后退速度 (单位需与PID匹配) */
/* 状态机跳转阈值 */
static const float ANGLE_ERROR_THRESHOLD = 0.5f; /* 姿态到位误差阈值 */
/*=========================== 3.类型定义（枚举） ===========================*/
/**
* 水平单道自动运行状态枚举
*/
enum single_lane_auto_state {
STATE_ATTITUDE_JUDGE = 0, /* 姿态误差判断 */
STATE_ATTITUDE_ADJUST, /* 姿态调整 */
STATE_WORK_WAY_OR_DIRECT_MOVE, /* 工作模式选择 */
STATE_FIGHT_ALTERNATELY, /* 交替后退 */
STATE_FIGHT_RETREATING, /* 连续后退 */
STATE_HALT, /* 停机状态 */
STATE_COUNT /* 状态总数 */
};
/**
* 打退交替状态枚举
*/
enum alternately_state{
STATE_ROBOT_STOP = 0, /* 打的时候机器人静止 */
STATE_ROBOT_BACK, /* 打到最后5°机器人后退 */
};
/**
* 换道自动执行状态枚举
*/
enum lane_change_auto_state {
LANE_CHANGE_STATE_ATTITUDE_JUDGE = 0, /* 姿态误差判断 */
LANE_CHANGE_STATE_ATTITUDE_ADJUST, /* 姿态调整 */
LANE_CHANGE_STATE_CALC_RETREAT_TIME, /* 计算后退时间 */
LANE_CHANGE_STATE_CONTINUOUS_RETREAT, /* 连续后退 */
LANE_CHANGE_STATE_RETREAT_DONE_SWITCH, /* 后退完成，切换期望姿态 */
LANE_CHANGE_STATE_COUNT /* 状态总数 */
};
/**
* 摆臂状态枚举
*/
enum swing_state {
SWING_START_STATE = 0, /* 起始状态 */
SWING_LEFT_MOVE, /* 向左运动姿态调整 */
SWING_RIGHT_MOVE, /* 向右运动 */
};
/**
* 焊缝状态枚举
*/
enum weld_auto_state {
//STATE_ATTITUDE_JUDGE = 0, /* 姿态误差判断 */
WELD_STATE_AVG_INT = 0, /* 初始化滑动均值 */
WELD_STATE_FIGHT_RETREATING , /* 连续后退 */
WELD_STATE_HALT, /* 停机状态 */
WELD_STATE_COUNT /* 状态总数 */
};
/**
* 状态机类型枚举（用于多状态机管理）
*/
typedef enum {
FSM_init_state = 0, // 默认初始化的值（占位，不使用）
FSM_HORIZONTAL_AUTO_Flag, // 水平单道自动
FSM_HORIZONTAL_FORWARD_Flag, // 水平前进（简化版）
FSM_VERTICAL_FORWARD_Flag, // 垂直前进
FSM_VERTICAL_CHANGE_ROAD_Flag, // 垂直左换道
FSM_VERTICAL_AUTO_Flag, // 垂直单道自动
FSM_SWING, //摆臂
REGION_STATE_AUTO_CONTINUOUS_Flag, //区域自动作业
WELD_AUTO_Flag, //区域自动作业
FSM_COUNT // 状态机总数
} FsmType;
/*=========================== 4.全局变量 ===========================*/
////局部静态变量
/* 状态机当前状态（多个状态机共用，可能存在潜在冲突，但是现在还不知道咋搞呢） */
static FsmType s_activeFsm;
static FsmType s_lastActiveFsm=FSM_init_state;
static int s_fsmState[FSM_COUNT] = {0};
int s_currentState = STATE_ATTITUDE_JUDGE;
int horizontal_s_currentState = STATE_ATTITUDE_JUDGE;
int vertical_s_currentState = STATE_ATTITUDE_JUDGE;
int ChangeRoad_currentState = LANE_CHANGE_STATE_ATTITUDE_JUDGE;
int swing_currentState = SWING_START_STATE;
int alternately_flag = STATE_ROBOT_STOP;
/* 换道计时变量 */
static uint32_t s_retreatStartTick = 0; /* 后退起始系统节拍 */
static uint32_t s_retreatDurationTicks = 0; /* 后退持续节拍数 */
/* 期望角度数组（上下左右换道） */
static float LEFT_TARGET_ANGLES[2];
int Change_stop_flag=0;
static int S1_Last_Value;
static int S2_Last_Value;
float center_angle=0;
int offset_angle;
int center_position; //中间位置-944334
int present_angle; //摆臂电机当前角度
int alternately_times=0; //后退时间
int alternately_times_total=0; //后退总时间
//滑动均值所用变量
float avg_buffer[5]; // 窗口缓冲区
uint8_t avg_index; // 当前索引
uint8_t avg_count; // 有效计数
int32_t weld_cnt=0;
/*===========================5. 静态函数声明 ===========================*/
/* 水平/垂直状态机函数 */
static void handleAttitudeJudge_horizontal(void);
static void handleAttitudeJudge_vertical(void);
static void handleAttitudeAdjustHorizontal(void);
static void handleAttitudeAdjustVertical(void);
static void handleAttitudeAdjustChangeRoad(void);
static void handleWorkWay(void);
void horizontal_work(void);
void Move_Horizontal_Auto_Sub_Func(void);
static void auto_drive_pid_weld(void);
void avg_init(void);
/* 换道状态机函数 */
void get_ChangeAngle(void);
static void handleLaneChangeAttitudeJudgeLeft(void);
static void handleLaneChangeCalcRetreatTime(void);
static void handleLaneChangeContinuousRetreat(void);
/* 打退连续函数 */
void Fight_Countinus_Function_Horizontal();
void Fight_Countinus_Function_Vertical();
void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(void);
static void Move_Weld_Task_Backwards_Do_Backward(void);
void Fight_Countinus_Function_Weld();
void Update_Angle_compensation_hor(void);
void Update_Angle_compensation_ver(void);
void Auto_Forward_Function_Horizontal_group(void);
void Auto_Forward_Function_Vertical_group(void);
void Fight_Alternately_Function_Horizontal(void);
/* 打退交替函数 */
void alternately_stop_func(void);
void alternately_back_func(void);
/* 辅助函数 */
static void auto_drive_pid_horizontal(void);
static void auto_drive_pid_vertical(void);
static void handleHalt(void);
/* 摆臂运动函数 */
void swing_work(void);
static void swing_start_move(void);
static void swing_left_move(void);
static void swing_right_move(void);
static void get_swing_mode(void);
void updata_swing_angle(void);
/* 急停函数 */
void Emergency_Stop_Action(void);
/* PID系数 */
int factor_1= 1;
int factor_2= -1;
/*===========================6.函数指针表 ===========================*/
/**
* 水平单道自动运行状态函数表
* 注意：STATE_FIGHT_ALTERNATELY 和 STATE_FIGHT_RETREATING 暂未实现，用 NULL 占位
*/
static const void (*const horizontal_single_lane_auto_operation[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = handleWorkWay,
[STATE_FIGHT_ALTERNATELY] = Fight_Alternately_Function_Horizontal, /* 打退交替 */
[STATE_FIGHT_RETREATING] = Fight_Countinus_Function_Horizontal, /*打退连续 */
[STATE_HALT] = handleHalt,
};
/**
* 打退交替状态函数表
* 注意：STATE_FIGHT_ALTERNATELY 和 STATE_FIGHT_RETREATING 暂未实现，用 NULL 占位
*/
static const void (*const alternately_work[])(void) = {
[STATE_ROBOT_STOP] = alternately_stop_func,
[STATE_ROBOT_BACK] = alternately_back_func,
[STATE_HALT] = handleHalt,
};
/**
* 水平前进简化状态表（用于 horizontal_forward）
*/
static const void (*const robot_horizontal_drive[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = Auto_Forward_Function_Horizontal_group,
[STATE_HALT] = handleHalt,
};
/**
* 垂直前进简化状态表（用于 vertical_forward）
*/
static const void (*const robot_vertical_drive[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_vertical,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustVertical,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = Auto_Forward_Function_Vertical_group,
[STATE_HALT] = handleHalt,
};
/**
* 垂直单道自动运行状态函数表
* 注意：STATE_FIGHT_ALTERNATELY 和 STATE_FIGHT_RETREATING 暂未实现，用 NULL 占位
*/
static const void (*const vertical_single_lane_auto_operation[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_vertical,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = handleWorkWay,
[STATE_FIGHT_ALTERNATELY] = NULL, /* 待实现 */
[STATE_FIGHT_RETREATING] = Fight_Countinus_Function_Vertical, /* 待实现 */
[STATE_HALT] = handleHalt,
};
/**
* 换道状态函数表
*/
static const void (*const vertical_lane_change_left[])(void) = {
[LANE_CHANGE_STATE_ATTITUDE_JUDGE] = handleLaneChangeAttitudeJudgeLeft,
[LANE_CHANGE_STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustChangeRoad,
[LANE_CHANGE_STATE_CALC_RETREAT_TIME] = handleLaneChangeCalcRetreatTime,
[LANE_CHANGE_STATE_CONTINUOUS_RETREAT] = handleLaneChangeContinuousRetreat,
[LANE_CHANGE_STATE_RETREAT_DONE_SWITCH] = handleAttitudeAdjustChangeRoad,
[STATE_HALT] = handleHalt,
};
/**
* 摆臂运动函数表
*/
static const void (*const swing_move_control[])(void) = {
[SWING_START_STATE] = swing_start_move,
[SWING_LEFT_MOVE] = swing_left_move,
[SWING_RIGHT_MOVE] = swing_right_move,
[STATE_HALT] = handleHalt,
};
/**
* 焊缝跟踪运动函数表
*/
static const void (*const weld_auto_operation[])(void) = {
[WELD_STATE_AVG_INT] = avg_init,
[WELD_STATE_FIGHT_RETREATING] = Fight_Countinus_Function_Weld,
[WELD_STATE_HALT] = handleHalt,
};
/*=========================== 7.静态函数实现 ===========================*/
/*-----------------------------------------------------------------
* 状态机切换管理
*-----------------------------------------------------------------*/
static void check_and_reset_fsm(FsmType current) {
if (s_lastActiveFsm != current) {
// 发生了状态机切换，重置其他所有状态机
for (int i = 0; i < FSM_COUNT; i++) {
if (i != current) {
s_fsmState[i] = 0; // 重置为初始状态
}
}
s_lastActiveFsm = current; // 更新上次类型
}
// 如果相同，不做任何操作
}
/*-----------------------------------------------------------------
* 基础运动控制（手动模式）
*-----------------------------------------------------------------*/
void Manually_Forward(void)
{
GV.Left_Speed_M_min = GV.Robot_Move_Speed;
GV.Right_Speed_M_min = -GV.Robot_Move_Speed;
}
void Manually_Backward(void)
{
GV.Left_Speed_M_min = -GV.Robot_Move_Speed;
GV.Right_Speed_M_min = GV.Robot_Move_Speed;
}
void Turn_Left(void)
{
GV.Left_Speed_M_min = -GV.Robot_Move_Speed/8;
GV.Right_Speed_M_min = -GV.Robot_Move_Speed/8;
}
void Turn_Right(void)
{
GV.Left_Speed_M_min = GV.Robot_Move_Speed/8;
GV.Right_Speed_M_min = GV.Robot_Move_Speed/8;
}
void Robot_Stop(void)
{
check_and_reset_fsm(FSM_init_state);
swing_currentState = 0;
alternately_times =0;
alternately_flag=0;
get_swing_mode(); //仅在机器人停的时候允许更新摆臂模式
Robot_Swing_Operation_Function(); //机器人停的时候允许摆臂
GV.Left_Speed_M_min = 0;
GV.Right_Speed_M_min = 0;
GV.turn_center_difference=0;
Change_stop_flag=1;
PaintGun_Contronl(); //机器人停的时候允许动推杆
//GV.GroundManagementValue.MaualControlPower=0; //恢复急停断电
}
/*-----------------------------------------------------------------
* 姿态调整辅助函数（重力方向）
*-----------------------------------------------------------------*/
void Robot_Posture_Adjus_Gravity(float *pError)
{
float actualAngle = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
float error = GV.Robot_Angle_Desire - actualAngle;
/* 归一化到 [-180, 180) */
if (error >= ANGLE_NORMALIZE_BOUNDARY) {
error -= 2.0f * ANGLE_NORMALIZE_BOUNDARY;
} else if (error < -ANGLE_NORMALIZE_BOUNDARY) {
error += 2.0f * ANGLE_NORMALIZE_BOUNDARY;
}
float absError = fabsf(error);
*pError = absError;
float speedFactor = (absError > POSTURE_ERROR_THRESHOLD) ? SPEED_FACTOR_LARGE : SPEED_FACTOR_SMALL;
float direction = (error >= 0.0f) ? 1.0f : -1.0f;
float motorSpeed = (float)(speedFactor * direction * SPEED_BASE);
GV.Left_Speed_M_min = motorSpeed;
GV.Right_Speed_M_min = motorSpeed;
}
static void handleAttitudeAdjustHorizontal(void)
{
float errorAbs;
Robot_Posture_Adjus_Gravity(&errorAbs);
if (errorAbs < ANGLE_ERROR_THRESHOLD) {
s_fsmState[s_activeFsm]++;
}
}
static void handleAttitudeAdjustVertical(void)
{
float errorAbs;
Robot_Posture_Adjus_Gravity(&errorAbs);
if (errorAbs < ANGLE_ERROR_THRESHOLD) {
s_fsmState[s_activeFsm]++;
}
}
static void handleAttitudeAdjustChangeRoad(void)
{
float errorAbs;
Robot_Posture_Adjus_Gravity(&errorAbs);
if (errorAbs < ANGLE_ERROR_THRESHOLD) {
s_fsmState[s_activeFsm]++;
}
}
/*-----------------------------------------------------------------
* 水平/垂直/焊缝状态机核心函数
*-----------------------------------------------------------------*/
static void handleAttitudeJudge_horizontal(void)
{
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
if (robotRoll > -180.0f && robotRoll < 0.0f) {
GV.Robot_Angle_Desire = -90.0f + (float)GV.Right_Compensation / 100.0f;
} else {
GV.Robot_Angle_Desire = 90.0f + (float)GV.Left_Compensation / 100.0f;
}
s_fsmState[s_activeFsm]=STATE_ATTITUDE_ADJUST;
}
static void handleAttitudeJudge_vertical(void)
{
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
if (robotRoll > -90.0f && robotRoll <= 90.0f) {
GV.Robot_Angle_Desire = -GV.Vertical_Adjust;
}
// else
// {
// GV.Robot_Angle_Desire = 180.0f + GV.Vertical_Adjust;
// if (GV.Robot_Angle_Desire > 180.0f) {
// GV.Robot_Angle_Desire -= 360.0f;
// }
// }
s_fsmState[s_activeFsm] = STATE_ATTITUDE_ADJUST;
}
//打退交替状态下机器人停止状态
void alternately_stop_func(void)
{
GV.Left_Speed_M_min = 0;
GV.Right_Speed_M_min = 0;
if((present_angle<left_compare_value+5)||(present_angle>right_compare_value-5)) //进入最后5°范围
{
alternately_flag=STATE_ROBOT_BACK;
}
}
//打退交替状态下机器人后退状态
void alternately_back_func(void)
{
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
alternately_times_total=-300*GV.robot_back_distance/GV.Robot_Desired_Speed; //计算出后退总时间
if(alternately_times<=alternately_times_total)
{
auto_drive_pid_horizontal();
alternately_times++;
}
else
{
alternately_times=0;
alternately_flag=STATE_ROBOT_STOP;
}
}
static void handleWorkWay(void)
{
// int mode = 1;//GV.Robot_backMode;
// if (mode == 1) {
// s_fsmState[s_activeFsm]= STATE_FIGHT_ALTERNATELY;
// } else if (mode == 2) {
// s_fsmState[s_activeFsm]= STATE_FIGHT_RETREATING;
// }
/* 其他模式保持当前状态？目前没有 else，可能默认留在该状态 */
s_fsmState[s_activeFsm]= STATE_FIGHT_RETREATING;
}
/*-----------------------------------------------------------------
* 换道状态机函数
*-----------------------------------------------------------------*/
static void handleLaneChangeAttitudeJudgeLeft(void)
{
if(Change_stop_flag==1)
{
Change_stop_flag=0;
get_ChangeAngle();
GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[0];
s_fsmState[s_activeFsm] = LANE_CHANGE_STATE_ATTITUDE_ADJUST;
}
}
/*-----------------------------------------------------------------
* 确定换道状态函数
*-----------------------------------------------------------------*/
void get_ChangeAngle(void)
{
if(GV.PV.Robot_Operation_Mode==2) //需要水平下换道
{
if(GV.TL720DParameters.RF_Angle_Roll<0)
{
LEFT_TARGET_ANGLES[0]=0;
LEFT_TARGET_ANGLES[1]=90;
}
else if(GV.TL720DParameters.RF_Angle_Roll>0)
{
LEFT_TARGET_ANGLES[0]=0;
LEFT_TARGET_ANGLES[1]=-90;
}
}
else if(GV.PV.Robot_Operation_Mode==4) //需要竖直左换道
{
LEFT_TARGET_ANGLES[0]=90;
LEFT_TARGET_ANGLES[1]=0;
}
else if(GV.PV.Robot_Operation_Mode==5) //需要竖直右换道
{
LEFT_TARGET_ANGLES[0]=-90;
LEFT_TARGET_ANGLES[1]=0;
}
}
static void handleLaneChangeCalcRetreatTime(void)
{
float distanceM;
if(GV.PV.Robot_Operation_Mode==2)//水平
{
distanceM = ((float)GV.LaneChangeDistance-B_OF_DIS_HOR)/K_OF_DIS_HOR; /* 厘米 → 米 */
distanceM=distanceM * 0.01f;
}
else if(GV.PV.Robot_Operation_Mode==4||GV.PV.Robot_Operation_Mode==5) //竖直
{
distanceM = ((float)GV.LaneChangeDistance-B_OF_DIS_VER)/K_OF_DIS_VER; /* 厘米 → 米 */
distanceM=distanceM * 0.01f;
}
float timeSec = distanceM / LANE_CHANGE_SPEED; /* 秒 */
float timeMs = timeSec * 60.0f * 1000.0f; /* 毫秒 */
s_retreatDurationTicks = (uint32_t)timeMs;
uint32_t tickNow = HAL_GetTick();
s_retreatStartTick = tickNow;
s_fsmState[s_activeFsm] = LANE_CHANGE_STATE_CONTINUOUS_RETREAT;
if(P_MK32->CH4_SA == -1000&&Change_stop_flag==1)
{
s_fsmState[s_activeFsm] = LANE_CHANGE_STATE_ATTITUDE_JUDGE;
}
}
static void handleLaneChangeContinuousRetreat(void)
{
uint32_t tickNow = HAL_GetTick();
GV.Robot_Desired_Speed = -LANE_CHANGE_SPEED;
auto_drive_pid_vertical();
if ((tickNow - s_retreatStartTick) >= s_retreatDurationTicks) {
s_fsmState[s_activeFsm]++;
GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[1];
}
}
/*-----------------------------------------------------------------
* 打退连续函数
*-----------------------------------------------------------------*/
void Fight_Countinus_Function_Manual()
{
swing_work();
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
GV.Left_Speed_M_min = GV.Robot_Desired_Speed;
GV.Right_Speed_M_min = -GV.Robot_Desired_Speed;
}
void Fight_Countinus_Function_Horizontal()
{
Update_Angle_compensation_hor();
swing_work();
Move_Horizontal_Vertical_Task_Backwards_Do_Backward();
}
void Fight_Countinus_Function_Vertical()
{
Update_Angle_compensation_ver();
swing_work();
Move_Horizontal_Vertical_Task_Backwards_Do_Backward();
}
void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(void)
{
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
auto_drive_pid_horizontal();
}
void Fight_Countinus_Function_Weld()
{
updata_swing_angle();
swing_work();
auto_drive_pid_weld();
}
void Update_Angle_compensation_hor(void)
{
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
if (robotRoll > -180.0f && robotRoll < 0.0f) {
GV.Robot_Angle_Desire = -90.0f + (float)GV.Right_Compensation / 100.0f;
} else {
GV.Robot_Angle_Desire = 90.0f + (float)GV.Left_Compensation / 100.0f;
}
}
void Update_Angle_compensation_ver(void)
{
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
if (robotRoll > -90.0f && robotRoll <= 90.0f) {
GV.Robot_Angle_Desire = -GV.Vertical_Adjust;
}
// else
// {
// GV.Robot_Angle_Desire = 180.0f + GV.Vertical_Adjust;
// if (GV.Robot_Angle_Desire > 180.0f) {
// GV.Robot_Angle_Desire -= 360.0f;
// }
// }
}
void Auto_Forward_Function_Horizontal_group(void)
{
Update_Angle_compensation_hor();
auto_drive_pid_horizontal();
}
void Auto_Forward_Function_Vertical_group(void)
{
Update_Angle_compensation_ver();
auto_drive_pid_vertical();
}
/*-----------------------------------------------------------------
*打一道退一道函数
*-----------------------------------------------------------------*/
void Fight_Alternately_Function_Horizontal(void)
{
swing_work();
if (alternately_flag >= 0 && alternately_flag < STATE_COUNT) {
if (alternately_work[alternately_flag] != NULL) {
alternately_work[alternately_flag]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
/*-----------------------------------------------------------------
* 摆臂运动函数
*-----------------------------------------------------------------*/
float first_center;
static void swing_start_move(void)
{
left_compare_updata=left_compare_value;
right_compare_updata=right_compare_value;
first_center=center_angle;
present_angle=-(GV.SwingMotor.Real_Position+944334)/11014;
if(present_angle>=left_compare_value+2)
{
swing_currentState=SWING_LEFT_MOVE;
Move_Swing_Left_Func_Do_imm();
}
else
{
swing_currentState=SWING_RIGHT_MOVE;
Move_Swing_Right_Func_Do_imm();
}
}
static void swing_left_move(void)
{
present_angle=-(GV.SwingMotor.Real_Position+944334)/11014;
if(present_angle<=left_compare_value+1)
{
//焊缝跟踪时允许改变摆臂中心位置，若检测到中心位置变化，则更新偏距
if(right_compare_updata!=right_compare_value)
{
GV.turn_center_difference=center_angle-first_center;
right_compare_value=right_compare_updata;
center_angle=(right_compare_updata+left_compare_updata)/2;
}
Move_Swing_Right_Func_Do_lay();
swing_currentState=SWING_RIGHT_MOVE;
}
}
static void swing_right_move(void)
{
present_angle=-(GV.SwingMotor.Real_Position+944334)/11014;
if(present_angle>right_compare_value-1)
{
//焊缝跟踪时允许改变摆臂中心位置，若检测到中心位置变化，则更新偏距
if(left_compare_updata!=left_compare_value)
{
GV.turn_center_difference=center_angle-first_center;
left_compare_value=left_compare_updata;
center_angle=(right_compare_updata+left_compare_updata)/2;
}
Move_Swing_Left_Func_Do_lay();
swing_currentState=SWING_LEFT_MOVE;
}
}
static void get_swing_mode(void)
{
if(GV.symmetricalOrNot==1)
{
if(GV.PV.Robot_Swing_Range_Angle!=offset_angle && GV.SwingMotor.Real_Position!=0)
{
offset_angle=GV.PV.Robot_Swing_Range_Angle;
center_angle=-((float)GV.SwingMotor.Real_Position+944334)/TT_One_Deg_Count2;
}
left_compare_value=center_angle-offset_angle/2;
right_compare_value=center_angle+offset_angle/2;
}
else if(GV.symmetricalOrNot==2) //非对称式
{
if(GV.SwingMotor.Real_Position!=0)
{
if(P_MK32->CH12_S1!=S1_Last_Value)
{
left_compare_value=-(GV.SwingMotor.Real_Position+944334)/11014;
}
else if(P_MK32->CH13_S2!=S2_Last_Value)
{
right_compare_value=-(GV.SwingMotor.Real_Position+944334)/11014;
}
}
S1_Last_Value=P_MK32->CH12_S1;
S2_Last_Value=P_MK32->CH13_S2;
}
}
int32_t updata_cnt=0;
void updata_swing_angle(void) //焊缝跟踪模式下允许更新摆臂中心位置
{
if(P_MK32->CH0_RY_H<-300)
{
updata_cnt++;
if(updata_cnt>200)
{
left_compare_updata=left_compare_value-2;
right_compare_updata=right_compare_value-2;
updata_cnt=0;
}
}
else if(P_MK32->CH0_RY_H>300)
{
updata_cnt++;
if(updata_cnt>200)
{
left_compare_updata=left_compare_value+2;
right_compare_updata=right_compare_value+2;
updata_cnt=0;
}
}
}
/*-----------------------------------------------------------------
* PID驱动及停机处理
*-----------------------------------------------------------------*/
static void auto_drive_pid_horizontal(void)
{
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
float speeds[2];
TwoWheel_AngleControl(robotRoll, GV.Robot_Angle_Desire, GV.Robot_Desired_Speed,1, speeds);
GV.Left_Speed_M_min = factor_1*speeds[0];
GV.Right_Speed_M_min = factor_2*speeds[1]; /* 根据电机方向可能需要调整符号 */
}
void avg_init(void) //初始化滑动均值所需要的参数
{
slide_averager_init(avg_buffer, &avg_index, &avg_count);
weld_cnt=0;
s_fsmState[s_activeFsm] = WELD_STATE_FIGHT_RETREATING;
}
float avg;
static void auto_drive_pid_weld(void)
{
float robotRoll = (float)(ReadLazorData->Feature_X);
avg = slide_averager_calc(avg_buffer, &avg_index, &avg_count, -robotRoll);
float speeds[2];
TwoWheel_AngleControl_Weld(avg, 0, GV.Robot_Desired_Speed,0.5, speeds);
GV.Left_Speed_M_min = factor_1*speeds[0];
GV.Right_Speed_M_min = factor_2*speeds[1]; /* 根据电机方向可能需要调整符号 */
//未检测到则焊缝速度为0
if((*Weld_Out_Flag)==16)
{
weld_cnt++;
if(weld_cnt>1500)
{
s_fsmState[s_activeFsm] = WELD_STATE_HALT;
GV.Left_Speed_M_min = 0;
GV.Right_Speed_M_min = 0;
GV.Tar_Position_Velcity_Degree_S=0;
}
}
else
{
weld_cnt=0;
}
}
float PID_factor=1;
static void auto_drive_pid_vertical(void)
{
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
float speeds[2];
TwoWheel_AngleControl(robotRoll, GV.Robot_Angle_Desire, GV.Robot_Desired_Speed,PID_factor, speeds);
GV.Left_Speed_M_min = factor_1*speeds[0];
GV.Right_Speed_M_min = factor_2*speeds[1]; /* 根据电机方向可能需要调整符号 */
}
static void Move_Weld_Task_Backwards_Do_Backward(void)
{
double Deri_Speed_Robot_MAX = GV.Robot_Move_Speed * 1.5;
double Robot_Speed[4] = {0, 0, 0, 0};
double robotDeriAngleDegGrity=0;
//GV.weld_data_X = ReadLazorData->Feature_X;
float Robot_Weld_Real = GV.weld_data_X;
GF_MSP_PID_Now_Der_adj_Com_Weld(Robot_Weld_Real, robotDeriAngleDegGrity, GV.Robot_Move_Speed, Deri_Speed_Robot_MAX, 0.1, Robot_Speed);
GV.Left_Speed_M_min = Robot_Speed[1];
GV.Right_Speed_M_min = Robot_Speed[0];
}
static void handleHalt(void)
{
GV.Left_Speed_M_min = 0;
GV.Right_Speed_M_min = 0;
GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Target_Velcity=0;
horizontal_s_currentState = 0;
vertical_s_currentState = 0;
ChangeRoad_currentState = 0;
s_currentState = 0;
}
/*=========================== 8.对外接口函数（需外部调用） ===========================*/
void Move_Manual_Auto_Sub_Func(void)
{
Fight_Countinus_Function_Manual();
}
void Move_Horizontal_Auto_Sub_Func(void)
{
s_activeFsm = FSM_HORIZONTAL_AUTO_Flag;
check_and_reset_fsm(FSM_HORIZONTAL_AUTO_Flag);
int state = s_fsmState[FSM_HORIZONTAL_AUTO_Flag];
if (state >= 0 && state < STATE_COUNT) {
if (horizontal_single_lane_auto_operation[state] != NULL) {
horizontal_single_lane_auto_operation[state]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
void Move_Vertical_Auto_Sub_Func(void)
{
s_activeFsm = FSM_VERTICAL_AUTO_Flag;
check_and_reset_fsm(FSM_VERTICAL_AUTO_Flag);
int state = s_fsmState[FSM_VERTICAL_AUTO_Flag];
if (state >= 0 && state < STATE_COUNT) {
if (vertical_single_lane_auto_operation[state] != NULL) {
vertical_single_lane_auto_operation[state]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
void Weld_Auto_Sub_Func(void)
{
s_activeFsm = WELD_AUTO_Flag;
check_and_reset_fsm(WELD_AUTO_Flag);
int state = s_fsmState[WELD_AUTO_Flag];
if (state >= 0 && state < STATE_COUNT) {
if (weld_auto_operation[state] != NULL) {
weld_auto_operation[state]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
void Change_Road_Func(void)
{
s_activeFsm = FSM_VERTICAL_CHANGE_ROAD_Flag;
check_and_reset_fsm(FSM_VERTICAL_CHANGE_ROAD_Flag);
int state = s_fsmState[FSM_VERTICAL_CHANGE_ROAD_Flag];
if (state >= 0 && state < STATE_COUNT) {
if (vertical_lane_change_left[state] != NULL) {
vertical_lane_change_left[state]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
void vertical_forward(void)
{
s_activeFsm = FSM_VERTICAL_FORWARD_Flag;
check_and_reset_fsm(FSM_VERTICAL_FORWARD_Flag);
int state = s_fsmState[FSM_VERTICAL_FORWARD_Flag];
if (state >= 0 && state < STATE_COUNT) {
if (robot_vertical_drive[state] != NULL) {
robot_vertical_drive[state]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
void horizontal_forward(void)
{
s_activeFsm = FSM_HORIZONTAL_AUTO_Flag;
check_and_reset_fsm(FSM_HORIZONTAL_AUTO_Flag);
int state = s_fsmState[FSM_HORIZONTAL_AUTO_Flag];
if (state >= 0 && state < STATE_COUNT) {
if (robot_horizontal_drive[state] != NULL) {
robot_horizontal_drive[state]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
void horizontal_work(void)
{
if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) {
horizontal_single_lane_auto_operation[horizontal_s_currentState]();
} else {
handleHalt();
}
}
void swing_work(void)
{
if (swing_currentState >= 0 && swing_currentState < STATE_COUNT) {
if (swing_move_control[swing_currentState] != NULL) {
swing_move_control[swing_currentState]();
} else {
/* 遇到未实现的状态，停机处理 */
handleHalt();
}
} else {
handleHalt();
}
}
/* 重置标志位，任何标志位都可以放到这里面重置*/
void flag_reset(void)
{
if(P_MK32->CH4_SA==0&&P_MK32->CH5_SB==0&&P_MK32->CH6_SC==0&&P_MK32->CH7_SD==0
&&P_MK32->CH0_RY_H==0&&P_MK32->CH1_RY_V==0&&P_MK32->CH2_LY_V==0
&&P_MK32->CH3_LY_H==0)
{
s_currentState = 0;
horizontal_s_currentState = 0;
vertical_s_currentState = 0;
ChangeRoad_currentState = 0;
swing_currentState = 0;
}
}
void Emergency_Stop_Action(void)
{
GV.GroundManagementValue.MaualControlPower=1;
GV.GroundManagementValue.MaualPowerState=0;
GV.SwingMotor.Position_immediately1_Lag2=1;
GV.Tar_Position_Velcity_Degree_S=0;
GV.Left_Speed_M_min=0;
GV.Right_Speed_M_min=0;
flag_reset();
}