diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 8abf83c..13fd3c0 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/BASE/Inc/MSP/msp_T_F4.h b/BASE/Inc/MSP/msp_T_F4.h
index bf3d361..b9f9b10 100644
--- a/BASE/Inc/MSP/msp_T_F4.h
+++ b/BASE/Inc/MSP/msp_T_F4.h
@@ -140,7 +140,7 @@ typedef struct
uint16_t robot_Compensation_Left :8;
-
+ uint16_t robot_Heart_Error;
} T_F4_Value;
extern T_F4_Value Current_T_F4_Value;
diff --git a/BASE/Src/MSP/msp_T_F4.c b/BASE/Src/MSP/msp_T_F4.c
index 63d6eb0..f051e0a 100644
--- a/BASE/Src/MSP/msp_T_F4.c
+++ b/BASE/Src/MSP/msp_T_F4.c
@@ -15,7 +15,7 @@ void decode_T_F4(uint8_t *buffer, uint16_t length);
DispacherController *T_F4_dispacherController;
T_F4_Value Current_T_F4_Value;
-T_F4_Value_2 Current_T_F4_Value_2;
+
struct UARTHandler *T_F4_Remote_Controller_UART_Handler;
void T_F4_Remote_Controller_intialize(struct UARTHandler *Handler)
@@ -81,13 +81,24 @@ void Write_T_F4_Data(void)
uint16_t Decoded_Reg_Value[6];
+uint16_t Last_Count;
void decode_T_F4(uint8_t *buffer, uint16_t length)
{
uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
/* CRC 校验正确 */
if (buffer[0] == 0x01 && buffer[1] == 0x04 && crc_check == MB_CRC16(buffer, length - 2))
{
- memcpy(&Current_T_F4_Value, &buffer[3], 6 * 2);
+ memcpy(&Current_T_F4_Value, &buffer[3], 5 * 2);
+
+ if(Current_T_F4_Value.heart_beat == Last_Count)
+ {
+ Current_T_F4_Value.robot_Heart_Error ++;
+ }
+ else
+ {
+ Current_T_F4_Value.robot_Heart_Error = 0;
+ }
+ Last_Count = Current_T_F4_Value.heart_beat;
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
"T_F4_Remote", 1);
@@ -95,7 +106,6 @@ void decode_T_F4(uint8_t *buffer, uint16_t length)
else
{
LOGFF(DL_ERROR, "DeChi decoding failed");
-
}
}
diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c
index e703649..5575616 100644
--- a/Core/Src/FSM.c
+++ b/Core/Src/FSM.c
@@ -150,7 +150,89 @@ void AutoBackSpray_On()
}
//--
//--车体停
-void Off_AutoSpray()
+void Off_AutoSpray(MoveSTATE_t MoveState)
+{
+ //喷漆自动关闭
+ if(AutoSpray_Flag == 1)
+ {
+
+ GF_BSP_GPIO_SetIO(Out_Spray,1);
+ AutoSpray_Stop_Count++;
+ }
+ //喷漆程序结束
+
+ //若自动喷漆启动，计数AutoSpray_Stop_Count*2ms后再停车
+ if(AutoSpray_Flag == 1)
+ {
+ if(AutoSpray_Stop_Count <= 250)
+ {
+ AutoSpray_Flag = 0;
+ AutoSpray_Stop_Count = 0;
+ CurrentMoveState = Move_HALT; //停车
+ }
+ }
+ else
+ {
+ CurrentMoveState = Move_HALT; //停车
+ }
+
+ //停车，禁止其它置位
+ if(Current_T_F4_Value.AutoForWard == 1
+ || Current_T_F4_Value.AutoBackWard == 1)
+ {
+ GV.AuTo_Flag = 99;
+ }
+ else
+ {
+ GV.AuTo_Flag = 0;
+ }
+
+ if(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1)
+ {
+ GV.Chg_Flag = 1;
+ Chg_Pa.change_road_finish_flag = 1;
+
+ }
+ else
+ {
+ GV.Chg_Flag = 0;
+ Chg_Pa.change_road_finish_flag = 0;
+ }
+
+}
+void Off_Emergency(MoveSTATE_t MoveState)
+{
+ GF_BSP_GPIO_SetIO(Out_Spray,1);
+ AutoSpray_Flag = 0;
+ CurrentMoveState = Move_HALT; //停车
+ //停车，禁止其它置位
+ if(Current_T_F4_Value.AutoForWard == 1
+ || Current_T_F4_Value.AutoBackWard == 1)
+ {
+ GV.AuTo_Flag = 99;
+ }
+ else
+ {
+ GV.AuTo_Flag = 0;
+ }
+
+ if(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
+ || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1)
+ {
+ GV.Chg_Flag = 1;
+ Chg_Pa.change_road_finish_flag = 1;
+
+ }
+ else
+ {
+ GV.Chg_Flag = 0;
+ Chg_Pa.change_road_finish_flag = 0;
+ }
+
+}
+
+void Off_ESpray()
{
//喷漆自动关闭
if(AutoSpray_Flag == 1)
@@ -201,10 +283,17 @@ void Off_AutoSpray()
}
}
+//--
+
//--
//--探边检测
void Dangerous_Test()
{
+ if(Current_T_F4_Value.AutoForWard == 0
+ && Current_T_F4_Value.AutoBackWard == 0)
+ {
+ return;
+ }
//计数，置位标志
//前进探边计数
if(GV.Robot_Ultrasonic.USValue_1 >= 5000 || GV.Robot_Ultrasonic.USValue_1 == 0)
@@ -374,32 +463,28 @@ void Dangerous_Flag_Delete()
}
-//--遥控器急停判断
-void RC_Emergency()
+
+//运动部分 Move Region
+void Robot_Move()
{
+ //--遥控器急停判断
if(Current_T_F4_Value.emergency_stop == 1)
{
- GV.AuTo_Flag = 0;
- GV.Chg_Flag = 0;
- Chg_Pa.change_road_finish_flag = 0;
-
- GF_BSP_GPIO_SetIO(Out_Spray,1);
GF_BSP_GPIO_SetIO(0,1);
- CurrentMoveState = Move_Emergency;
+ Off_Emergency(Move_Emergency);
+ return;
+ }
+ else if(Current_T_F4_Value.robot_Heart_Error == 1)
+ {
+ GF_BSP_GPIO_SetIO(0,1);
+ Off_Emergency(Move_Emergency);
+ return;
}
else
{
GF_BSP_GPIO_SetIO(0,0);
}
-}
-//运动部分 Move Region
-void Robot_Move()
-{
- if(Current_T_F4_Value.emergency_stop == 1)
- {
- return;
- }
if((Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
|| Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1
|| Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
@@ -493,29 +578,21 @@ void Robot_Move()
}
else
{
- Off_AutoSpray();
+ Off_AutoSpray(Move_HALT);
}
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
- //--遥控器急停判断
- RC_Emergency();
- //--自动作业模式下的探边检测判断
- if(Current_T_F4_Value.AutoForWard == 1
- || Current_T_F4_Value.AutoBackWard == 1)
- {
- Dangerous_Test();
- }
-
//--气压判断
Dym_Test();
-
+ //--自动作业模式下的探边检测判断
+ Dangerous_Test();
+ //--探边置位消除
+ Dangerous_Flag_Delete();
Robot_Move();
- //--探边置位消除
- Dangerous_Flag_Delete();
//--
/****************************************/