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

Compare commits

merge into: YuLin_Zhen:master
Branches Tags
YuLin_Zhen:3.2
YuLin_Zhen:master
...
pull from: YuLin_Zhen:3.2
Branches Tags
YuLin_Zhen:3.2
YuLin_Zhen:master

4 Commits
master ... 3.2

Author SHA1 Message Date
LIN\54376 fda8b070b8 新增自动作业里实时调整补偿值
1 month ago
LIN\54376 f221f5f7d0 新增心跳包急停判断
1 month ago
LIN\54376 a124f81105 新增漆膜测厚的序列号；在校验之后再判断新数据；急停信号的UDP正确上传
2 months ago
LIN\54376 9314843ef2 修改usart协议，增加各种状态
2 months ago
10 changed files with 377 additions and 175 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats
  1. 4
      .settings/language.settings.xml
  2. 2
      BASE/Inc/MSP/msp_T_F4.h
  3. 85
      BASE/Src/BSP/bsp_client_setting.c
  4. 23
      BASE/Src/MSP/msp_PaintThickness.c
  5. 16
      BASE/Src/MSP/msp_T_F4.c
  6. 1
      Core/Inc/FSM.h
  7. 13
      Core/Protobuf/PSource/bsp_GV.pb.h
  8. 4
      Core/Protobuf/Proto/bsp_GV.proto
  9. 374
      Core/Src/FSM.c
  10. 30
      Core/Src/robot_state.c

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <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.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" 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="-1716098405222817540" 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="1328381156250390778" 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.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <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.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" 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="-1716098405222817540" 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="1328381156250390778" 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.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </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_Compensation_Left :8;
uint16_t robot_Heart_Error;
} T_F4_Value; } T_F4_Value;
extern T_F4_Value Current_T_F4_Value; extern T_F4_Value Current_T_F4_Value;

85
BASE/Src/BSP/bsp_client_setting.c
View File

