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.
657 lines
16 KiB
657 lines
16 KiB
6 days ago
|
/*
|
||
|
|
* change_line_control.c
|
||
|
|
*
|
||
|
|
* Created on: 2025年8月12日
|
||
|
|
* Author: xsq
|
||
|
|
*/
|
||
|
|
#include "change_line_control.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
|
||
|
|
#define Veritical_To_Left_Flag 1
|
||
|
|
#define Veritical_To_Right_Flag 2
|
||
|
|
|
||
|
|
#define Horizontal_Head_To_Left_Flag 0
|
||
|
|
#define Horizontal_Head_To_Right_Flag 1
|
||
|
|
int LaneChangeWaittime_ms = 0;
|
||
|
|
/*
|
||
|
|
* return: ms
|
||
|
|
*/
|
||
|
|
int32_t RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
int LaneChangeWaittime = 0;
|
||
|
|
|
||
|
|
char LaneChangeFlage = -1;
|
||
|
|
|
||
|
|
Lane_Horizontal_ChangeState CurrentHorizontal_ChangeState; /*当前换道处于哪一步*/
|
||
|
|
Lane_Vertical_ChangeState Current_Vertical_ChangeState;
|
||
|
|
|
||
|
|
LaneChangeControlSTATE HorizontalLaneChangeState, VerticalLaneChangeState; /*当前换道处于开始或者结束*/
|
||
|
|
|
||
|
|
|
||
|
|
/* 手动换道
|
||
|
|
* 换道依据当前车头朝向 */
|
||
|
|
int LaneChangeControl_Rough()
|
||
|
|
{
|
||
|
|
//机器人SA按键处于中间状态
|
||
|
|
if (P_MK32->CH4_SA == 0)
|
||
|
|
{
|
||
|
|
HorizontalLaneChangeState = Lane_Change_Start; //start and stop
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_StateZero; //设定初始方向
|
||
|
|
|
||
|
|
VerticalLaneChangeState = Lane_Change_Start;
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_StateZero;
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) //头朝左
|
||
|
|
<= 45 * 100)
|
||
|
|
{
|
||
|
|
// 头朝左，换道意味着头
|
||
|
|
LaneChangeFlage = Horizontal_Head_To_Right_Flag;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) //头朝右
|
||
|
|
<= 45 * 100)
|
||
|
|
{
|
||
|
|
LaneChangeFlage = Horizontal_Head_To_Left_Flag;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) //头朝上
|
||
|
|
<= 45 * 100)
|
||
|
|
{
|
||
|
|
if (GV.PV.RunMode == Move_Vertical_Move_To_Left)
|
||
|
|
{
|
||
|
|
LaneChangeFlage = Veritical_To_Left_Flag;
|
||
|
|
|
||
|
|
}
|
||
|
|
if (GV.PV.RunMode == Move_Vertical_Move_To_Right)
|
||
|
|
{
|
||
|
|
LaneChangeFlage = Veritical_To_Right_Flag;
|
||
|
|
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
LaneChangeFlage = -1;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (P_MK32->CH4_SA == -1000) /*竖直上or水平换道*/
|
||
|
|
{
|
||
|
|
// 防止无模式下，SB按下后，之后按下SA，SA抢占SB控制权
|
||
|
|
if (GV.PV.RunMode == Move_Manual)
|
||
|
|
{
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
// if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/
|
||
|
|
// {
|
||
|
|
// return 0;
|
||
|
|
// }
|
||
|
|
|
||
|
|
if (GV.PV.RunMode == Move_Horizontal_Move)/*水平换道*/
|
||
|
|
{
|
||
|
|
if (HorizontalLaneChangeState != Lane_Change_Start)
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (LaneChangeFlage == Horizontal_Head_To_Left_Flag) /*需要换到头朝左*/
|
||
|
|
{
|
||
|
|
Horizontal_Lane_Change_Turn_To_Left_Control(); /*水平左换道*/
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
if (LaneChangeFlage == Horizontal_Head_To_Right_Flag)
|
||
|
|
{
|
||
|
|
Horizontal_Lane_Change_Turn_To_Right_Control(); /*水平右换道*/
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
/*************/
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/
|
||
|
|
// {
|
||
|
|
// return 1;
|
||
|
|
// }
|
||
|
|
|
||
|
|
if (VerticalLaneChangeState != Lane_Change_Start)
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (LaneChangeFlage == Veritical_To_Left_Flag) /*向左作业*/
|
||
|
|
{
|
||
|
|
Vertical_Lane_Change_From_Right_To_Left_UP_Control(); /*竖直上换道*/
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
if (LaneChangeFlage == Veritical_To_Right_Flag) /*向右作业*/
|
||
|
|
{
|
||
|
|
Vertical_Lane_Change_From_Left_To_Right_UP_Control(); /*竖直上换道*/
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
return 1;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
if (P_MK32->CH4_SA == 1000) //竖直下换道
|
||
|
|
{
|
||
|
|
|
||
|
|
// 防止无模式下，SB按下后，之后按下SA，SA抢占SB控制权
|
||
|
|
if (GV.PV.RunMode == Move_Manual)
|
||
|
|
{
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
// if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/
|
||
|
|
// {
|
||
|
|
// return 0;
|
||
|
|
// }
|
||
|
|
|
||
|
|
if (GV.PV.RunMode != Move_Vertical_Move_To_Left
|
||
|
|
&& GV.PV.RunMode != Move_Vertical_Move_To_Right)
|
||
|
|
{
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (VerticalLaneChangeState != Lane_Change_Start) /*换道结束了*/
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
if (LaneChangeFlage == Veritical_To_Left_Flag) /*向左作业*/
|
||
|
|
{
|
||
|
|
Vertical_Lane_Change_From_Right_To_Left_Down_Control(); /*竖直下换道*/
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
if (LaneChangeFlage == Veritical_To_Right_Flag) /*向右作业*/
|
||
|
|
{
|
||
|
|
Vertical_Lane_Change_From_Left_To_Right_Down_Control(); /*竖直下换道*/
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* 水平 向下换道 最终头朝右
|
||
|
|
* */
|
||
|
|
void Horizontal_Lane_Change_Turn_To_Right_Control()
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.Robot_Move_Speed = CV.Lane_Change_Speed_m_per_min*10;
|
||
|
|
switch (CurrentHorizontal_ChangeState)
|
||
|
|
{
|
||
|
|
case HorizontalChange_StateZero:
|
||
|
|
{
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_TurnToUP:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_head_to_up_enum_state); /*转到朝上*/
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_vertical_task_backwards_state); /*转到朝左*/
|
||
|
|
//开启计时
|
||
|
|
timer_handler_1.start_timer = 1;
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_DelayMove:
|
||
|
|
{
|
||
|
|
//m/min 100cm/60s 100cm/(60*1000)mm 1cm/600ms
|
||
|
|
LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
||
|
|
{
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToRight;
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_right_enum_state); /*转到朝上*/
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_TurnToRight:
|
||
|
|
{
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_right_enum_state); /*转到朝右*/
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/
|
||
|
|
if (abs(GV.Robot_Angle - (CV.RobotRightAngleValue + GV.PV.Vertical_Calibration))
|
||
|
|
<= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_End;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_End:
|
||
|
|
{
|
||
|
|
HorizontalLaneChangeState = Lane_Change_Stop;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 水平 向下换道 最终头朝左
|
||
|
|
* */
|
||
|
|
void Horizontal_Lane_Change_Turn_To_Left_Control()
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
|
||
|
|
|
||
|
|
switch (CurrentHorizontal_ChangeState)
|
||
|
|
{
|
||
|
|
case HorizontalChange_StateZero:
|
||
|
|
{
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_TurnToUP:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_head_to_up_enum_state); /*转到朝上*/
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_vertical_task_backwards_state); /*后退纠偏*/
|
||
|
|
//开启计时
|
||
|
|
timer_handler_1.start_timer = 1;
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_DelayMove:
|
||
|
|
{
|
||
|
|
LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1)) //计时结束
|
||
|
|
{
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToLeft;
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_left_enum_state); /*转到朝左*/
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_TurnToLeft:
|
||
|
|
{
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_left_enum_state); /*转到朝左*/
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - (CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration))
|
||
|
|
<= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
CurrentHorizontal_ChangeState = HorizontalChange_End;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case HorizontalChange_End:
|
||
|
|
{
|
||
|
|
HorizontalLaneChangeState = Lane_Change_Stop;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* 以下4个：竖直左、竖直右模式下，换道函数
|
||
|
|
* 已改进项：将竖直微调数据同步应用于换道参数配置。
|
||
|
|
*
|
||
|
|
* */
|
||
|
|
|
||
|
|
/*********************************************************************/
|
||
|
|
/* 竖直从左往右作业 上端 向左换道 最终头朝上
|
||
|
|
* */
|
||
|
|
void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
|
||
|
|
{
|
||
|
|
GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
|
||
|
|
switch (Current_Vertical_ChangeState)
|
||
|
|
{
|
||
|
|
case VerticalChange_StateZero:
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToLeft:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_head_to_left_enum_state); /* 移动至头朝左 */
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_horizontal_task_backwards_left_state);/* 水平纠偏后退 头朝左*/
|
||
|
|
|
||
|
|
//开启计时
|
||
|
|
timer_handler_1.start_timer = 1;
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_DelayMove;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_DelayMove:
|
||
|
|
{
|
||
|
|
LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
||
|
|
{
|
||
|
|
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToUP;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToUP:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_End;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_End:
|
||
|
|
{
|
||
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 竖直从左往右作业 下端 向右换道 最终头朝上
|
||
|
|
* */
|
||
|
|
void Vertical_Lane_Change_From_Left_To_Right_Down_Control()
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
|
||
|
|
switch (Current_Vertical_ChangeState)
|
||
|
|
{
|
||
|
|
case VerticalChange_StateZero:
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToRight;
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToRight:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_head_to_right_enum_state); /* 移动至头朝右 */
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_horizontal_task_forwards_right_state);/* 水平纠偏前进 头朝右*/
|
||
|
|
|
||
|
|
|
||
|
|
//开启计时
|
||
|
|
timer_handler_1.start_timer = 1;
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_DelayMove;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_DelayMove:
|
||
|
|
{
|
||
|
|
LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
||
|
|
{
|
||
|
|
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToUP;
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToUP:
|
||
|
|
{
|
||
|
|
// 这里判断条件从原来的 “当前值-9000” 改成了 “当前值-9000-竖直微调值”，否则回正时来回抖动
|
||
|
|
if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
|
||
|
|
{
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_End;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_End:
|
||
|
|
{
|
||
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* 竖直从右往左作业 上端 向左换道 最终头朝上
|
||
|
|
* */
|
||
|
|
void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
|
||
|
|
switch (Current_Vertical_ChangeState)
|
||
|
|
{
|
||
|
|
case VerticalChange_StateZero:
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToRight;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToRight:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_head_to_right_enum_state); /* 移动至头朝右 */
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_horizontal_task_backwards_right_state);/* 水平纠偏后退 头朝右*/
|
||
|
|
|
||
|
|
|
||
|
|
//开启计时
|
||
|
|
timer_handler_1.start_timer = 1;
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_DelayMove;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_DelayMove:
|
||
|
|
{
|
||
|
|
LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToUP;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToUP:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_End;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_End:
|
||
|
|
{
|
||
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 竖直从右往左作业 下端 向左换道 最终头朝上
|
||
|
|
* */
|
||
|
|
void Vertical_Lane_Change_From_Right_To_Left_Down_Control()
|
||
|
|
{
|
||
|
|
GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
|
||
|
|
switch (Current_Vertical_ChangeState)
|
||
|
|
{
|
||
|
|
case VerticalChange_StateZero:
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToLeft:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
{
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_head_to_left_enum_state); /* 移动至头朝左 */
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
fsm_state_set(¤t_robot_move_state,
|
||
|
|
&robot_move_horizontal_task_forwards_left_state);/* 水平纠偏前进 头朝左*/
|
||
|
|
|
||
|
|
|
||
|
|
//开启计时
|
||
|
|
timer_handler_1.start_timer = 1;
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_DelayMove;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_DelayMove:
|
||
|
|
{
|
||
|
|
LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
||
|
|
{
|
||
|
|
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToUP;
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_TurnToUP:
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
|
||
|
|
>= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
|
||
|
|
{
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
|
||
|
|
fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Current_Vertical_ChangeState = VerticalChange_End;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case VerticalChange_End:
|
||
|
|
{
|
||
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
/***********************************************************************************/
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* return: ms
|
||
|
|
*/
|
||
|
|
int32_t RunTime_DistanceCm_SpeedE_2MPMin()
|
||
|
|
{
|
||
|
|
|
||
|
|
if (CurrentHorizontal_ChangeState==HorizontalChange_DelayMove)
|
||
|
|
{
|
||
|
|
|
||
|
|
//VerticalLaneChangeDistanceCalibrationCM(6cm)是下降距离的误差 1min =60*1000ms
|
||
|
|
LaneChangeWaittime_ms = 600
|
||
|
|
* (GV.PV.LaneChangeDistance+ CV.Horizontal_ChangeLane_Compensation)
|
||
|
|
/ CV.Lane_Change_Speed_m_per_min;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
//m/min 5/3 cm/ms
|
||
|
|
|
||
|
|
LaneChangeWaittime_ms = 600 * (GV.PV.LaneChangeDistance+CV.Vertical_ChangeLane_Compensation)
|
||
|
|
/ CV.Lane_Change_Speed_m_per_min;
|
||
|
|
}
|
||
|
|
return LaneChangeWaittime_ms;
|
||
|
|
|
||
|
|
}
|
||
|
|
|