xushengqi
/
Code_Pcakages
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 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.
21 Commits
3 Branches
1 Tag
136 MiB
Tree: e982d5cb93
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'e982d5cb93'
${ noResults }
Code_Pcakages/Paint_0626/Core/Src/fsm.c

1872 lines
46 KiB
Raw Normal View History

江南喷漆
8 months ago
/*
* fsm.c
*
* Created on: Oct 18, 2024
* Author: akeguo
*/
#include "bsp_tempature.h"
#include "fsm.h"
#include "math.h"
int LaneChangeWaittimeConstant = 200;
double Err_Left = 0; //差值
double Err_Right = 0;
double Err_Horizontal_Hold = 0.00;
double Err_Vertical_Up_Hold = 0.1;
double Err_Paint=0.00;
double Err_Vertical_Down_Hold = 0;
int Region_Move_time = 0;
char IsRobotHaltSateChangedFlag = 0;
#define M_PI 3.14159265358979323846
#define Move_Manual 1
#define Move_Horizontal_Move 2 //手动水平纠偏
#define Move_Vertical_Move_To_Left 3 //竖直向左
#define Move_Vertical_Move_To_Right 4 //竖直向右
#define Move_Automation_Move_Horizontal_Move 5
#define Move_Automation_Move_Vertical_Move_To_Left 6
#define Move_Automation_Move_Vertical_Move_To_Right 7
#define AutoLaneChange_Close 0
#define AutoLaneChange_Open 1
#define LeftRightCalbrationTime 100
#define Move_Horizontal_AngleThreshold_D 80
int Vertical_LaneChangeFlag = 0; //竖直换道标志 --------》似乎没用
#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 //似乎没用
char LaneChangeFlage = -1;
int AutoMoveStartFlag = 0; //自动作业模式开启 0无 1 开
int Auto_Move_Cycle_Total_Count = 0;
int Auto_Move_Cycle_Count = 0;
void IV_control();
void compensation_control();
void PV_Data_Reading();
void MoveControl();
int RegionAutoMoveControl();
int32_t GetVehicleSpeed();
int LaneChangeControl();
int LaneChangeControl2();
void PaintGunControl();
int Move_Halt_AngleError();
void PaintGun_Working_OFF();
int AutoMoveControl();
void Horizontal_Lane_Change_Turn_To_Left_Control();
void Horizontal_Lane_Change_Turn_To_Right_Control();
void Vertical_Lane_Change_Left_Control();
void Vertical_Lane_Change_From_Left_To_Right_UP_Control();
void Vertical_Lane_Change_From_Left_To_Right_Down_Control();
void Vertical_Lane_Change_From_Right_To_Left_UP_Control();
void Vertical_Lane_Change_From_Right_To_Left_Down_Control();
int MoveTask_Start();
void RegionAuto_Vertical_Distance_Err();
void Auto_Move_StartFrom_Right_Up();
void RegionAuto_Vertical_RightToLeft(); //右下角
void RegionAuto_Vertical_LeftToRight(); //左下角
int MoveTaskIsOk(double DistanceMeters);
void Paint_Gun_Control_In_Automation_Horizontal_Or_Vertical_Move_State(
double RealWorkingWidth);
void robot_backwards();
void robot_forwards();
void joysticker_manual_control();
int LaneChangeWaittime = 0;
int LaneChangeWaittime4444 = 0;
int CH13_S2_Value = 0;
int32_t speed_selection;
Motor_Power_STATE_t Current_Motor_Power_State; //当前电机电源状态控制上下文
MoveSTATE_t CurrentMoveState;
MoveTaskTimeCheck_t MoveTaskTimeCheck;
LaneChangeSTATE_t CurrentLaneChangeSTATE;//没用到
SetSTATE_t CurrentSetState; //设置
Lane_Horizontal_ChangeState CurrentHorizontal_ChangeState;
Lane_Vertical_ChangeState Current_Vertical_ChangeState;
LaneChangeControlSTATE HorizontalLaneChangeState, VerticalLaneChangeState; //水平和竖直换道标志位
AutoMoveSTATE_t Current_Auto_Move_State;
PaintGunSTATE_t Current_PaintGun_STATE; //机器人喷枪状态
int index_counter = 0;
//手动下 机器人运动
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 },
};
transition_t PaintGun_Transitions[] =
{
{ PaintGunOFF, PaintGun_OFF_Do },
{ PaintGunON, PaintGun_ON_Do },
};
transition_t Motors_Power_State_Transitions[] =
{
{ Power_OFF, Motors_Power_OFF_Do },
{ Power_ON, Motors_Power_ON_Do },
};
void Fsm_Init()
{
Current_PaintGun_STATE = PaintGunOFF; //关
VerticalLaneChangeState = Lane_Change_Stop; //换道停
Current_Vertical_ChangeState = VerticalChange_StateZero; //0状态
HorizontalLaneChangeState = Lane_Change_Stop;
CurrentHorizontal_ChangeState = HorizontalChange_StateZero; //设定初始方向
GF_BSP_Interrupt_Add_CallBack(
DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}
char is_upper_computer_take_over_control = 0;
char Switch_Off_Flag = 1; //急停标志
/*
*
* */
void GF_Dispatch()
{
PV_Data_Reading(); ////按钮复位时 app数据生效
GV.LaneChangeDistance = GV.PV.LaneChangeDistance;
compensation_control(); //调节水平走时候的补偿值
IV_control();
//车体
action_perfrom(MoveTransitions,
sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);
action_perfrom(PaintGun_Transitions, sizeof(PaintGun_Transitions),
Current_PaintGun_STATE);
action_perfrom(Motors_Power_State_Transitions,
sizeof(Motors_Power_State_Transitions) / sizeof(transition_t),
Current_Motor_Power_State);
//上位机此时，其他设备全部处于不可操作状态，喷漆停止、摆臂上抬，只保持轮子的基本操作（为处理遥控器无法使用的情况）
if (is_upper_computer_take_over_control == Taken_Over)
{
GV.PV.RunMode = Move_Manual;
Current_PaintGun_STATE = PaintGunOFF;
Current_Motor_Power_State = Power_ON;
if (Switch_Off_Flag == 1)
{
CurrentMoveState = Move_HALT;
Switch_Off_Flag = 0;
LS_Motor_Need_To_Activate = 1;
}
return;
}
//急停按下 断电
if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000) //急停按下
{
GV.LeftMotor.Target_Velcity = 0;
GV.RightMotor.Target_Velcity = 0;
Switch_Off_Flag = 1;
Current_Motor_Power_State = Power_OFF;
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
IV.RunMode = IV_Run_Mode_EmergencyStop;
return;
}
Current_Motor_Power_State = Power_ON;
if (Switch_Off_Flag == 1) //急停按下
{
Switch_Off_Flag = 0;
LS_Motor_Need_To_Activate = 1;
Current_PaintGun_STATE = PaintGunOFF;
return;
}
if (Get_BIT(SystemErrorCode, ComError_Remote_Button_Reset_State) //按钮未复位
!= Has_Reset)
{
CH13_S2_Value = P_MK32->CH13_S2;
GV.PV.RunMode = Move_Manual;
Current_PaintGun_STATE = PaintGunOFF;
CurrentMoveState = Move_HALT; //运动状态停止、 喷漆状态停止、其他控制按钮需要添加
return;
}
if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == DISCONNECTED) //SBUS出错
{
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return;
}
//遥控器串口断开，串口时断时续，此时有可能会造成机器人停止运动
//陀螺仪无信号
if (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == DISCONNECTED
|| Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
{
GV.PV.RunMode = Move_Manual;
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return;
}
if (P_MK32->IsOnline == 0)
{
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return;
}
if (GV.PV.RunMode == Move_Manual) //app 无 的时候才能使用测试开启按钮
{
PaintGunControl(); //测试开启关闭
}
else if (GV.PV.RunMode >= Move_Horizontal_Move
&& GV.PV.RunMode <= Move_Vertical_Move_To_Right) //其余除自动模式外 控制开喷枪
{
if (P_MK32->CH13_S2 != CH13_S2_Value)
{
Current_PaintGun_STATE = PaintGunON;
IsRobotHaltSateChangedFlag = 0;
CH13_S2_Value = P_MK32->CH13_S2;
}
}
if (CurrentMoveState != Move_HALT)
{
IsRobotHaltSateChangedFlag = 1; //机器人动了就是1
}
if(Move_Halt_AngleError()==0)
{
return;
}
MoveControl();
}
/* 暂存App传输的PV数据，用于在业务执行过程中进行数据隔离
*/
void PV_Data_Reading() //按钮复位时 app数据生效
{
//&& P_MK32->CH6_SC == 0
if (P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH7_SD == 0
&& P_MK32->CH14_LT == 0 && P_MK32->CH15_RT == 0)
{
GV.PV = decoded_PV;
}
}
/* IV 更新
* */
void IV_control()
{
IV.Tempature = GV.TempatureE_2C;
IV.LeftCompensation = GV.Left_Compensation;
IV.RightCompensation = GV.Right_Compensation;
IV.CurrentAngle = GV.Robot_Angle;
IV.RobotMoveSpeed = GV.PV.RobotSpeed;
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
IV.Left_Motor_Err = GV.LeftMotor.Motor_Fault;
IV.Right_Motor_Err = GV.RightMotor.Motor_Fault;
}
/* 状态执行器
* */
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 (GV.PV.RunMode == 0) //自动模式 手动模式为未启用时 为0 不让控制（app重启时）
{
CurrentMoveState = Move_HALT;
return;
}
GV.Robot_Move_Speed = speed_m_per_min_to_pulse_s(GetVehicleSpeed());
if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move
|| GV.PV.RunMode == Move_Automation_Move_Vertical_Move_To_Left
|| GV.PV.RunMode == Move_Automation_Move_Vertical_Move_To_Right)
{
IV.RunMode = IV_Run_Mode_Automation; //属于自动作业范畴
RegionAutoMoveControl();
// if (RegionAutoMoveControl() == 1) //属于自动作业范畴 0 则进入下一步
// {
// return;
// }
return; //返回零 不属于自动作业范畴
}
//换道优先级优于普通的运行;
if (LaneChangeControl2() == 1) //自动作业范畴
{
return;
}
if (P_MK32->CH5_SB == -1000) //自动巡航前进模式 不是换道和自动就能用
{
robot_forwards();
return;
}
if (P_MK32->CH5_SB == 1000) //自动巡航后退模式
{
robot_backwards();
return;
}
joysticker_manual_control(); //摇杆手动控制
}
/*
* //一旦选择了自动模式，主逻辑后续代码都不执行
*/
int RegionAutoMoveControl()
{
if (P_MK32->CH7_SD != -1000) //自动作业按钮未打开
{
AutoMoveStartFlag = 0;
CurrentMoveState=Move_HALT; //防止按下自动模式按钮 ，再归位 机器不停车
return 0;
}
//自动作业打开
//等价于 if (P_MK32->CH7_SD != -1000){ //to do sth.}
//自动作业标志位复位
if (AutoMoveStartFlag == 0) //设置初始化状态
{
Current_Auto_Move_State = Auto_Move_Start;
AutoMoveStartFlag = 1;
Auto_Move_Cycle_Count = 0;
//return 1;
}
//执行到这里 AutoMoveStartFlag == 1，
//为自动模式，并且按钮实现，则进行操作
if (GV.PV.RunMode == Move_Automation_Move_Vertical_Move_To_Left)
{
RegionAuto_Vertical_RightToLeft();
return 1;
}
if (GV.PV.RunMode == Move_Automation_Move_Vertical_Move_To_Right)
{
RegionAuto_Vertical_LeftToRight();
return 1;
}
if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) //暂时不要水平
{
// if()
// {
// Auto_Move_StartFrom_Right_And_Up();
// }
//Auto_Move_StartFrom_Right_And_Up();
//IV.RunMode = IV_Run_Mode_Automation;
return 1;
}
return 0;
}
/**
* @brief 手动模式下自动巡航前进，
* @function 前进
* @param 无
* @retval 无
*/
void robot_forwards()
{
//#define Move_Manual 1
if (GV.PV.RunMode == Move_Horizontal_Move) //水平
{
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
< Move_Horizontal_AngleThreshold_D * 100)
{
CurrentMoveState = Move_Horizontal_Task_Forwards_Left; //头朝左前进
return;
}
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) //头朝右前进
< Move_Horizontal_AngleThreshold_D * 100)
{
CurrentMoveState = Move_Horizontal_Task_Forwards_Right;
return;
}
CurrentMoveState = Move_HALT; //角度不对 不走
return;
}
if (GV.PV.RunMode == Move_Vertical_Move_To_Left
|| GV.PV.RunMode == Move_Vertical_Move_To_Right) //竖直向左 向右
{
CurrentMoveState = Move_Vertical_Task_Forwards; //前进纠偏
return;
}
CurrentMoveState = Move_Forwards; // app 无
return;
}
/**
* @brief 手动模式下自动巡航后退，
* @function 自动后退： 竖直模式下
* @param 无
* @retval 无
*/
void robot_backwards()
{
if (GV.PV.RunMode == Move_Horizontal_Move) //水平
{
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
< Move_Horizontal_AngleThreshold_D * 100)
{
CurrentMoveState = Move_Horizontal_Task_Backwards_Left;
return;
}
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
< Move_Horizontal_AngleThreshold_D * 100)
{
CurrentMoveState = Move_Horizontal_Task_Backwards_Right;
return;
}
CurrentMoveState = Move_HALT;
return;
}
if (GV.PV.RunMode == Move_Vertical_Move_To_Left
|| GV.PV.RunMode == Move_Vertical_Move_To_Right)
{
CurrentMoveState = Move_Vertical_Task_Backwards;
return;
}
CurrentMoveState = Move_Backwards;
return;
}
/**
* @brief 手动模式下运动，根据左侧控制按钮的值进行控制
* @param 无
* @retval 无
* @detail 前进，后退;水平模式下的前进后退，需要有校准，竖直的前进后退也需要有校准
*/
void joysticker_manual_control()
{
// int front_angle=90;
// int right_move_angle=0;
// int back_move_angle=-90;
IV.RunMode = IV_Run_Mode_Maunal;
//若都在设置的范围内，则机器人运动为停止
if (abs(P_MK32->CH2_LY_V) <= CV.Joy_Sticker_Value_Allowance
&& abs(P_MK32->CH3_LY_H) <= CV.Joy_Sticker_Value_Allowance)
{
CurrentMoveState = Move_HALT;
return;
}
int angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;
if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)
{
//前进
robot_forwards(); //根据app模式选择纠偏与不纠偏
return;
}
//后退
if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
{
robot_backwards();
return;
}
//右转
if (abs(angle - 0) <= CV.Joy_Sticker_Angle_Allowance)
{
CurrentMoveState = Move_TurnRight;
return;
}
//左转
if (abs(angle - 180) <= CV.Joy_Sticker_Angle_Allowance
|| abs(angle - (-180)) <= CV.Joy_Sticker_Angle_Allowance)
{
CurrentMoveState = Move_TurnLeft;
return;
}
}
char Paint_Gun_Open_Flag = 0; //喷枪开启 1yes
/* 喷枪控制
* 喷漆项目
* */
void PaintGunControl()
{
if (P_MK32->CH6_SC == -1000)
{
if (Paint_Gun_Open_Flag == 0)
{
Current_PaintGun_STATE = PaintGunON;
IsRobotHaltSateChangedFlag = 0;
Paint_Gun_Open_Flag = 1;
}
}
else if (P_MK32->CH6_SC == 0)
{
Current_PaintGun_STATE = PaintGunOFF;
Paint_Gun_Open_Flag = 0;
//IsRobotHaltSateChangedFlag=1;
}
else if (P_MK32->CH6_SC == 1000)
{
Current_PaintGun_STATE = PaintGunOFF;
Paint_Gun_Open_Flag = 0;
//IsRobotHaltSateChangedFlag=1;
}
}
///* 作业过程中，角度偏差超过允许值时 关闭喷枪并停车
// * 扫描当前车体运动状态 及 当前车体角度
// * */
int Move_Halt_AngleError()
{
if (GV.PV.RunMode == Move_Horizontal_Move) //水平
{
if (CurrentMoveState == Move_Horizontal_Task_Forwards_Left)
{
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
> CV.Robot_Permitted_Angler_Error_Value_E_2D)
{
if(Current_PaintGun_STATE == PaintGunON)
{
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return 0;
}
}
}
else if (CurrentMoveState == Move_Horizontal_Task_Forwards_Right)
{
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
> CV.Robot_Permitted_Angler_Error_Value_E_2D)
{
if(Current_PaintGun_STATE == PaintGunON)
{
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return 0;
}
}
}
//后退的无限制
}
else if (GV.PV.RunMode == Move_Vertical_Move_To_Right
|| GV.PV.RunMode == Move_Vertical_Move_To_Left) //竖直
{
if (CurrentMoveState == Move_Vertical_Task_Forwards
|| CurrentMoveState == Move_Vertical_Task_Backwards)
{
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)
> CV.Robot_Permitted_Angler_Error_Value_E_2D)
{
if(Current_PaintGun_STATE == PaintGunON)
{
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return 0;
}
return 1;
}
}
}
else if( GV.PV.RunMode == Move_Automation_Move_Horizontal_Move ) //自动水平
{
return 1;
}
else if( GV.PV.RunMode == Move_Automation_Move_Vertical_Move_To_Left
|| GV.PV.RunMode == Move_Automation_Move_Vertical_Move_To_Right ) //自动竖直
{
if (CurrentMoveState == Move_Vertical_Task_Forwards
|| CurrentMoveState == Move_Vertical_Task_Backwards)
{
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)
> CV.Robot_Permitted_Angler_Error_Value_E_2D)
{
if(Current_PaintGun_STATE == PaintGunON)
{
CurrentMoveState = Move_HALT;
Current_PaintGun_STATE = PaintGunOFF;
return 0;
}
return 1;
}
}
}
return 1;
}
/* 手动换道
* 换道依据当前车头朝向
* */
int LaneChangeControl2()
{
//机器人SA按键处于中间状态
if (P_MK32->CH4_SA == 0)
{
HorizontalLaneChangeState = Lane_Change_Start;
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水平换道
{
if (GV.PV.RunMode == Move_Horizontal_Move)
{
IV.RunMode = IV_Run_Mode_Horizontal_LaneChange; //水平换道
if (HorizontalLaneChangeState != Lane_Change_Start)
{
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;
}
IV.RunMode = IV_Run_Mode_Vertical_LaneChange; //竖直换道
if (VerticalLaneChangeState != Lane_Change_Start)
{
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;
}
IV.RunMode = IV_Run_Mode_Vertical_LaneChange; //竖直换道
if (VerticalLaneChangeState != Lane_Change_Start) //换道结束了
{
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 = LaneChangeSpeed / 0.880 * 101 * 10;
GV.Robot_Move_Speed = speed_m_per_min_to_pulse_s(
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 )
{
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:
{
//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;
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)
<= 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_per_min_to_pulse_s(
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)
{
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 = RunTime_DistanceCm_SpeedE_2MPMin();
if (CompareTimer(LaneChangeWaittime, &timer_handler_1)) //计时结束
{
CurrentHorizontal_ChangeState = HorizontalChange_TurnToLeft;
CurrentMoveState = Move_Head_To_Left_Enum; //转到朝左
}
break;
}
case HorizontalChange_TurnToLeft:
{
CurrentMoveState = Move_Head_To_Left_Enum; //转到朝左
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
<= CV.Allowable_Error_For_Angle_Tracking)
{
CurrentHorizontal_ChangeState = HorizontalChange_End;
}
break;
}
case HorizontalChange_End:
{
HorizontalLaneChangeState = Lane_Change_Stop;
break;
}
default:
break;
}
}
/* 竖直从左往右作业 上端 向右换道 最终头朝上
* */
void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
{
//GV.Robot_Move_Speed = LaneChangeSpeed / 0.880 * 101 * 10;
GV.Robot_Move_Speed = speed_m_per_min_to_pulse_s(
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 )
{
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 = 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)
>= CV.Allowable_Error_For_Angle_Tracking )
{
CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上 到达角度速度为0
}
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 = LaneChangeSpeed / 0.880 * 101 * 10;
GV.Robot_Move_Speed = speed_m_per_min_to_pulse_s(
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 )
{
CurrentMoveState = Move_Head_To_Right_Enum; //转到朝右
}
else
{
CurrentMoveState = Move_Horizontal_Task_Forwards_Right; //qianjin
//开启计时
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)
>= CV.Allowable_Error_For_Angle_Tracking ) //误差在1度内
{
CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
}
else
{
Current_Vertical_ChangeState = VerticalChange_End;
}
break;
}
case VerticalChange_End:
{
VerticalLaneChangeState = Lane_Change_Stop;
break;
}
default:
break;
}
}
int a = 1000;
/* 竖直从右往左作业 上端 向左换道 最终头朝上
* */
void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
{
GV.Robot_Move_Speed = speed_m_per_min_to_pulse_s(
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 )
{
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 = 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)
>= CV.Allowable_Error_For_Angle_Tracking)
{
CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
}
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 = LaneChangeSpeed / 0.880 * 101 * 10;
GV.Robot_Move_Speed = speed_m_per_min_to_pulse_s(
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 )
{
CurrentMoveState = Move_Head_To_Left_Enum; //转到朝左
}
else
{
CurrentMoveState = Move_Horizontal_Task_Forwards_Left; //后退
//开启计时
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)
>= CV.Allowable_Error_For_Angle_Tracking ) //误差在1度内
{
CurrentMoveState = Move_Head_To_UP_Enum; //转到朝上
}
else
{
Current_Vertical_ChangeState = VerticalChange_End;
}
break;
}
case VerticalChange_End:
{
VerticalLaneChangeState = Lane_Change_Stop;
break;
}
default:
break;
}
}
/* 自动水平作业 从左至右
* */
void Auto_Move_StartFrom_Right_And_Up()
{
switch (Current_Auto_Move_State)
{
case Auto_Move_Start:
{
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) >= 200) //大于2°
{
CurrentMoveState = Move_Head_To_Right_Enum; //转到朝右
}
else
{
CurrentMoveState = Move_HALT;
GV.RightMotor.Start_Measuring = 1;
Current_Auto_Move_State = Auto_Move_Time_Wait;
timer_handler_1.start_timer = 1;
}
break;
}
case Auto_Move_Time_Wait:
{
if (CompareTimer(100, &timer_handler_1))
{
Current_Auto_Move_State = Auto_Move_HeadToRight_Backwards;
break;
}
}
case Auto_Move_HeadToRight_Backwards:
{
CurrentMoveState = Move_Horizontal_Task_Backwards_Right; //水平朝右 自动后退
if (abs(GV.RightMotor.Real_Disatnce) - GV.PV.WorkDistanceMeters
>= 0)
{
HorizontalLaneChangeState = Lane_Change_Start;
Current_Auto_Move_State = Auto_Move_Horizontal_Left_LaneChange;
CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
}
break;
}
case Auto_Move_Horizontal_Left_LaneChange:
{
Horizontal_Lane_Change_Turn_To_Left_Control(); //换道
if (HorizontalLaneChangeState == Lane_Change_Stop)
{
GV.RightMotor.Start_Measuring = 1;
Current_Auto_Move_State = Auto_Move_HeadToLeft_Backwards;
}
break;
}
case Auto_Move_HeadToLeft_Backwards:
{
CurrentMoveState = Move_Horizontal_Task_Backwards_Left; //水平朝左 自动后退
if (abs(GV.RightMotor.Real_Disatnce) - GV.PV.WorkDistanceMeters
>= 0)
{
HorizontalLaneChangeState = Lane_Change_Start;
Current_Auto_Move_State = Auto_Move_Horizontal_Right_LaneChange;
CurrentHorizontal_ChangeState = HorizontalChange_StateZero;
}
break;
}
case Auto_Move_Horizontal_Right_LaneChange:
{
Horizontal_Lane_Change_Turn_To_Right_Control(); //换道
if (HorizontalLaneChangeState == Lane_Change_Stop)
{
GV.RightMotor.Start_Measuring = 1;
timer_handler_1.start_timer = 1;
Current_Auto_Move_State = Auto_Move_Time_Wait2;
}
break;
}
case Auto_Move_Time_Wait2:
{
if (CompareTimer(100, &timer_handler_1))
{
Current_Auto_Move_State = Auto_Move_HeadToRight_Backwards;
break;
}
}
default:
break;
}
}
/*
* 自动区域作业 右向左 右下角开始
*/
void RegionAuto_Vertical_RightToLeft() //右下角
{
if (GV.PV.LaneChangeDistance == 0)
{
return;
}
Auto_Move_Cycle_Total_Count = GV.PV.WorkDistanceMeters * 100
/ GV.PV.LaneChangeDistance;
if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
{
CurrentMoveState = Move_HALT;
}
else
{
switch (Current_Auto_Move_State)
{
case Auto_Move_Start: //转至头朝上
{
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
{
CurrentMoveState = Move_Head_To_UP_Enum; //转至头朝上
}
else
{
timer_handler_1.start_timer = 1;
Current_Auto_Move_State = Auto_Move_Time_Wait; //停一会
}
break;
}
case Auto_Move_Time_Wait:
{
CurrentMoveState = Move_HALT; //看喷枪 开了 延时停 没开直接停 此时肯定没开
if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
{
GV.RightMotor.Start_Measuring = 1; //开始测距离
GV.LeftMotor.Start_Measuring = 1;
Current_Auto_Move_State = Auto_Move_HeadToUp_Forwards;
}
break;
}
case Auto_Move_HeadToUp_Forwards: //开始前进，自动作业正式执行
{
if (GV.RightMotor.Start_Measuring == 0
&& GV.LeftMotor.Start_Measuring == 0) //已经记录运动 起 始点
{
CurrentMoveState = Move_Vertical_Task_Forwards; //Forwards
MoveTask_Start(); //计时
RegionAuto_Vertical_Distance_Err(); //左右轮行驶距离与竖直方向宽度差值
//喷枪控制
Paint_Gun_Control_In_Automation_Horizontal_Or_Vertical_Move_State(
(double) GV.PV.WorkWidthMeters + Err_Paint);
if (GV.PV.WorkWidthMeters > 2) //Err_Right=真实距离-设置距离
{
//MoveTaskIsOk 计算时间停车
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Up_Hold)
== 1 || Err_Left - Err_Vertical_Up_Hold >= 0
|| Err_Right - Err_Vertical_Up_Hold >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Left_LaneChange_Up;
Auto_Move_Cycle_Count++;
}
}
else
{
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Up_Hold)
== 1 || Err_Left >= 0 || Err_Right >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Left_LaneChange_Up;
Auto_Move_Cycle_Count++;
}
}
}
break;
}
case Auto_Move_Vertical_Left_LaneChange_Up: //竖直上换道
{
//
Vertical_Lane_Change_From_Right_To_Left_UP_Control();
if (VerticalLaneChangeState == Lane_Change_Stop)
{
timer_handler_1.start_timer = 1;
Current_Auto_Move_State = Auto_Move_Time_Wait2;
}
break;
}
case Auto_Move_Time_Wait2:
{
CurrentMoveState = Move_HALT;
if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
{
GV.RightMotor.Start_Measuring = 1;
GV.LeftMotor.Start_Measuring = 1;
Current_Auto_Move_State = Auto_Move_HeadToUp_Backwards;
}
break;
}
case Auto_Move_HeadToUp_Backwards: //后退
{
if (GV.RightMotor.Start_Measuring == 0
&& GV.LeftMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
{
CurrentMoveState = Move_Vertical_Task_Backwards; //backwards
MoveTask_Start();
RegionAuto_Vertical_Distance_Err();
//喷枪控制
Paint_Gun_Control_In_Automation_Horizontal_Or_Vertical_Move_State(
(double) GV.PV.WorkWidthMeters + Err_Paint);
if (GV.PV.WorkWidthMeters > 2)
{
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Down_Hold)
== 1 || Err_Left - Err_Vertical_Down_Hold >= 0
|| Err_Right - Err_Vertical_Down_Hold >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Left_LaneChange_Down;
Auto_Move_Cycle_Count++;
}
}
else
{
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Down_Hold)
== 1 || Err_Left >= 0 || Err_Right >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Left_LaneChange_Down;
Auto_Move_Cycle_Count++;
}
}
}
break;
}
case Auto_Move_Vertical_Left_LaneChange_Down:
{
Vertical_Lane_Change_From_Right_To_Left_Down_Control(); //竖直下换道
if (VerticalLaneChangeState == Lane_Change_Stop)
{
Current_Auto_Move_State = Auto_Move_Time_Wait;
timer_handler_1.start_timer = 1;
}
break;
}
}
}
}
/*
* 自动区域作业 左向右 左下角开始
*/
void RegionAuto_Vertical_LeftToRight() //左下角
{
if (GV.PV.LaneChangeDistance == 0)
{
return;
}
Auto_Move_Cycle_Total_Count = GV.PV.WorkDistanceMeters * 100
/ GV.PV.LaneChangeDistance;
if (Auto_Move_Cycle_Count >= Auto_Move_Cycle_Total_Count)
{
CurrentMoveState = Move_HALT;
}
else
{
switch (Current_Auto_Move_State)
{
case Auto_Move_Start: //转至头朝上
{
if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) >= 200)
{
CurrentMoveState = Move_Head_To_UP_Enum; //转至头朝上
}
else
{
timer_handler_1.start_timer = 1;
Current_Auto_Move_State = Auto_Move_Time_Wait;
}
break;
}
case Auto_Move_Time_Wait:
{
CurrentMoveState = Move_HALT;
if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
{
GV.RightMotor.Start_Measuring = 1;
GV.LeftMotor.Start_Measuring = 1;
Current_Auto_Move_State = Auto_Move_HeadToUp_Forwards;
}
break;
}
case Auto_Move_HeadToUp_Forwards: //前进
{
if (GV.RightMotor.Start_Measuring == 0
&& GV.LeftMotor.Start_Measuring == 0) //已经记录运动 起 始点
{
CurrentMoveState = Move_Vertical_Task_Forwards; //Forwards
MoveTask_Start();
RegionAuto_Vertical_Distance_Err(); //竖直方向距离
Paint_Gun_Control_In_Automation_Horizontal_Or_Vertical_Move_State(
(double) GV.PV.WorkWidthMeters + Err_Paint);
if (GV.PV.WorkWidthMeters > 2)
{
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Up_Hold)
== 1 || Err_Left - Err_Vertical_Up_Hold >= 0
|| Err_Right - Err_Vertical_Up_Hold >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Right_LaneChange_Up;
Auto_Move_Cycle_Count++;
}
}
else
{
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Up_Hold)
== 1 || Err_Left >= 0 || Err_Right >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Right_LaneChange_Up;
Auto_Move_Cycle_Count++;
}
}
}
break;
}
case Auto_Move_Vertical_Right_LaneChange_Up: //竖直上换道 左--->右 喷枪在左
{
Vertical_Lane_Change_From_Left_To_Right_UP_Control();
if (VerticalLaneChangeState == Lane_Change_Stop)
{
timer_handler_1.start_timer = 1;
Current_Auto_Move_State = Auto_Move_Time_Wait2;
}
break;
}
case Auto_Move_Time_Wait2:
{
CurrentMoveState = Move_HALT;
if (CompareTimer(LaneChangeWaittimeConstant, &timer_handler_1))
{
GV.RightMotor.Start_Measuring = 1;
GV.LeftMotor.Start_Measuring = 1;
Current_Auto_Move_State = Auto_Move_HeadToUp_Backwards;
}
break;
}
case Auto_Move_HeadToUp_Backwards: //后退
{
if (GV.RightMotor.Start_Measuring == 0
&& GV.LeftMotor.Start_Measuring == 0) //已经属于在等到了，跟其他的一样
{
CurrentMoveState = Move_Vertical_Task_Backwards; //backwards
MoveTask_Start();
RegionAuto_Vertical_Distance_Err();
Paint_Gun_Control_In_Automation_Horizontal_Or_Vertical_Move_State(
(double) GV.PV.WorkWidthMeters
+ Err_Horizontal_Hold);
if (GV.PV.WorkWidthMeters > 2)
{
if ( MoveTaskIsOk(GV.PV.WorkWidthMeters + Err_Vertical_Down_Hold)
== 1 || Err_Left - Err_Vertical_Down_Hold >= 0
|| Err_Right - Err_Vertical_Down_Hold >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Right_LaneChange_Down;
Auto_Move_Cycle_Count++;
}
}
else
{
if (MoveTaskIsOk(
GV.PV.WorkWidthMeters + Err_Vertical_Down_Hold)
== 1 || Err_Left >= 0 || Err_Right >= 0)
{
MoveTask_Reset();
VerticalLaneChangeState = Lane_Change_Start;
Current_Vertical_ChangeState =
VerticalChange_StateZero;
Current_Auto_Move_State =
Auto_Move_Vertical_Right_LaneChange_Down;
Auto_Move_Cycle_Count++;
}
}
}
break;
}
case Auto_Move_Vertical_Right_LaneChange_Down:
{
Vertical_Lane_Change_From_Left_To_Right_Down_Control(); //竖直下换道
if (VerticalLaneChangeState == Lane_Change_Stop)
{
Current_Auto_Move_State = Auto_Move_Time_Wait;
timer_handler_1.start_timer = 1;
}
break;
}
}
}
}
int MoveTask_Start()
{
if (MoveTaskTimeCheck == MoveTaskTimeCheck_finish)
{
MoveTaskTimeCheck = MoveTaskTimeCheck_start;
timer_handler_1.start_timer = 1;
}
}
int MoveTask_Reset()
{
MoveTaskTimeCheck = MoveTaskTimeCheck_finish;
}
/*
*
* DistanceMeters: m
* return: 1 true,0 false
*/
int MoveTaskIsOk(double DistanceMeters)
{
return 0;
Region_Move_time = 60000 * DistanceMeters / GV.PV.RobotSpeed;
if (CompareTimer(Region_Move_time, &timer_handler_1))
{
MoveTask_Reset();
return 1;
}
else
{
return 0;
}
}
/*
* 计算竖直 实际距离与理论距离误差
*/
void RegionAuto_Vertical_Distance_Err()
{
Err_Left = fabs(GV.LeftMotor.Real_Disatnce)
//* cos(PI * GV.Left_Compensation / 180.0)
- GV.PV.WorkWidthMeters-0.1;
Err_Right = fabs(GV.RightMotor.Real_Disatnce)
//* cos(PI * GV.Left_Compensation / 180.0)
- GV.PV.WorkWidthMeters-0.1;
}
void Paint_Gun_Control_In_Automation_Horizontal_Or_Vertical_Move_State(
double RealWorkingWidth)
{
double ErrIn = (double) ((double) CV.Paint_Gun_Shutdown_Distance / 100.00); //关枪滑移距离m
if (fabs(GV.LeftMotor.Real_Disatnce) <= ErrIn
|| fabs(GV.RightMotor.Real_Disatnce) <= ErrIn) //先走才能开喷枪
{
//关闭喷枪
Current_PaintGun_STATE = PaintGunOFF;
return;
}
if ( fabs(GV.LeftMotor.Real_Disatnce) > ErrIn
&& fabs(GV.LeftMotor.Real_Disatnce) <RealWorkingWidth ) //走到滑移距离直接开
{
//打开喷枪
Current_PaintGun_STATE = PaintGunON;
return;
}
Current_PaintGun_STATE = PaintGunOFF;
return;
}
// double ErrIn = (double) ((double) CV.Paint_Gun_Shutdown_Distance / 100.00); //关枪滑移距离m
// //CV.Paint_Gun_Shutdown_Distance 单位是cm
// //GV.LeftMotor.Real_Disatnce 单位是m
// if (fabs(GV.LeftMotor.Real_Disatnce) <= ErrIn
// || fabs(GV.RightMotor.Real_Disatnce) <= ErrIn) //先走才能开喷枪
// {
// //关闭喷枪
// Current_PaintGun_STATE = PaintGunOFF;
// return;
// }
//
// if (fabs(GV.LeftMotor.Real_Disatnce) < (RealWorkingWidth - ErrIn - 0.05) //此时Real_Disatnce>errin <RealWorkingWidth-ErrIn
// && fabs(GV.RightMotor.Real_Disatnce) < (RealWorkingWidth - ErrIn - 0.05) //走到滑移距离直接开
// )
// {
// //打开喷枪
// Current_PaintGun_STATE = PaintGunON;
//
// return;
// }
// //关闭喷枪
// Current_PaintGun_STATE = PaintGunOFF;
// return;
/* 车体速度选择
* 具体车速在配置结构体CV
* 0.01m/min
* */
int32_t GetVehicleSpeed()
{
return GV.PV.RobotSpeed;
}
// every 2 miliseconds
bool Left_Time_Start = false;
bool Right_Time_Start = false;
/* 更新补偿 左右值
* 依据MK32键值
* P_MK32->CH14_LT
* P_MK32->CH15_RT
* */
void compensation_control()
{
if (P_MK32->CH14_LT > 100)
{
if (!Left_Time_Start)
{
Left_Time_Start = true;
timer_handler_3.start_timer = 1; //重新开始计时
}
else
{ //500
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;
}
}