@ -10,13 +10,16 @@
#include "gpio.h" #include "gpio.h"
#include "bsp_UART.h" #include "bsp_UART.h"
void UpdateIV_State();
void UpdateIV(); void UpdateIV_PaintThickness();
void decode_received_data_from_client(uint8_t *buffer, uint16_t length); void decode_received_data_from_client(uint8_t *buffer, uint16_t length);
struct UARTHandler *client_setting_Handler; //相当于当前行及下一行这两个结构体与绑定的共享地址 struct UARTHandler *client_setting_Handler; //相当于当前行及下一行这两个结构体与绑定的共享地址
DispacherController *client_setting_dispacher; DispacherController *client_setting_dispacher;
int LastCount; int LastCount;
uint16_t Send_State_Count = 0;
uint16_t Send_PaintThickness_Flag = 0;
void client_setting_intialize(struct UARTHandler *Handler) void client_setting_intialize(struct UARTHandler *Handler)
{ {
@ -25,7 +28,9 @@ void client_setting_intialize(struct UARTHandler *Handler)
client_setting_Handler->Wait_time = 30; // 最低不要低于4； client_setting_Handler->Wait_time = 30; // 最低不要低于4；
client_setting_dispacher=Handler->dispacherController; client_setting_dispacher=Handler->dispacherController;
client_setting_dispacher->Add_Dispatcher_List(client_setting_dispacher,UpdateIV); client_setting_dispacher->Add_Dispatcher_List(client_setting_dispacher,UpdateIV_State);
client_setting_dispacher->Add_Dispatcher_List(client_setting_dispacher,UpdateIV_PaintThickness);
client_setting_dispacher->DispacherCallTime=1000; client_setting_dispacher->DispacherCallTime=1000;
client_setting_dispacher->Dispacher_Enable = 1; client_setting_dispacher->Dispacher_Enable = 1;
LOG("client_setting_intialize"); LOG("client_setting_intialize");
@ -34,37 +39,89 @@ void client_setting_intialize(struct UARTHandler *Handler)
} }
void UpdateIV()//往安卓发的
void UpdateIV_State()//往安卓发的
{ {
Send_State_Count ++;
client_setting_Handler->Tx_Buf[0] = 0x01;
client_setting_Handler->Tx_Buf[1] = 0x03;
client_setting_Handler->Tx_Buf[2] = 0x0C;
client_setting_Handler->Tx_Buf[3] = (Send_State_Count>>8) &0xff;
client_setting_Handler->Tx_Buf[4] = Send_State_Count & 0xff;
client_setting_Handler->Tx_Buf[5] = ((ushort)GV.Robot_State>>8) &0xff;
client_setting_Handler->Tx_Buf[6] = (ushort)GV.Robot_State & 0xff;
client_setting_Handler->Tx_Buf[7] = ((ushort)abs(GV.LeftFrontMotor.Current)>>8) &0xff;
client_setting_Handler->Tx_Buf[8] = (ushort)abs(GV.LeftFrontMotor.Current) & 0xff;
client_setting_Handler->Tx_Buf[9] = ((ushort)abs(GV.RightFrontMotor.Current)>>8) &0xff;
client_setting_Handler->Tx_Buf[10] = (ushort)abs(GV.RightFrontMotor.Current) & 0xff;
client_setting_Handler->Tx_Buf[11] = ((ushort)GV.Robot_Speed_mpm>>8) &0xff;
client_setting_Handler->Tx_Buf[12] = (ushort)GV.Robot_Speed_mpm & 0xff;
client_setting_Handler->Tx_Buf[13] = ((ushort)GV.Robot_Angle.RF_Angle_Roll>>8) &0xff;
client_setting_Handler->Tx_Buf[14] = (ushort)GV.Robot_Angle.RF_Angle_Roll & 0xff;
client_setting_Handler->TxCount = 17;
uint16_t crc=MB_CRC16(&(client_setting_Handler->Tx_Buf[0]), 15);
client_setting_Handler->Tx_Buf[16] = (crc>>8) &0xff;
client_setting_Handler->Tx_Buf[15] = crc & 0xff;
if(GV.Robot_PaintThickness_Count > LastCount) client_setting_Handler->UART_Tx(client_setting_Handler);
if(Send_State_Count >= ((uint16_t)pow(2, 32)) - 1)
{ {
Send_State_Count = 0;
}
}
void UpdateIV_PaintThickness()//往安卓发的
{
if(GV.Robot_PaintThickness_Count <= LastCount)
{
return;
} }
else if(GV.Robot_PaintThickness_Count == 1)
{ {
return; return;
} }
client_setting_Handler->Tx_Buf[0] = 0x01; client_setting_Handler->Tx_Buf[0] = 0x01;
client_setting_Handler->Tx_Buf[1] = 0x03; client_setting_Handler->Tx_Buf[1] = 0x03;
client_setting_Handler->Tx_Buf[2] = 0x02; client_setting_Handler->Tx_Buf[2] = 0x04;
client_setting_Handler->TxCount = 7; client_setting_Handler->TxCount = 9;
client_setting_Handler->Tx_Buf[3] = ((GV.Robot_PaintThickness_Count - 1)>>8) &0xff;
client_setting_Handler->Tx_Buf[4] = (GV.Robot_PaintThickness_Count - 1) & 0xff;
client_setting_Handler->Tx_Buf[3] = (GV.Robot_PaintThickness>>8) &0xff;
client_setting_Handler->Tx_Buf[4] = GV.Robot_PaintThickness & 0xff;
uint16_t crc=MB_CRC16(&(client_setting_Handler->Tx_Buf[0]), 5); client_setting_Handler->Tx_Buf[5] = (GV.Robot_PaintThickness>>8) &0xff;
client_setting_Handler->Tx_Buf[6] = GV.Robot_PaintThickness & 0xff;
client_setting_Handler->Tx_Buf[6] = (crc>>8) &0xff; uint16_t crc=MB_CRC16(&(client_setting_Handler->Tx_Buf[0]), 7);
client_setting_Handler->Tx_Buf[5] = crc & 0xff;
client_setting_Handler->Tx_Buf[8] = (crc>>8) &0xff;
client_setting_Handler->Tx_Buf[7] = crc & 0xff;
client_setting_Handler->UART_Tx(client_setting_Handler); client_setting_Handler->UART_Tx(client_setting_Handler);
if(GV.Robot_PaintThickness_Count >= ((uint16_t)pow(2, 31)) - 1)
{
GV.Robot_PaintThickness_Count = 1;
}
LastCount = GV.Robot_PaintThickness_Count; LastCount = GV.Robot_PaintThickness_Count;
} }
void decode_received_data_from_client(uint8_t *buffer, uint16_t length) void decode_received_data_from_client(uint8_t *buffer, uint16_t length)
{ {
uint8_t data[2000]; uint8_t data[2000];

23
BASE/Src/MSP/msp_PaintThickness.c
View File

@ -38,6 +38,7 @@ void Paint_Thickness_intialize(struct UARTHandler *Handler)
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"PaintThickness_sensor",0,PaintThickness_sensor); HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"PaintThickness_sensor",0,PaintThickness_sensor);
//uartHandler->Insert_HardWare_Entry_UART //uartHandler->Insert_HardWare_Entry_UART
LOG("steering_engine_intialize"); LOG("steering_engine_intialize");
} }
uint8_t Inquiry_Order_PT[6]={0X2E, 0X64, 0X30, 0X3C, 0X3A, 0X3E}; uint8_t Inquiry_Order_PT[6]={0X2E, 0X64, 0X30, 0X3C, 0X3A, 0X3E};
@ -53,18 +54,11 @@ void GF_PT_Inquiry()
void decode_PaintThickness(uint8_t *buffer, uint16_t length) void decode_PaintThickness(uint8_t *buffer, uint16_t length)
{ {
uint8_t data[2000];
memcpy(data,buffer,length);
//memcpy(data_PaintThickness,buffer,length); //memcpy(data_PaintThickness,buffer,length);
LOG("start decoding and the length is %d",length); LOG("start decoding and the length is %d",length);
if(length >= PaintThickness_LastLength + 5)
{
*PaintThickness_Count = *PaintThickness_Count + 1;
}
else
{
return;
}
/* 校验 */ /* 校验 */
//;<:> //;<:>
uint8_t crc_check[4] = {';','<',':','>'}; uint8_t crc_check[4] = {';','<',':','>'};
@ -75,6 +69,17 @@ void decode_PaintThickness(uint8_t *buffer, uint16_t length)
&& crc_check[1] == buffer[length - 3] && crc_check[1] == buffer[length - 3]
&& crc_check[0] == buffer[length - 4]) && crc_check[0] == buffer[length - 4])
{ {
//判断是否是新数据
if(length >= PaintThickness_LastLength + 5)
{
*PaintThickness_Count = *PaintThickness_Count + 1;
}
else
{
return;
}
int startIndex=0; int startIndex=0;
for(int j=5;j<=14;j++) for(int j=5;j<=14;j++)

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; DispacherController *T_F4_dispacherController;
T_F4_Value Current_T_F4_Value; T_F4_Value Current_T_F4_Value;
T_F4_Value_2 Current_T_F4_Value_2;
struct UARTHandler *T_F4_Remote_Controller_UART_Handler; struct UARTHandler *T_F4_Remote_Controller_UART_Handler;
void T_F4_Remote_Controller_intialize(struct UARTHandler *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 Decoded_Reg_Value[6];
uint16_t Last_Count;
void decode_T_F4(uint8_t *buffer, uint16_t length) void decode_T_F4(uint8_t *buffer, uint16_t length)
{ {
uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]); uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
/* CRC 校验正确 */ /* CRC 校验正确 */
if (buffer[0] == 0x01 && buffer[1] == 0x04 && crc_check == MB_CRC16(buffer, length - 2)) 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, HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
"T_F4_Remote", 1); "T_F4_Remote", 1);
@ -95,7 +106,6 @@ void decode_T_F4(uint8_t *buffer, uint16_t length)
else else
{ {
LOGFF(DL_ERROR, "DeChi decoding failed"); LOGFF(DL_ERROR, "DeChi decoding failed");
} }
} }

