From 4d259f5c7ba3db6861185e2f20ba4d9da9ccfb43 Mon Sep 17 00:00:00 2001 From: "LIN\\54376" <543769318@qq.com> Date: Tue, 23 Sep 2025 10:45:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E7=A8=8B=E5=BA=8F=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E5=8C=96=E5=90=8E=E6=8C=89=E9=94=AE=E6=98=AF=E5=90=A6?= =?UTF-8?q?=E5=A4=8D=E4=BD=8D=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BASE/Inc/MSP/msp_DH_Roughening.h | 1 + BASE/Src/MSP/msp_DH_Roughening.c | 14 +- Core/Protobuf/PSource/bsp_GV.pb.h | 9 +- Core/Protobuf/Proto/bsp_GV.proto | 3 +- Core/Src/FSM.c | 440 ++++++++++++++++-------------- 5 files changed, 256 insertions(+), 211 deletions(-) diff --git a/BASE/Inc/MSP/msp_DH_Roughening.h b/BASE/Inc/MSP/msp_DH_Roughening.h index 7c6ff01..2a979ea 100644 --- a/BASE/Inc/MSP/msp_DH_Roughening.h +++ b/BASE/Inc/MSP/msp_DH_Roughening.h @@ -186,6 +186,7 @@ typedef struct _DHRoughening uint16_t LaneChange_Vertical_Up :1;//竖直上换道 uint16_t LaneChange_Vertical_Down :1;//竖直下换道 + uint16_t Wireless_FirstFlag; }DHRoughening; diff --git a/BASE/Src/MSP/msp_DH_Roughening.c b/BASE/Src/MSP/msp_DH_Roughening.c index 5e3a4dc..a9376c0 100644 --- a/BASE/Src/MSP/msp_DH_Roughening.c +++ b/BASE/Src/MSP/msp_DH_Roughening.c @@ -44,7 +44,9 @@ void read_DHRoughening_Data(void) { DHRoughening_Controller_UART_Handler); } -uint8_t data5555[20]; +uint8_t data5555[2000]; +uint8_t Wireless_LastState; + void decode_DHRoughening(uint8_t *buffer, uint16_t length) { memcpy(data5555,buffer,length); @@ -52,10 +54,18 @@ void decode_DHRoughening(uint8_t *buffer, uint16_t length) { /* CRC 校验正确 */ if (crc_check == MB_CRC16(buffer, length - 2)) { + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "DHRoughening", 1); memcpy(&DHRougheningContrller,&buffer[3],12);//16个字节数字 + if(Wireless_LastState = 0 + && DHRougheningContrller.Wireless_State == 1) + { + DHRougheningContrller.Wireless_FirstFlag = 1; + } + + Wireless_LastState = DHRougheningContrller.Wireless_State; // *Roughenging_LeftCompensation=(*Roughenging_LeftCompensation_Range)*DHRougheningContrller.Left_Compensation*100/65535; // *Roughenging_RightCompensation=(*Roughenging_RightCompensation_Range)*DHRougheningContrller.Right_Compensation*100/65535; } else @@ -72,8 +82,6 @@ void send_to_DHRoughening() (uint16_t) abs(GV.Robot_Angle.RF_Angle_Roll), (uint16_t) abs(GV.Robot_ForceValue )); - - } //左补偿 xxx.xx 度 0x0000—0xFFFF==0—655.35 度 diff --git a/Core/Protobuf/PSource/bsp_GV.pb.h b/Core/Protobuf/PSource/bsp_GV.pb.h index d78d86b..71a61d7 100644 --- a/Core/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/Protobuf/PSource/bsp_GV.pb.h @@ -42,6 +42,7 @@ typedef struct _GV_struct_define { int32_t Robot_ForceValue; int32_t Robot_DynamometerValue; double Robot_Speed_Mpm; + int32_t Robot_Move_Step; int32_t Emergency; } GV_struct_define; @@ -51,8 +52,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define GV_struct_define_init_default {false, PV_struct_define_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, SP_MSP_U7_Button_init_default, 0, false, ErrorData_init_default, false, MSP_TL720DParameters_init_default, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#define GV_struct_define_init_zero {false, PV_struct_define_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, SP_MSP_U7_Button_init_zero, 0, false, ErrorData_init_zero, false, MSP_TL720DParameters_init_zero, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define GV_struct_define_init_default {false, PV_struct_define_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, SP_MSP_U7_Button_init_default, 0, false, ErrorData_init_default, false, MSP_TL720DParameters_init_default, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define GV_struct_define_init_zero {false, PV_struct_define_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, SP_MSP_U7_Button_init_zero, 0, false, ErrorData_init_zero, false, MSP_TL720DParameters_init_zero, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define GV_struct_define_PV_tag 1 @@ -72,6 +73,7 @@ extern "C" { #define GV_struct_define_Robot_ForceValue_tag 19 #define GV_struct_define_Robot_DynamometerValue_tag 20 #define GV_struct_define_Robot_Speed_Mpm_tag 21 +#define GV_struct_define_Robot_Move_Step_tag 22 #define GV_struct_define_Emergency_tag 23 /* Struct field encoding specification for nanopb */ @@ -93,6 +95,7 @@ X(a, STATIC, SINGULAR, INT32, Robot_ChgLength, 18) \ X(a, STATIC, SINGULAR, INT32, Robot_ForceValue, 19) \ X(a, STATIC, SINGULAR, INT32, Robot_DynamometerValue, 20) \ X(a, STATIC, SINGULAR, DOUBLE, Robot_Speed_Mpm, 21) \ +X(a, STATIC, SINGULAR, INT32, Robot_Move_Step, 22) \ X(a, STATIC, SINGULAR, INT32, Emergency, 23) #define GV_struct_define_CALLBACK NULL #define GV_struct_define_DEFAULT NULL @@ -110,7 +113,7 @@ extern const pb_msgdesc_t GV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size -#define GV_struct_define_size 912 +#define GV_struct_define_size 924 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/Proto/bsp_GV.proto b/Core/Protobuf/Proto/bsp_GV.proto index 66b2c43..6cca518 100644 --- a/Core/Protobuf/Proto/bsp_GV.proto +++ b/Core/Protobuf/Proto/bsp_GV.proto @@ -32,7 +32,8 @@ message GV_struct_define int32 Robot_ForceValue = 19; int32 Robot_DynamometerValue = 20; double Robot_Speed_Mpm= 21; - + int32 Robot_Move_Step = 22; + int32 Emergency = 23; }; diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c index 2de491f..cddd7ad 100644 --- a/Core/Src/FSM.c +++ b/Core/Src/FSM.c @@ -138,272 +138,304 @@ double Speed_Judge() return 0; } -void Robot_Move() +void Robot_Halt(MoveSTATE_t STATE) { - // 急停版本一 - if(DHRougheningContrller.Emergency_Stop == 1) - { - GF_BSP_GPIO_SetIO(Out_Vacucm,1); - GF_BSP_GPIO_SetIO(Out_Lift_1,1); - GF_BSP_GPIO_SetIO(Out_Lift_2,1); - if(Polish_Count == 100 && polish_Flag == 1) - { - GF_BSP_GPIO_SetIO(Out_Polish,1); - polish_Flag = 0; - Polish_Count = 0; - } - else if(polish_Flag == 1) - { - GF_BSP_GPIO_SetIO(Out_Polish,0); - Polish_Count++; - } + if(DHRougheningContrller.Horizontal_Task == 1 + || DHRougheningContrller.Vertical_Task == 1) + { + GV.AuTo_Flag = 99; + } + else + { + GV.AuTo_Flag = 0; + } - CurrentMoveState = Move_Emergency; - return; - } - else if(DHRougheningContrller.Emergency_Stop == 0) - { - } + if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 + || DHRougheningContrller.LaneChange_Horizontal_Right == 1 + || DHRougheningContrller.LaneChange_Vertical_Down == 1 + || DHRougheningContrller.LaneChange_Vertical_Up == 1)) + { + GV.Chg_Flag = 1; + Chg_Pa.change_road_finish_flag = 1; + } + else + { + GV.Chg_Flag = 0; + Chg_Pa.change_road_finish_flag = 0; + } + + CurrentMoveState = STATE; //停车 +} +void Robot_ResetJudge() +{ + if(DHRougheningContrller.Wireless_State == 0) + { + return; + } + if((DHRougheningContrller.LaneChange_Horizontal_Left == 0 + && DHRougheningContrller.LaneChange_Horizontal_Right == 0 + && DHRougheningContrller.LaneChange_Vertical_Down == 0 + && DHRougheningContrller.LaneChange_Vertical_Up == 0 + && DHRougheningContrller.Vertical_Task == 0 + && DHRougheningContrller.Horizontal_Task ==0) + && DHRougheningContrller.Forwards == 0 + && DHRougheningContrller.Backwards == 0 + && DHRougheningContrller.Left_Turn == 0 + && DHRougheningContrller.Right_Turn == 0 + && DHRougheningContrller.Roughening_Start == 0 + && DHRougheningContrller.End_Down == 0 + && DHRougheningContrller.End_Up == 0) + { + GV.Robot_Move_Step = 1; + } +} - // //运动部分 Move Region - if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 - || DHRougheningContrller.LaneChange_Horizontal_Right == 1 - || DHRougheningContrller.LaneChange_Vertical_Down == 1 - || DHRougheningContrller.LaneChange_Vertical_Up == 1 - || DHRougheningContrller.Vertical_Task == 1 - || DHRougheningContrller.Horizontal_Task ==1) - && DHRougheningContrller.Forwards == 0 - && DHRougheningContrller.Backwards == 0 - && DHRougheningContrller.Left_Turn == 0 - && DHRougheningContrller.Right_Turn == 0) +void Robot_Move() +{ + // 急停版本一 + if(DHRougheningContrller.Emergency_Stop == 1) { - if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 - && DHRougheningContrller.LaneChange_Horizontal_Right == 0 - && DHRougheningContrller.LaneChange_Vertical_Down == 0 - && DHRougheningContrller.LaneChange_Vertical_Up == 0 - && (DHRougheningContrller.Vertical_Task == 1 - || DHRougheningContrller.Horizontal_Task == 1)) //自动作业与换道互斥 + GF_BSP_GPIO_SetIO(Out_Vacucm,1); + GF_BSP_GPIO_SetIO(Out_Lift_1,1); + GF_BSP_GPIO_SetIO(Out_Lift_2,1); + + if(Polish_Count == 100 && polish_Flag == 1) { - if(DHRougheningContrller.Horizontal_Task == 1) - { - CurrentMoveState = Move_AutoForward; - if(GV.AuTo_Flag == 0) - { - GV.AuTo_Flag=1; - } - } - else if(DHRougheningContrller.Vertical_Task == 1) - { - CurrentMoveState = Move_AutoBackward; - if(GV.AuTo_Flag == 0) - { - GV.AuTo_Flag=3; - } - } - else - { - CurrentMoveState = Move_HALT;//停车 - } + GF_BSP_GPIO_SetIO(Out_Polish,1); + polish_Flag = 99; + Polish_Count = 0; } - else if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 - || DHRougheningContrller.LaneChange_Horizontal_Right == 1 - || DHRougheningContrller.LaneChange_Vertical_Down == 1 - || DHRougheningContrller.LaneChange_Vertical_Up == 1) - && (DHRougheningContrller.Vertical_Task == 0 - && DHRougheningContrller.Horizontal_Task == 0)) + else if(polish_Flag == 1) { - if(GV.Chg_Flag == 1 - && (DHRougheningContrller.LaneChange_Horizontal_Left == 1 - || DHRougheningContrller.LaneChange_Horizontal_Right == 1 - || DHRougheningContrller.LaneChange_Vertical_Down == 1 - || DHRougheningContrller.LaneChange_Vertical_Up == 1)) - { - CurrentMoveState = Move_ChgFinish; - } - else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 - && DHRougheningContrller.LaneChange_Horizontal_Right == 0 - && DHRougheningContrller.LaneChange_Vertical_Down == 0 - && DHRougheningContrller.LaneChange_Vertical_Up == 1 ) - { - CurrentMoveState = Move_ChgUp; - } - else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 - && DHRougheningContrller.LaneChange_Horizontal_Right == 0 - && DHRougheningContrller.LaneChange_Vertical_Down == 1 - && DHRougheningContrller.LaneChange_Vertical_Up == 0 ) - { - CurrentMoveState = Move_ChgDown; - } - else if(DHRougheningContrller.LaneChange_Horizontal_Left == 1 - && DHRougheningContrller.LaneChange_Horizontal_Right == 0 - && DHRougheningContrller.LaneChange_Vertical_Down == 0 - && DHRougheningContrller.LaneChange_Vertical_Up == 0 ) - { - CurrentMoveState = Move_ChgLeft; - } - else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 - && DHRougheningContrller.LaneChange_Horizontal_Right == 1 - && DHRougheningContrller.LaneChange_Vertical_Down == 0 - && DHRougheningContrller.LaneChange_Vertical_Up == 0 ) - { - CurrentMoveState = Move_ChgRight; - } - else - { - CurrentMoveState = Move_HALT;//停车 - } + GF_BSP_GPIO_SetIO(Out_Polish,0); + Polish_Count++; } - else + Robot_Halt(Move_Emergency); + return; + } + else if(DHRougheningContrller.Emergency_Stop == 0) + { + if(DHRougheningContrller.Roughening_Stop && polish_Flag == 99) { - CurrentMoveState = Move_HALT;//停车 + polish_Flag = 0; } } - else if((DHRougheningContrller.LaneChange_Horizontal_Left == 0 + + + //运动部分 Move Region + if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 + || DHRougheningContrller.LaneChange_Horizontal_Right == 1 + || DHRougheningContrller.LaneChange_Vertical_Down == 1 + || DHRougheningContrller.LaneChange_Vertical_Up == 1 + || DHRougheningContrller.Vertical_Task == 1 + || DHRougheningContrller.Horizontal_Task ==1) + && DHRougheningContrller.Forwards == 0 + && DHRougheningContrller.Backwards == 0 + && DHRougheningContrller.Left_Turn == 0 + && DHRougheningContrller.Right_Turn == 0) + { + if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 && DHRougheningContrller.LaneChange_Horizontal_Right == 0 && DHRougheningContrller.LaneChange_Vertical_Down == 0 && DHRougheningContrller.LaneChange_Vertical_Up == 0 - && DHRougheningContrller.Vertical_Task == 0 - && DHRougheningContrller.Horizontal_Task == 0) - && (DHRougheningContrller.Forwards == 1 - || DHRougheningContrller.Backwards == 1 - || DHRougheningContrller.Left_Turn == 1 - || DHRougheningContrller.Right_Turn == 1)) + && (DHRougheningContrller.Vertical_Task == 1 + || DHRougheningContrller.Horizontal_Task == 1)) //自动作业与换道互斥 { - if (DHRougheningContrller.Left_Turn == 0 - && DHRougheningContrller.Right_Turn == 0)//前进周围300范围内才继续检测前进是否按下 - { - if (DHRougheningContrller.Forwards == 1)//前进 + if(DHRougheningContrller.Horizontal_Task == 1) + { + CurrentMoveState = Move_AutoForward; + if(GV.AuTo_Flag == 0) { - CurrentMoveState = Move_Forwards; + GV.AuTo_Flag=1; } - else if (DHRougheningContrller.Backwards == 1) + } + else if(DHRougheningContrller.Vertical_Task == 1) + { + CurrentMoveState = Move_AutoBackward; + if(GV.AuTo_Flag == 0) { - CurrentMoveState = Move_Backwards; //后退 + GV.AuTo_Flag=3; } - else - { - CurrentMoveState = Move_HALT;//停车 - } - } - else if (DHRougheningContrller.Forwards == 0 - && DHRougheningContrller.Backwards == 0) + } + else + { + CurrentMoveState = Move_HALT;//停车 + } + } + else if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 + || DHRougheningContrller.LaneChange_Horizontal_Right == 1 + || DHRougheningContrller.LaneChange_Vertical_Down == 1 + || DHRougheningContrller.LaneChange_Vertical_Up == 1) + && (DHRougheningContrller.Vertical_Task == 0 + && DHRougheningContrller.Horizontal_Task == 0)) + { + if(GV.Chg_Flag == 1 + && (DHRougheningContrller.LaneChange_Horizontal_Left == 1 + || DHRougheningContrller.LaneChange_Horizontal_Right == 1 + || DHRougheningContrller.LaneChange_Vertical_Down == 1 + || DHRougheningContrller.LaneChange_Vertical_Up == 1)) { - if (DHRougheningContrller.Right_Turn == 1) - { - CurrentMoveState = Move_TurnRight; //右转 - } - else if (DHRougheningContrller.Left_Turn == 1) - { - CurrentMoveState = Move_TurnLeft; //左转 - } - else - { - CurrentMoveState = Move_HALT; //停车 - } + CurrentMoveState = Move_ChgFinish; } - else + else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 + && DHRougheningContrller.LaneChange_Horizontal_Right == 0 + && DHRougheningContrller.LaneChange_Vertical_Down == 0 + && DHRougheningContrller.LaneChange_Vertical_Up == 1 ) { - CurrentMoveState = Move_HALT; //停车 + CurrentMoveState = Move_ChgUp; } - } - else - { - - if(DHRougheningContrller.Horizontal_Task == 1 - || DHRougheningContrller.Vertical_Task == 1) + else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 + && DHRougheningContrller.LaneChange_Horizontal_Right == 0 + && DHRougheningContrller.LaneChange_Vertical_Down == 1 + && DHRougheningContrller.LaneChange_Vertical_Up == 0 ) { - GV.AuTo_Flag = 99; + CurrentMoveState = Move_ChgDown; } - else + else if(DHRougheningContrller.LaneChange_Horizontal_Left == 1 + && DHRougheningContrller.LaneChange_Horizontal_Right == 0 + && DHRougheningContrller.LaneChange_Vertical_Down == 0 + && DHRougheningContrller.LaneChange_Vertical_Up == 0 ) { - GV.AuTo_Flag = 0; + CurrentMoveState = Move_ChgLeft; } - - - if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 - || DHRougheningContrller.LaneChange_Horizontal_Right == 1 - || DHRougheningContrller.LaneChange_Vertical_Down == 1 - || DHRougheningContrller.LaneChange_Vertical_Up == 1)) + else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0 + && DHRougheningContrller.LaneChange_Horizontal_Right == 1 + && DHRougheningContrller.LaneChange_Vertical_Down == 0 + && DHRougheningContrller.LaneChange_Vertical_Up == 0 ) { - GV.Chg_Flag = 1; - Chg_Pa.change_road_finish_flag = 1; + CurrentMoveState = Move_ChgRight; } else { - GV.Chg_Flag = 0; - Chg_Pa.change_road_finish_flag = 0; + CurrentMoveState = Move_HALT;//停车 } - - CurrentMoveState = Move_HALT; //停车 } - - -} -void GF_Dispatch()//2ms调用一次 给车体速度等赋值 -{ - Robot_Move(); - /****************************************/ - - //LU_Single 控制打磨电机 - if(DHRougheningContrller.Roughening_Start && polish_Flag == 0) + else { - GF_BSP_GPIO_SetIO(Out_Vacucm,0); - GF_BSP_GPIO_SetIO(Out_Polish,0); - if(Polish_Count == 100) + CurrentMoveState = Move_HALT;//停车 + } + } + else if((DHRougheningContrller.LaneChange_Horizontal_Left == 0 + && DHRougheningContrller.LaneChange_Horizontal_Right == 0 + && DHRougheningContrller.LaneChange_Vertical_Down == 0 + && DHRougheningContrller.LaneChange_Vertical_Up == 0 + && DHRougheningContrller.Vertical_Task == 0 + && DHRougheningContrller.Horizontal_Task == 0) + + && (DHRougheningContrller.Forwards == 1 + || DHRougheningContrller.Backwards == 1 + || DHRougheningContrller.Left_Turn == 1 + || DHRougheningContrller.Right_Turn == 1)) + { + if (DHRougheningContrller.Left_Turn == 0 + && DHRougheningContrller.Right_Turn == 0)//前进周围300范围内才继续检测前进是否按下 + { + if (DHRougheningContrller.Forwards == 1)//前进 + { + CurrentMoveState = Move_Forwards; + } + else if (DHRougheningContrller.Backwards == 1) { - GF_BSP_GPIO_SetIO(Out_Polish,1); - polish_Flag = 1; + CurrentMoveState = Move_Backwards; //后退 } else { - Polish_Count++; + CurrentMoveState = Move_HALT;//停车 } } - else if(DHRougheningContrller.Roughening_Stop && polish_Flag == 1) + else if (DHRougheningContrller.Forwards == 0 + && DHRougheningContrller.Backwards == 0) { - GF_BSP_GPIO_SetIO(Out_Vacucm,1); - GF_BSP_GPIO_SetIO(Out_Polish,0); - if(Polish_Count == 100) + if (DHRougheningContrller.Right_Turn == 1) + { + CurrentMoveState = Move_TurnRight; //右转 + } + else if (DHRougheningContrller.Left_Turn == 1) { - GF_BSP_GPIO_SetIO(Out_Polish,1); - polish_Flag = 0; + CurrentMoveState = Move_TurnLeft; //左转 } else { - Polish_Count++; + CurrentMoveState = Move_HALT; //停车 } } else { - Polish_Count=0; + CurrentMoveState = Move_HALT; //停车 } - - - - - - //LU_Three控制打磨升降 - if(DHRougheningContrller.End_Up) + } + else + { + Robot_Halt(Move_HALT); //停车 + } + /****************************************/ + //LU_Single 控制打磨电机 + if(DHRougheningContrller.Roughening_Start && polish_Flag == 0) + { + GF_BSP_GPIO_SetIO(Out_Vacucm,0); + GF_BSP_GPIO_SetIO(Out_Polish,0); + if(Polish_Count == 100) { - GF_BSP_GPIO_SetIO(Out_Lift_1,0); - GF_BSP_GPIO_SetIO(Out_Lift_2,1); + GF_BSP_GPIO_SetIO(Out_Polish,1); + polish_Flag = 1; } - else if(DHRougheningContrller.End_Down) + else { - GF_BSP_GPIO_SetIO(Out_Lift_1,1); - GF_BSP_GPIO_SetIO(Out_Lift_2,0); + Polish_Count++; + } + } + else if(DHRougheningContrller.Roughening_Stop && polish_Flag == 1) + { + GF_BSP_GPIO_SetIO(Out_Vacucm,1); + GF_BSP_GPIO_SetIO(Out_Polish,0); + if(Polish_Count == 100) + { + GF_BSP_GPIO_SetIO(Out_Polish,1); + polish_Flag = 0; } else { - GF_BSP_GPIO_SetIO(Out_Lift_1,1); - GF_BSP_GPIO_SetIO(Out_Lift_2,1); + Polish_Count++; } + } + else + { + Polish_Count=0; + } + //LU_Three控制打磨升降 + if(DHRougheningContrller.End_Up) + { + GF_BSP_GPIO_SetIO(Out_Lift_1,0); + GF_BSP_GPIO_SetIO(Out_Lift_2,1); + } + else if(DHRougheningContrller.End_Down) + { + GF_BSP_GPIO_SetIO(Out_Lift_1,1); + GF_BSP_GPIO_SetIO(Out_Lift_2,0); + } + else + { + GF_BSP_GPIO_SetIO(Out_Lift_1,1); + GF_BSP_GPIO_SetIO(Out_Lift_2,1); + } +} +void GF_Dispatch()//2ms调用一次 给车体速度等赋值 +{ + switch(GV.Robot_Move_Step) + { + case 0: + Robot_ResetJudge(); + break; + case 1: + Robot_Move(); + default: + break; - + } IV.Robot_CurrentState = CurrentMoveState;