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.
951 lines
22 KiB
951 lines
22 KiB
/*
|
|
* fsm.c
|
|
*
|
|
* Created on: Oct 18, 2024
|
|
* Author: akeguo
|
|
*/
|
|
|
|
#include <bsp_tempature.h>
|
|
#include "fsm.h"
|
|
#include <math.h>
|
|
|
|
#include "BHBF_ROBOT.h"
|
|
#include "BSP/bsp_include.h"
|
|
|
|
#include "MSP/msp_PID.h"
|
|
#include "robot_state.h"
|
|
#include "MSP/msp_MK32_1.h"
|
|
|
|
#define Move_Manual 0
|
|
#define Move_Horizontal_Move_Left 1
|
|
#define Move_Horizontal_Move_Right 2
|
|
#define Move_Vertical_Move 3
|
|
#define Move_Horizontal_Move 1
|
|
#define LeftRightCalbrationTime 500
|
|
|
|
double LaneChangeSpeed = 4;
|
|
IV_Information_State IV_MCU_Run_State; //返回安卓程序的部分
|
|
void IV_control();
|
|
void compensation_control();
|
|
void MoveControl();
|
|
|
|
int32_t GetVehicleSpeed(int speed_selection);
|
|
|
|
int LaneChangeControl();
|
|
void Horizontal_Lane_Change_Turn_To_Left_Control();
|
|
void Horizontal_Lane_Change_Turn_To_Right_Control();
|
|
void Vertical_Lane_Change_Right_Control();
|
|
void Vertical_Lane_Change_Left_Control();
|
|
int LaneChangeWaittime = 0;
|
|
int32_t speed_selection;
|
|
|
|
MoveSTATE_t CurrentMoveState;
|
|
SwingSTATE_t CurrentSwingState;
|
|
TiltSTATE_t CurrentTiltState; //点头，抬头
|
|
|
|
LaneChangeSTATE_t CurrentLaneChangeSTATE;
|
|
|
|
RougheningEndState_t CurrentRougheningEndState; //拉毛运动或者不动
|
|
SetSTATE_t CurrentSetState; //设置
|
|
|
|
Lane_Horizontal_ChangeState CurrentHorizontal_ChangeState;
|
|
Lane_Vertical_ChangeState Current_Vertical_ChangeState;
|
|
|
|
LaneChangeControlSTATE HorizontalLaneChangeState, VerticalLaneChangeState; //水平和竖直换道标志位
|
|
|
|
int index_counter = 0;
|
|
|
|
int LaneChangeControl();
|
|
void DHRougheningControl();
|
|
void TiltControl();
|
|
|
|
//手动下 机器人运动
|
|
transition_t MoveTransitions[] =
|
|
{
|
|
|
|
{ Move_Forwards, Forwards_State_Do },
|
|
{ Move_Backwards, Backwards_State_Do },
|
|
{ Move_TurnLeft, TurnLeft_State_Do },
|
|
{ Move_TurnRight, TurnRight_State_Do },
|
|
{ Move_HALT, HALT_State_Do },
|
|
{ Move_Vertical_Task_Forwards, Move_Vertical_Task_Forwards_Do },
|
|
{ Move_Vertical_Task_Backwards, Move_Vertical_Task_Backwards_Do },
|
|
{ Move_Horizontal_Task_Forwards_Left, Move_Horizontal_Task_Forwards_Left_Do },
|
|
{ Move_Horizontal_Task_Backwards_Left, Move_Horizontal_Task_Backwards_Left_Do },
|
|
{ Move_Horizontal_Task_Forwards_Right, Move_Horizontal_Task_Forwards_Right_Do },
|
|
{ Move_Horizontal_Task_Backwards_Right,
|
|
Move_Horizontal_Task_Backwards_Right_Do },
|
|
{ Move_Head_To_Left_Enum, Move_Head_To_Left_Do },
|
|
{ Move_Head_To_UP_Enum, Move_Head_To_UP_Do },
|
|
{ Move_Head_To_Right_Enum, Move_Head_To_Right_Do },
|
|
{ Move_Head_To_Down_Enum, Move_Head_To_Down_Do },
|
|
|
|
};
|
|
|
|
//Move_Horizontal_Task_Forwards_Left,//水平方向前进，保持水平方向
|
|
//Move_Horizontal_Task_Backwards_Left,//水平方向后退，保持水平方向
|
|
//Move_Horizontal_Task_Forwards_Right,//水平方向前进，保持水平方向
|
|
//Move_Horizontal_Task_Backwards_Right,//水平方向后退，保持水平方向
|
|
|
|
transition_t SetTransitions[] =
|
|
{
|
|
|
|
{ OtherMode, OtherMode_State_Do },
|
|
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Do },
|
|
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Do },
|
|
|
|
};
|
|
|
|
transition_t TiltTransitions[] =
|
|
{
|
|
{ TiltHALT, TiltHalt_Do },
|
|
{ TiltUP, TiltUp_Do },
|
|
{ TiltDown, TiltDown_Do },
|
|
|
|
};
|
|
|
|
transition_t RougheningEndTransitions[] =
|
|
{
|
|
{ RougheningEndHALT, RougheningEndHALT_Do },
|
|
{ RougheningEndSwingClockWise, RougheningEndSwingClockWise_Do },
|
|
{ RougheningEndSwingAntiClockWise, RougheningEndSwingAntiClockWise_Do }, };
|
|
|
|
void Fsm_Init()
|
|
{
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
|
Current_Vertical_ChangeState = VerticalChange_StateZero;
|
|
HorizontalLaneChangeState = Lane_Change_Stop;
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP; //设定初始方向
|
|
|
|
GF_BSP_Interrupt_Add_CallBack(
|
|
DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
|
|
}
|
|
|
|
void GF_Dispatch()
|
|
{
|
|
|
|
|
|
|
|
|
|
if (Get_BIT(SystemErrorCode, mk32_sbus) == 1)
|
|
{
|
|
CurrentMoveState = Move_HALT;
|
|
CurrentTiltState = TiltHALT; //点头，抬头
|
|
CurrentRougheningEndState = RougheningEndHALT;
|
|
}
|
|
else
|
|
{
|
|
if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
|
|
{
|
|
CurrentMoveState = Move_HALT;
|
|
CurrentTiltState = TiltHALT; //点头，抬头
|
|
CurrentRougheningEndState = RougheningEndHALT;
|
|
|
|
}
|
|
else
|
|
{
|
|
MoveControl();
|
|
TiltControl();
|
|
DHRougheningControl();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
GV.LaneChangeDistance = CV.PV.LaneChangeDistance;
|
|
|
|
//电机直径280mm/101传动比 发送的数据单位是0.1 rpm
|
|
// 1 rpm 意思是1分钟转1圈，英文是 1rotation per minite。一圈的弧度是2π rad，一分钟60秒，那么
|
|
// 1 rpm = 1(r)/1(min) = 1(r)/60(s) = 2π (rad)/60(s) =π/30 (rad/s)
|
|
// 所以，1 rpm = π/30 (rad/s), 即 1 rpm = 0.1047 （rad/s）
|
|
// v=ω×r
|
|
|
|
//double length=sqrt(P_MK32->CH0_RY_H*P_MK32->CH0_RY_H+P_MK32->CH1_RY_V*P_MK32->CH1_RY_V);
|
|
//double MoveDir=
|
|
|
|
compensation_control();
|
|
IV_control();
|
|
|
|
//点头、抬头 这里是上下移动
|
|
action_perfrom(TiltTransitions,
|
|
sizeof(TiltTransitions) / sizeof(transition_t), CurrentTiltState);
|
|
//拉毛
|
|
action_perfrom(RougheningEndTransitions,
|
|
sizeof(RougheningEndTransitions) / sizeof(transition_t),
|
|
CurrentRougheningEndState);
|
|
//手动模式
|
|
action_perfrom(MoveTransitions,
|
|
sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);
|
|
|
|
}
|
|
void IV_control()
|
|
{
|
|
IV.Tempature = GV.Tempature;
|
|
IV.RunMode = IV_MCU_Run_State;
|
|
IV.LeftCompensation = GV.Left_Compensation;
|
|
IV.RightCompensation = GV.Right_Compensation;
|
|
IV.CurrentAngle = GV.Robot_Angle;
|
|
IV.RobotMoveSpeed = (int32_t) GetVehicleSpeed(speed_selection);
|
|
IV.EndPressure = GV.ForceValue;
|
|
|
|
}
|
|
|
|
void action_perfrom(transition_t transitions[], int length, int state)
|
|
{
|
|
|
|
for (index_counter = 0; index_counter < length; index_counter++)
|
|
{
|
|
if (transitions[index_counter].State == state)
|
|
{
|
|
if (transitions[index_counter].robotRun != NULL)
|
|
{
|
|
transitions[index_counter].robotRun();
|
|
}
|
|
|
|
break;
|
|
//return;
|
|
}
|
|
}
|
|
}
|
|
|
|
//机器人运动部分
|
|
void MoveControl()
|
|
{
|
|
//换道优先级优于普通的运行;
|
|
if (LaneChangeControl() != 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
speed_selection = (P_MK32->CH11_RD1 + 1000) / 200;
|
|
|
|
GV.Robot_Move_Speed = (int32_t) GetVehicleSpeed(speed_selection) / 0.880
|
|
* 101 * 10;
|
|
//自动巡航
|
|
if (P_MK32->CH5_SB == -1000) //前进
|
|
{
|
|
//#define Move_Manual 0
|
|
if (CV.PV.RunMode == Move_Horizontal_Move)
|
|
{
|
|
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < 45 * 100)
|
|
{
|
|
CurrentMoveState = Move_Horizontal_Task_Forwards_Left;
|
|
IV_MCU_Run_State = IV_Ding_Su_Shui_Ping_Qian_Jin;
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < 45 * 100)
|
|
{
|
|
CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
|
|
IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Qian_Jin;
|
|
}else
|
|
{
|
|
CurrentMoveState = Move_HALT;
|
|
}
|
|
|
|
}
|
|
// else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
|
|
// IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Qian_Jin;
|
|
// }
|
|
else if (CV.PV.RunMode == Move_Vertical_Move)
|
|
{
|
|
CurrentMoveState = Move_Vertical_Task_Forwards;
|
|
IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Qian_Jin;
|
|
}
|
|
else //0
|
|
{
|
|
CurrentMoveState = Move_Forwards;
|
|
IV_MCU_Run_State = IV_Ding_Su_Shou_Dong_Hou_Tui;
|
|
}
|
|
}
|
|
else if (P_MK32->CH5_SB == 1000) //后退
|
|
{
|
|
// if (CV.PV.RunMode == Move_Horizontal_Move_Left)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
|
|
// IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
|
|
// }
|
|
// else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
|
|
// IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
|
|
// }
|
|
// else if (CV.PV.RunMode == Move_Vertical_Move)
|
|
// {
|
|
// CurrentMoveState = Move_Vertical_Task_Backwards;
|
|
// IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Hou_Tui;
|
|
// }
|
|
//
|
|
// else //-1000
|
|
// {
|
|
// CurrentMoveState = Move_Backwards;
|
|
// IV_MCU_Run_State = IV_Ding_Su_Shou_Dong_Hou_Tui;
|
|
// }
|
|
|
|
if (CV.PV.RunMode == Move_Horizontal_Move)
|
|
{
|
|
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) < 45 * 100)
|
|
{
|
|
CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
|
|
IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) < 45 * 100)
|
|
{
|
|
CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
|
|
IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
|
|
}else
|
|
{
|
|
CurrentMoveState = Move_HALT;
|
|
}
|
|
|
|
}
|
|
// else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
|
|
// IV_MCU_Run_State = IV_Shui_Ping_Mo_Shi_Hou_Tui;
|
|
// }
|
|
else if (CV.PV.RunMode == Move_Vertical_Move)
|
|
{
|
|
CurrentMoveState = Move_Vertical_Task_Backwards;
|
|
IV_MCU_Run_State = IV_Ding_Su_Shu_Zhi_Hou_Tui;
|
|
}
|
|
|
|
else //-1000
|
|
{
|
|
CurrentMoveState = Move_Backwards;
|
|
IV_MCU_Run_State = IV_Ding_Su_Shou_Dong_Hou_Tui;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (abs(P_MK32->CH3_LY_H) <= 600)
|
|
{
|
|
if (abs(P_MK32->CH2_LY_V) >= 600)
|
|
{
|
|
if (P_MK32->CH2_LY_V >= 600) //forward
|
|
{
|
|
|
|
if (CV.PV.RunMode == Move_Horizontal_Move)
|
|
{
|
|
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
|
|
< 45 * 100)
|
|
{
|
|
CurrentMoveState =
|
|
Move_Horizontal_Task_Forwards_Left;
|
|
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
|
|
< 45 * 100)
|
|
{
|
|
CurrentMoveState =
|
|
Move_Horizontal_Task_Forwards_Right;
|
|
|
|
}else
|
|
{
|
|
CurrentMoveState = Move_HALT;
|
|
}
|
|
|
|
}
|
|
|
|
// if (CV.PV.RunMode == Move_Horizontal_Move_Left)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Forwards_Left;
|
|
// }
|
|
// else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
|
|
// }
|
|
else if (CV.PV.RunMode == Move_Vertical_Move)
|
|
{
|
|
CurrentMoveState = Move_Vertical_Task_Forwards;
|
|
}
|
|
else //0
|
|
{
|
|
CurrentMoveState = Move_Forwards;
|
|
}
|
|
|
|
}
|
|
else if (P_MK32->CH2_LY_V <= 600) //backward
|
|
{
|
|
|
|
if (CV.PV.RunMode == Move_Horizontal_Move)
|
|
{
|
|
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
|
|
< 45 * 100)
|
|
{
|
|
CurrentMoveState =
|
|
Move_Horizontal_Task_Backwards_Left;
|
|
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
|
|
< 45 * 100)
|
|
{
|
|
CurrentMoveState =
|
|
Move_Horizontal_Task_Backwards_Right;
|
|
|
|
}else
|
|
{
|
|
CurrentMoveState = Move_HALT;
|
|
}
|
|
|
|
}
|
|
// if (CV.PV.RunMode == Move_Horizontal_Move_Left)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
|
|
// }
|
|
// else if (CV.PV.RunMode == Move_Horizontal_Move_Right)
|
|
// {
|
|
// CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
|
|
// }
|
|
else if (CV.PV.RunMode == Move_Vertical_Move)
|
|
{
|
|
CurrentMoveState = Move_Vertical_Task_Backwards;
|
|
}
|
|
else //-1000
|
|
{
|
|
CurrentMoveState = Move_Backwards;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//只有在停止运动的时候，才可以进行换道命令
|
|
CurrentMoveState = Move_HALT;
|
|
}
|
|
}
|
|
else //>300
|
|
{
|
|
if (abs(P_MK32->CH2_LY_V) <= 600)
|
|
{
|
|
|
|
if (P_MK32->CH3_LY_H <= -600)
|
|
{
|
|
CurrentMoveState = Move_TurnLeft;
|
|
}
|
|
else if (P_MK32->CH3_LY_H >= 600)
|
|
{
|
|
CurrentMoveState = Move_TurnRight;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//只有在停止运动的时候，才可以进行换道命令
|
|
|
|
CurrentMoveState = Move_HALT;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
//超过某个角度，则一直向左转，在误差允许范围内，进行水平左移动，然后计算时间，随后停止
|
|
|
|
void TiltControl()
|
|
{
|
|
//Tilt 部分 前端上升、前端下降
|
|
if (P_MK32->CH1_RY_V == 1000)
|
|
{
|
|
CurrentTiltState = TiltDown;
|
|
}
|
|
else if (P_MK32->CH1_RY_V == -1000)
|
|
{
|
|
CurrentTiltState = TiltUP;
|
|
|
|
}
|
|
else
|
|
{
|
|
CurrentTiltState = TiltHALT;
|
|
}
|
|
|
|
}
|
|
//拉毛前端运动部分
|
|
void DHRougheningControl()
|
|
{
|
|
//拉毛部分
|
|
if (P_MK32->CH7_SD == -1000)
|
|
{
|
|
CurrentRougheningEndState = RougheningEndSwingClockWise;
|
|
}
|
|
else if (P_MK32->CH7_SD == 1000)
|
|
{
|
|
CurrentRougheningEndState = RougheningEndSwingAntiClockWise;
|
|
}
|
|
else
|
|
{
|
|
CurrentRougheningEndState = RougheningEndHALT;
|
|
}
|
|
|
|
}
|
|
|
|
#define Horizontal_Head_To_Left_Flag 0
|
|
#define Horizontal_Head_To_Right_Flag 1
|
|
#define Veritical_To_Left_Flag 2
|
|
#define Veritical_To_Right_Flag 3
|
|
char LaneChangeFlage = -1;
|
|
int LaneChangeControl()
|
|
{
|
|
//m/min = 100cm/60s = 10/6 = 5/3 cm/s =5*1000/3 cm/ms
|
|
GV.Robot_Move_Speed = LaneChangeSpeed / 0.880 * 101 * 10;
|
|
|
|
if (P_MK32->CH4_SA == 1000) //水平换道
|
|
{
|
|
|
|
if (HorizontalLaneChangeState == Lane_Change_Start)
|
|
{
|
|
if (LaneChangeFlage == Horizontal_Head_To_Left_Flag)
|
|
{
|
|
Horizontal_Lane_Change_Turn_To_Left_Control();
|
|
}
|
|
else if (LaneChangeFlage == Horizontal_Head_To_Right_Flag)
|
|
{
|
|
Horizontal_Lane_Change_Turn_To_Right_Control();
|
|
}
|
|
|
|
}
|
|
IV_MCU_Run_State = IV_Shui_Ping_Huang_Dao;
|
|
|
|
}
|
|
else if (P_MK32->CH4_SA == -1000) //竖直换道
|
|
{
|
|
// if (HorizontalLaneChangeState == Lane_Change_Start)
|
|
// {
|
|
//
|
|
// Horizontal_Lane_Change_Turn_To_Right_Control();
|
|
// }
|
|
// IV_MCU_Run_State = IV_Shu_Zhi_Huang_Dao;
|
|
|
|
if (VerticalLaneChangeState == Lane_Change_Start)
|
|
{
|
|
|
|
if (LaneChangeFlage == Veritical_To_Left_Flag)
|
|
{
|
|
//Vertical_Lane_Change_Right_Control();
|
|
Vertical_Lane_Change_Left_Control();
|
|
}
|
|
else
|
|
{
|
|
Vertical_Lane_Change_Right_Control();
|
|
}
|
|
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//头朝左
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) <= 45 * 100)
|
|
{
|
|
// 头朝左，换道意味着头
|
|
LaneChangeFlage = Horizontal_Head_To_Right_Flag;
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) <= 45 * 100)
|
|
{
|
|
LaneChangeFlage = Horizontal_Head_To_Left_Flag;
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) <= 45 * 100)
|
|
{
|
|
LaneChangeFlage = Veritical_To_Left_Flag;
|
|
}
|
|
else
|
|
{
|
|
LaneChangeFlage = -1;
|
|
}
|
|
|
|
HorizontalLaneChangeState = Lane_Change_Start;
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP; //设定初始方向
|
|
|
|
VerticalLaneChangeState = Lane_Change_Start;
|
|
Current_Vertical_ChangeState = VerticalChange_StateZero;
|
|
|
|
}
|
|
|
|
return P_MK32->CH4_SA;
|
|
// if (P_MK32->CH4_SA != 0)
|
|
// {
|
|
// return P_MK32->CH4_SA;
|
|
// }
|
|
//
|
|
// if (P_MK32->CH6_SC == -1000) //竖直左换道
|
|
// {
|
|
// if (VerticalLaneChangeState == Lane_Change_Start)
|
|
// {
|
|
// Vertical_Lane_Change_Left_Control();
|
|
// }
|
|
//
|
|
// }
|
|
// else if (P_MK32->CH6_SC == 1000) //竖直右换道
|
|
// {
|
|
// if (VerticalLaneChangeState == Lane_Change_Start)
|
|
// {
|
|
// Vertical_Lane_Change_Right_Control();
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
// VerticalLaneChangeState = Lane_Change_Start;
|
|
// Current_Vertical_ChangeState = VerticalChange_StateZero;
|
|
// }
|
|
//
|
|
// return P_MK32->CH6_SC;
|
|
}
|
|
|
|
//水平 向下换道 最终朝右
|
|
void Horizontal_Lane_Change_Turn_To_Right_Control()
|
|
{
|
|
switch (CurrentHorizontal_ChangeState)
|
|
{
|
|
case HorizontalChange_TurnToUP:
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
|
|
{
|
|
CurrentMoveState = Move_Head_To_UP_Enum; //转到上面
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
|
|
//开启计时
|
|
timer_handler_1.start_timer = 1;
|
|
CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
|
|
}
|
|
break;
|
|
}
|
|
case HorizontalChange_DelayMove:
|
|
{
|
|
LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
|
|
/ LaneChangeSpeed;
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
|
{
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToRight;
|
|
CurrentMoveState = Move_Head_To_Right_Enum; //头朝RightAngle
|
|
}
|
|
break;
|
|
}
|
|
case HorizontalChange_TurnToRight:
|
|
{
|
|
CurrentMoveState = Move_Head_To_Right_Enum; //头朝RightAngle
|
|
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) <= 100)
|
|
{
|
|
CurrentHorizontal_ChangeState = HorizontalChange_End;
|
|
}
|
|
break;
|
|
}
|
|
case HorizontalChange_End:
|
|
{
|
|
HorizontalLaneChangeState = Lane_Change_Stop;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void Horizontal_Lane_Change_Turn_To_Left_Control()
|
|
{
|
|
switch (CurrentHorizontal_ChangeState)
|
|
{
|
|
case HorizontalChange_TurnToUP:
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
|
|
{
|
|
CurrentMoveState = Move_Head_To_UP_Enum; //转到上面
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_Vertical_Task_Backwards; //转到左边，前进
|
|
//开启计时
|
|
timer_handler_1.start_timer = 1;
|
|
CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
|
|
}
|
|
break;
|
|
}
|
|
case HorizontalChange_DelayMove:
|
|
{
|
|
LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
|
|
/ LaneChangeSpeed;
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1)) //计时结束
|
|
{
|
|
CurrentHorizontal_ChangeState = HorizontalChange_TurnToLeft;
|
|
CurrentMoveState = Move_Head_To_Left_Enum; //头朝LeftAngle
|
|
}
|
|
break;
|
|
}
|
|
case HorizontalChange_TurnToLeft:
|
|
{
|
|
CurrentMoveState = Move_Head_To_Left_Enum; //头朝LeftAngle
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) <= 100)
|
|
{
|
|
CurrentHorizontal_ChangeState = HorizontalChange_End;
|
|
}
|
|
break;
|
|
}
|
|
case HorizontalChange_End:
|
|
{
|
|
HorizontalLaneChangeState = Lane_Change_Stop;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void Vertical_Lane_Change_Left_Control()
|
|
{
|
|
switch (Current_Vertical_ChangeState)
|
|
{
|
|
case VerticalChange_StateZero:
|
|
{
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToRight;
|
|
|
|
break;
|
|
}
|
|
case VerticalChange_TurnToRight:
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) >= 100)
|
|
{
|
|
CurrentMoveState = Move_Head_To_Right_Enum;
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //转到左边，前进
|
|
//开启计时
|
|
timer_handler_1.start_timer = 1;
|
|
Current_Vertical_ChangeState = VerticalChange_DelayMove;
|
|
}
|
|
break;
|
|
}
|
|
case VerticalChange_DelayMove:
|
|
{
|
|
LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
|
|
/ LaneChangeSpeed;
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
|
{
|
|
CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToUP;
|
|
// Current_Vertical_ChangeState = VerticalChange_TurnToDown;
|
|
}
|
|
break;
|
|
}
|
|
case VerticalChange_TurnToUP:
|
|
//case VerticalChange_TurnToDown:
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 20 * 100)
|
|
{
|
|
CurrentMoveState = Move_Head_To_UP_Enum;
|
|
}
|
|
else
|
|
{
|
|
Current_Vertical_ChangeState = VerticalChange_End;
|
|
}
|
|
// if (abs(GV.Robot_Angle - CV.RobotDownAngleValue) >= 20 * 100)
|
|
// {
|
|
// CurrentMoveState = Move_Head_To_Down_Enum;
|
|
// }
|
|
// else
|
|
// {
|
|
// Current_Vertical_ChangeState = VerticalChange_End;
|
|
// }
|
|
|
|
break;
|
|
}
|
|
case VerticalChange_End:
|
|
{
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void Vertical_Lane_Change_Right_Control()
|
|
{
|
|
switch (Current_Vertical_ChangeState)
|
|
{
|
|
case VerticalChange_StateZero:
|
|
{
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
|
|
break;
|
|
}
|
|
case VerticalChange_TurnToLeft:
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) >= 100)
|
|
{
|
|
CurrentMoveState = Move_Head_To_Left_Enum; //
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //转到左边，前进
|
|
//开启计时
|
|
timer_handler_1.start_timer = 1;
|
|
Current_Vertical_ChangeState = VerticalChange_DelayMove;
|
|
}
|
|
break;
|
|
}
|
|
case VerticalChange_DelayMove:
|
|
{
|
|
LaneChangeWaittime = 60 * CV.PV.LaneChangeDistance
|
|
/ LaneChangeSpeed;
|
|
if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
|
|
{
|
|
CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
|
|
Current_Vertical_ChangeState = VerticalChange_TurnToUP;
|
|
}
|
|
break;
|
|
}
|
|
case VerticalChange_TurnToUP:
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 20 * 100)
|
|
{
|
|
CurrentMoveState = Move_Head_To_UP_Enum;
|
|
}
|
|
else
|
|
{
|
|
Current_Vertical_ChangeState = VerticalChange_End;
|
|
}
|
|
|
|
break;
|
|
}
|
|
case VerticalChange_End:
|
|
{
|
|
VerticalLaneChangeState = Lane_Change_Stop;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
int32_t GetVehicleSpeed(int speed_selection)
|
|
{
|
|
switch (speed_selection)
|
|
{
|
|
case 0:
|
|
return 0;
|
|
case 1:
|
|
return CV.Speed_1;
|
|
case 2:
|
|
return CV.Speed_2;
|
|
case 3:
|
|
return CV.Speed_3;
|
|
case 4:
|
|
return CV.Speed_4;
|
|
case 5:
|
|
return CV.Speed_5;
|
|
case 6:
|
|
return CV.Speed_6;
|
|
case 7:
|
|
return CV.Speed_7;
|
|
case 8:
|
|
return CV.Speed_8;
|
|
case 9:
|
|
return CV.Speed_9;
|
|
case 10:
|
|
return CV.Speed_10;
|
|
default:
|
|
return 0;
|
|
}
|
|
|
|
}
|
|
// every 2 miliseconds
|
|
bool Left_Time_Start = false;
|
|
bool Right_Time_Start = false;
|
|
void compensation_control()
|
|
{
|
|
if (P_MK32->CH14_LT > 100)
|
|
{
|
|
if (!Left_Time_Start)
|
|
{
|
|
Left_Time_Start = true;
|
|
timer_handler_3.start_timer = 1; //重新开始计时
|
|
}
|
|
else
|
|
{
|
|
if (CompareTimer(LeftRightCalbrationTime, &timer_handler_3)) //计时结束
|
|
{
|
|
GV.Left_Compensation += 10;
|
|
if (GV.Left_Compensation >= 500)
|
|
{
|
|
GV.Left_Compensation = 500;
|
|
}
|
|
Left_Time_Start = false;
|
|
}
|
|
}
|
|
|
|
}
|
|
else if (P_MK32->CH14_LT < -100)
|
|
{
|
|
if (!Left_Time_Start)
|
|
{
|
|
Left_Time_Start = true;
|
|
timer_handler_3.start_timer = 1; //重新开始计时
|
|
}
|
|
else
|
|
{
|
|
if (CompareTimer(LeftRightCalbrationTime, &timer_handler_3)) //计时结束
|
|
{
|
|
GV.Left_Compensation -= 10;
|
|
if (GV.Left_Compensation <= 0)
|
|
{
|
|
GV.Left_Compensation = 0;
|
|
}
|
|
Left_Time_Start = false;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Left_Time_Start = false;
|
|
//停止计时
|
|
}
|
|
if (P_MK32->CH15_RT < -100)
|
|
{
|
|
if (!Right_Time_Start)
|
|
{
|
|
Right_Time_Start = true;
|
|
timer_handler_4.start_timer = 1; //重新开始计时
|
|
}
|
|
else
|
|
{
|
|
if (CompareTimer(LeftRightCalbrationTime, &timer_handler_4)) //计时结束
|
|
{
|
|
GV.Right_Compensation += 10;
|
|
if (GV.Right_Compensation >= 500)
|
|
{
|
|
GV.Right_Compensation = 500;
|
|
}
|
|
Right_Time_Start = false;
|
|
}
|
|
}
|
|
|
|
}
|
|
else if (P_MK32->CH15_RT > 100)
|
|
{
|
|
|
|
if (!Right_Time_Start)
|
|
{
|
|
Right_Time_Start = true;
|
|
timer_handler_4.start_timer = 1; //重新开始计时
|
|
}
|
|
else
|
|
{
|
|
if (CompareTimer(LeftRightCalbrationTime, &timer_handler_4)) //计时结束
|
|
{
|
|
GV.Right_Compensation -= 10;
|
|
if (GV.Right_Compensation <= 0)
|
|
{
|
|
GV.Right_Compensation = 0;
|
|
}
|
|
Right_Time_Start = false;
|
|
}
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
Right_Time_Start = false;
|
|
}
|
|
|
|
}
|
|
|