1
Core/Inc/FSM.h
View File

@ -51,6 +51,7 @@ typedef enum _MoveSTATE_t
Move_ChgFinish, Move_ChgFinish,
Move_Emergency, Move_Emergency,
Move_Error,
} MoveSTATE_t; } MoveSTATE_t;
//设置 换道距离和设置后退距离 //设置 换道距离和设置后退距离

13
Core/Protobuf/PSource/bsp_GV.pb.h
View File

@ -47,6 +47,7 @@ typedef struct _GV_struct_define {
int32_t Robot_Speed_mpm; int32_t Robot_Speed_mpm;
bool has_Robot_Ultrasonic; bool has_Robot_Ultrasonic;
MSP_UltrasonicParameters Robot_Ultrasonic; MSP_UltrasonicParameters Robot_Ultrasonic;
int32_t Robot_State;
int32_t Emergency; int32_t Emergency;
} GV_struct_define; } GV_struct_define;
@ -56,8 +57,8 @@ extern "C" {
#endif #endif
/* Initializer values for message structs */ /* 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, 0, false, MSP_UltrasonicParameters_init_default, 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, false, MSP_UltrasonicParameters_init_default, 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, false, MSP_UltrasonicParameters_init_zero, 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, false, MSP_UltrasonicParameters_init_zero, 0, 0}
/* Field tags (for use in manual encoding/decoding) */ /* Field tags (for use in manual encoding/decoding) */
#define GV_struct_define_PV_tag 1 #define GV_struct_define_PV_tag 1
@ -80,7 +81,8 @@ extern "C" {
#define GV_struct_define_Robot_PaintThickness_Count_tag 18 #define GV_struct_define_Robot_PaintThickness_Count_tag 18
#define GV_struct_define_Robot_Speed_mpm_tag 19 #define GV_struct_define_Robot_Speed_mpm_tag 19
#define GV_struct_define_Robot_Ultrasonic_tag 20 #define GV_struct_define_Robot_Ultrasonic_tag 20
#define GV_struct_define_Emergency_tag 21 #define GV_struct_define_Robot_State_tag 21
#define GV_struct_define_Emergency_tag 22
/* Struct field encoding specification for nanopb */ /* Struct field encoding specification for nanopb */
#define GV_struct_define_FIELDLIST(X, a) \ #define GV_struct_define_FIELDLIST(X, a) \
@ -104,7 +106,8 @@ X(a, STATIC, SINGULAR, INT32, Robot_PaintThickness, 17) \
X(a, STATIC, SINGULAR, INT32, Robot_PaintThickness_Count, 18) \ X(a, STATIC, SINGULAR, INT32, Robot_PaintThickness_Count, 18) \
X(a, STATIC, SINGULAR, INT32, Robot_Speed_mpm, 19) \ X(a, STATIC, SINGULAR, INT32, Robot_Speed_mpm, 19) \
X(a, STATIC, OPTIONAL, MESSAGE, Robot_Ultrasonic, 20) \ X(a, STATIC, OPTIONAL, MESSAGE, Robot_Ultrasonic, 20) \
X(a, STATIC, SINGULAR, INT32, Emergency, 21) X(a, STATIC, SINGULAR, INT32, Robot_State, 21) \
X(a, STATIC, SINGULAR, INT32, Emergency, 22)
#define GV_struct_define_CALLBACK NULL #define GV_struct_define_CALLBACK NULL
#define GV_struct_define_DEFAULT NULL #define GV_struct_define_DEFAULT NULL
#define GV_struct_define_PV_MSGTYPE PV_struct_define #define GV_struct_define_PV_MSGTYPE PV_struct_define
@ -122,7 +125,7 @@ extern const pb_msgdesc_t GV_struct_define_msg;
/* Maximum encoded size of messages (where known) */ /* Maximum encoded size of messages (where known) */
#define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size #define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size
#define GV_struct_define_size 1099 #define GV_struct_define_size 1111
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

4
Core/Protobuf/Proto/bsp_GV.proto
View File

@ -37,7 +37,9 @@ message GV_struct_define
int32 Robot_Speed_mpm = 19; int32 Robot_Speed_mpm = 19;
MSP_UltrasonicParameters Robot_Ultrasonic = 20; MSP_UltrasonicParameters Robot_Ultrasonic = 20;
int32 Emergency = 21; int32 Robot_State = 21;
int32 Emergency = 22;
}; };
//protoc --nanopb_out=. *.proto //protoc --nanopb_out=. *.proto

