/* * 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) errin 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; } }