YuLin_Zhen
/
FW3.2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

新增心跳包急停判断

3.2
LIN\54376 1 month ago
parent
a124f81105
commit
f221f5f7d0
4 changed files with 123 additions and 36 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      .settings/language.settings.xml
  2. 2
      BASE/Inc/MSP/msp_T_F4.h
  3. 16
      BASE/Src/MSP/msp_T_F4.c
  4. 137
      Core/Src/FSM.c

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-48266367591953008" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1328381157137894459" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-48266367591953008" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1328381157137894459" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>

2
BASE/Inc/MSP/msp_T_F4.h
View File

@ -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;

16
BASE/Src/MSP/msp_T_F4.c
View File

@ -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");
}
}

137
Core/Src/FSM.c
View File

@ -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();
//--
/****************************************/

Write Preview
Loading…
Cancel
Save