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.
405 lines
10 KiB
405 lines
10 KiB
3 weeks 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
|
||
|
|
|
||
|
|
|
||
|
|
int LaneChangeWaittime_ms = 0;
|
||
|
|
/*
|
||
|
|
* return: ms
|
||
|
|
*/
|
||
|
|
int32_t RunTime_DistanceCm_SpeedE_2MPMin();
|
||
|
|
int LaneChangeWaittime = 0;
|
||
|
|
|
||
|
|
char LaneChangeFlage = -1;
|
||
|
|
|
||
|
|
|
||
|
|
Lane_Vertical_ChangeState Current_Vertical_ChangeState;
|
||
|
|
|
||
|
|
LaneChangeControlSTATE VerticalLaneChangeState; /*当前换道处于开始或者结束*/
|
||
|
|
|
||
|
|
/* 手动换道
|
||
|
|
* 换道依据当前车头朝向
|
||
|
|
// * */
|
||
|
|
//int LaneChangeControl_Paint()
|
||
|
|
//{
|
||
|
|
////机器人SA按键处于中间状态
|
||
|
|
//// if (P_MK32->CH4_SA == 0)
|
||
|
|
//// {
|
||
|
|
//// VerticalLaneChangeState = Lane_Change_Start;
|
||
|
|
//// Current_Vertical_ChangeState = VerticalChange_StateZero;
|
||
|
|
////
|
||
|
|
//// if (abs(GV.Robot_Angle_Desire - 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水平换道*/
|
||
|
|
//// {
|
||
|
|
////
|
||
|
|
//// 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) //竖直下换道
|
||
|
|
//// {
|
||
|
|
//// 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 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_Desire - 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_Desire - CV.RobotUpAngleValue)
|
||
|
|
// >= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
// {
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_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_Desire - 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:
|
||
|
|
// {
|
||
|
|
// if (abs(GV.Robot_Angle_Desire - CV.RobotUpAngleValue)
|
||
|
|
// >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
|
||
|
|
// {
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_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_Desire - 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_Desire - CV.RobotUpAngleValue)
|
||
|
|
// >= CV.Allowable_Error_For_Angle_Tracking)
|
||
|
|
// {
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_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);//转完 前进or后退得速度
|
||
|
|
// switch (Current_Vertical_ChangeState)
|
||
|
|
// {
|
||
|
|
// case VerticalChange_StateZero:
|
||
|
|
// {
|
||
|
|
// Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
|
||
|
|
//
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case VerticalChange_TurnToLeft:
|
||
|
|
// {
|
||
|
|
// if (abs(GV.Robot_Angle_Desire - 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_Desire - CV.RobotUpAngleValue)
|
||
|
|
// >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
|
||
|
|
// {
|
||
|
|
// fsm_state_set(¤t_robot_move_state,
|
||
|
|
// &robot_move_head_to_up_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()
|
||
|
|
//{
|
||
|
|
//
|
||
|
|
//// LaneChangeWaittime_ms = 600 * (GV.PV.LaneChangeDistance+CV.Vertical_ChangeLane_Compensation)
|
||
|
|
//// / CV.Lane_Change_Speed_m_per_min;
|
||
|
|
////
|
||
|
|
//// return LaneChangeWaittime_ms;
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
|