374
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) if(AutoSpray_Flag == 1)
@ -201,10 +283,17 @@ void Off_AutoSpray()
} }
} }
//--
//-- //--
//--探边检测 //--探边检测
void Dangerous_Test() 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) if(GV.Robot_Ultrasonic.USValue_1 >= 5000 || GV.Robot_Ultrasonic.USValue_1 == 0)
@ -374,166 +463,165 @@ void Dangerous_Flag_Delete()
} }
//--遥控器急停判断
void RC_Emergency() //运动部分 Move Region
void Robot_Move()
{ {
//--遥控器急停判断
if(Current_T_F4_Value.emergency_stop == 1) if(Current_T_F4_Value.emergency_stop == 1)
{ {
GF_BSP_GPIO_SetIO(Out_Spray,1);
GF_BSP_GPIO_SetIO(0,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 else
{ {
GF_BSP_GPIO_SetIO(0,0); GF_BSP_GPIO_SetIO(0,0);
} }
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值 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)
RC_Emergency(); && (Current_T_F4_Value.robot_forward_backward_speed >= 0X7A
//--自动作业模式下的探边检测判断 && Current_T_F4_Value.robot_forward_backward_speed <= 0X84)
if(Current_T_F4_Value.AutoForWard == 1 && (Current_T_F4_Value.robot_left_right_turn_speed >= 0X7A
|| Current_T_F4_Value.AutoBackWard == 1) && Current_T_F4_Value.robot_left_right_turn_speed <= 0X84)
&& GV.Robot_Ultrasonic.Dym_Value_Flag != 1)
{ {
Dangerous_Test(); if((Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
} &&(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))//自动作业与换道互斥
//--气压判断 {
Dym_Test(); if(Current_T_F4_Value.AutoForWard == 1)
//-- {
//运动部分 Move Region //如启停喷枪按下后判断前进速度到达后，喷漆自动开启
if((Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1 AutoForSpray_On();
|| 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) }
&& (Current_T_F4_Value.robot_forward_backward_speed >= 0X7A else if(Current_T_F4_Value.AutoBackWard == 1)
&& Current_T_F4_Value.robot_forward_backward_speed <= 0X84) {
&& (Current_T_F4_Value.robot_left_right_turn_speed >= 0X7A //如启停喷枪按下后判断前进速度到达后，喷漆自动开启
&& Current_T_F4_Value.robot_left_right_turn_speed <= 0X84) AutoBackSpray_On();
&& GV.Robot_Ultrasonic.Dym_Value_Flag != 1) //喷漆程序结束
}
}
else if((Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
&&(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))
{
if(GV.Chg_Flag == 1 && (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))
{ {
if((Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1) CurrentMoveState = Move_ChgFinish;
&&(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))//自动作业与换道互斥
{
if(Current_T_F4_Value.AutoForWard == 1)
{
//如启停喷枪按下后判断前进速度到达后，喷漆自动开启
AutoForSpray_On();
//喷漆程序结束
}
else if(Current_T_F4_Value.AutoBackWard == 1)
{
//如启停喷枪按下后判断前进速度到达后，喷漆自动开启
AutoBackSpray_On();
//喷漆程序结束
}
}
else if((Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
&&(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))
{
if(GV.Chg_Flag == 1 && (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))
{
CurrentMoveState = Move_ChgFinish;
}
else if(Current_T_F4_Value.ChgUp_Ver == 1
&& !(Current_T_F4_Value.ChgDown_Ver == 1
|| Current_T_F4_Value.ChgLeft_Hor == 1
|| Current_T_F4_Value.ChgRight_Hor == 1))
{
CurrentMoveState = Move_ChgUp;
}
else 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))
{
CurrentMoveState = Move_ChgDown;
}
else if(Current_T_F4_Value.ChgLeft_Hor == 1
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|| Current_T_F4_Value.ChgDown_Ver == 1
|| Current_T_F4_Value.ChgRight_Hor == 1))
{
CurrentMoveState = Move_ChgLeft;
}
else if(Current_T_F4_Value.ChgRight_Hor == 1
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|| Current_T_F4_Value.ChgDown_Ver == 1
|| Current_T_F4_Value.ChgLeft_Hor == 1))
{
CurrentMoveState = Move_ChgRight;
}
}
} }
else if((Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1 else if(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.ChgDown_Ver == 1
&& Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1) || Current_T_F4_Value.ChgLeft_Hor == 1
&& (Current_T_F4_Value.robot_forward_backward_speed < 0X7A //7A 84 || Current_T_F4_Value.ChgRight_Hor == 1))
|| Current_T_F4_Value.robot_forward_backward_speed > 0X84
|| Current_T_F4_Value.robot_left_right_turn_speed < 0X7A
|| Current_T_F4_Value.robot_left_right_turn_speed > 0X84))
{ {
if (Current_T_F4_Value.robot_forward_backward_speed < 0X7A)//前进 CurrentMoveState = Move_ChgUp;
{
CurrentMoveState = Move_Forwards;
}
else if (Current_T_F4_Value.robot_forward_backward_speed > 0X84)
{
CurrentMoveState = Move_Backwards; //后退
}
else if (Current_T_F4_Value.robot_left_right_turn_speed < 0X7A)
{
CurrentMoveState = Move_TurnLeft; //右转
}
else if (Current_T_F4_Value.robot_left_right_turn_speed > 0X84)
{
CurrentMoveState = Move_TurnRight; //左转
}
} }
else else 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))
{ {
Off_AutoSpray(); CurrentMoveState = Move_ChgDown;
}
else if(Current_T_F4_Value.ChgLeft_Hor == 1
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|| Current_T_F4_Value.ChgDown_Ver == 1
|| Current_T_F4_Value.ChgRight_Hor == 1))
{
CurrentMoveState = Move_ChgLeft;
}
else if(Current_T_F4_Value.ChgRight_Hor == 1
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|| Current_T_F4_Value.ChgDown_Ver == 1
|| Current_T_F4_Value.ChgLeft_Hor == 1))
{
CurrentMoveState = Move_ChgRight;
}
}
}
else 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)
&& (Current_T_F4_Value.robot_forward_backward_speed < 0X7A //7A 84
|| Current_T_F4_Value.robot_forward_backward_speed > 0X84
|| Current_T_F4_Value.robot_left_right_turn_speed < 0X7A
|| Current_T_F4_Value.robot_left_right_turn_speed > 0X84))
{
if (Current_T_F4_Value.robot_forward_backward_speed < 0X7A)//前进
{
CurrentMoveState = Move_Forwards;
}
else if (Current_T_F4_Value.robot_forward_backward_speed > 0X84)
{
CurrentMoveState = Move_Backwards; //后退
} }
//--探边置位消除 else if (Current_T_F4_Value.robot_left_right_turn_speed < 0X7A)
Dangerous_Flag_Delete(); {
//-- CurrentMoveState = Move_TurnLeft; //右转
}
else if (Current_T_F4_Value.robot_left_right_turn_speed > 0X84)
{
CurrentMoveState = Move_TurnRight; //左转
}
}
else
{
Off_AutoSpray(Move_HALT);
}
}
/****************************************/ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
//LU_Three控制升降 {
if(Current_T_F4_Value.Lift_Up == 1) //--气压判断
{ Dym_Test();
GF_BSP_GPIO_SetIO(Out_Lift_2,0); //--自动作业模式下的探边检测判断
GF_BSP_GPIO_SetIO(Out_Lift_1,1); Dangerous_Test();
} //--探边置位消除
else if(Current_T_F4_Value.Lift_Down == 1) Dangerous_Flag_Delete();
{
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
}
else
{
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
}
//控制电磁阀喷枪 Robot_Move();
if(Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
{
GF_BSP_GPIO_SetIO(Out_Spray,0);
}
else if(!Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward)
{
GF_BSP_GPIO_SetIO(Out_Spray,1);
}
//--
/****************************************/
//LU_Three控制升降
if(Current_T_F4_Value.Lift_Up == 1)
{
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
}
else if(Current_T_F4_Value.Lift_Down == 1)
{
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
}
else
{
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
}
//控制电磁阀喷枪
if(Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
{
GF_BSP_GPIO_SetIO(Out_Spray,0);
}
else if(!Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward)
{
GF_BSP_GPIO_SetIO(Out_Spray,1);
}
//计算速度档位m/min //计算速度档位m/min
GV.Robot_Speed_mpm = Speed_Judge(&Current_T_F4_Value); GV.Robot_Speed_mpm = Speed_Judge(&Current_T_F4_Value);
@ -547,12 +635,18 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
GV.Right_Compensation = Current_T_F4_Value.robot_Compensation_Right / 51.0; GV.Right_Compensation = Current_T_F4_Value.robot_Compensation_Right / 51.0;
action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态 action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态
// action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState);
// action_perfrom(TiltTransitions, sizeof(TiltTransitions) / sizeof(transition_t), CurrentTiltState);
GV.Robot_State = CurrentMoveState;
if((GV.LeftFrontMotor.ERROR_Flag != 0 || GV.RightFrontMotor.ERROR_Flag != 0)
&& Current_T_F4_Value.emergency_stop != 1)
{
GV.Robot_State = Move_Error;
}
} }
void action_perfrom(transition_t transitions[], int length, int state) void action_perfrom(transition_t transitions[], int length, int state)
{ {

30
Core/Src/robot_state.c
View File

@ -215,6 +215,21 @@ void Auto()
} }
break; break;
case 2: case 2:
//水平
if ((AM_Pa.ExpectationAngle >= 45 && AM_Pa.ExpectationAngle <= 135))
{
AM_Pa.ExpectationAngle = 90 - GV.Right_Compensation;
}
else if ((AM_Pa.ExpectationAngle >= -135 && AM_Pa.ExpectationAngle <= -45))
{
AM_Pa.ExpectationAngle = -90 + GV.Left_Compensation;
}
//垂直
else if ((AM_Pa.ExpectationAngle > -45 && AM_Pa.ExpectationAngle < 45))
{
AM_Pa.ExpectationAngle = 0;
}
VehicleAutoForward(angle, VehicleAutoForward(angle,
GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动前进 GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动前进
break; break;
@ -247,6 +262,21 @@ void Auto()
} }
break; break;
case 4: case 4:
//水平
if ((AM_Pa.ExpectationAngle >= 45 && AM_Pa.ExpectationAngle <= 135))
{
AM_Pa.ExpectationAngle = 90 + GV.Left_Compensation;
}
if ((AM_Pa.ExpectationAngle >= -135 && AM_Pa.ExpectationAngle <= -45))
{
AM_Pa.ExpectationAngle = - 90 - GV.Right_Compensation;
}
//垂直
if ((AM_Pa.ExpectationAngle > -45 && AM_Pa.ExpectationAngle < 45))
{
AM_Pa.ExpectationAngle = 0;
}
VehicleAutoBackward(angle, VehicleAutoBackward(angle,
GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动后退 GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动后退
break; break;

Write Preview
Loading…
Cancel
Save