/* * 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_PID.h" #include "bsp_slide_averager.h" #include /*=========================== 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_angleright_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; void Move_Swing_Left_Func_Do_imm(void); Move_Swing_Left_Func_Do_imm(); } else { swing_currentState=SWING_RIGHT_MOVE; void Move_Swing_Right_Func_Do_imm(void); 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; } void Move_Swing_Right_Func_Do_lay(void); 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; } void Move_Swing_Left_Func_Do_lay(void); 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; void GF_MSP_PID_Now_Der_adj_Com_Weld(double Current_Angle, double Desire_Angle_Input, double Auto_Speed, double Auto_Speed_Max, double System_time, double *W_Speed_1); 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(); }