From 27f6b7dfea626ea047424d5e247174babdad5f42 Mon Sep 17 00:00:00 2001 From: "LIN\\54376" <543769318@qq.com> Date: Fri, 10 Oct 2025 17:10:02 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=EF=BC=9A=E9=85=8D=E5=90=88?= =?UTF-8?q?=E7=95=8C=E9=9D=A2=E5=A2=9E=E5=8A=A0=E7=AB=96=E7=9B=B4=E5=B7=A6?= =?UTF-8?q?=E5=8F=B3=E6=8D=A2=E9=81=93=EF=BC=9B=E6=97=A5=E5=BF=97=EF=BC=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .settings/language.settings.xml | 1 - BASE/Inc/MSP/msp_U7.h | 3 + BASE/Src/BSP/bsp_decode_command.c | 4 +- BASE/Src/MSP/msp_U7.c | 91 ++++++-- Core/Inc/FSM.h | 9 +- Core/Inc/robot_state.h | 8 + Core/Protobuf/PSource/bsp_Cmd.pb.c | 2 +- Core/Protobuf/PSource/bsp_Cmd.pb.h | 13 +- Core/Protobuf/PSource/bsp_Error.pb.h | 12 +- Core/Protobuf/PSource/bsp_GV.pb.h | 2 +- Core/Protobuf/PSource/bsp_IV.pb.h | 17 +- Core/Protobuf/PSource/bsp_PV.pb.h | 11 +- Core/Protobuf/PSource/bsp_ReCmd.pb.c | 2 +- Core/Protobuf/PSource/bsp_ReCmd.pb.h | 13 +- Core/Protobuf/Proto/bsp_GV.proto | 1 - Core/Protobuf/Proto/bsp_IV.proto | 5 +- Core/Protobuf/Proto/bsp_PV.proto | 3 +- Core/Src/FSM.c | 18 +- Core/Src/main.c | 5 +- Core/Src/robot_state.c | 308 ++++++++++++++++----------- 20 files changed, 327 insertions(+), 201 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 19601ec..7c1ad0a 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -16,7 +16,6 @@ - diff --git a/BASE/Inc/MSP/msp_U7.h b/BASE/Inc/MSP/msp_U7.h index 7d27869..dbfbd15 100644 --- a/BASE/Inc/MSP/msp_U7.h +++ b/BASE/Inc/MSP/msp_U7.h @@ -11,6 +11,9 @@ #include "msp_U7.pb.h" void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value); extern SP_MSP_U7_Button *P_U7; +void Robot_ResetCheck_Loop(void); + // extern void U7_Sbus_UART_Handler_intialize(struct UARTHandler* Handler); +uint8_t IsRemoteAtInitialPosition(void); #endif /* INC_MSP_MSP_U7_H_ */ diff --git a/BASE/Src/BSP/bsp_decode_command.c b/BASE/Src/BSP/bsp_decode_command.c index 3e93c4b..a708e45 100644 --- a/BASE/Src/BSP/bsp_decode_command.c +++ b/BASE/Src/BSP/bsp_decode_command.c @@ -269,7 +269,7 @@ void decode_command_and_feedback(uint8_t *buffer, uint16_t length, char isMqtt, pb_istream_t i_cv_stream = { 0 }; - i_cv_stream = pb_istream_from_buffer(decoded_Cmd.Buff_Data, + i_cv_stream = pb_istream_from_buffer(&decoded_Cmd.Buff_Data, decoded_Cmd.Buff_Data_Length); pb_decode(&i_cv_stream, CV_struct_define_fields, &decoded_CV); //将CV写入EEPROM @@ -320,7 +320,7 @@ void WrapInCmdAndSend(ReCmd send_Cmd, uint8_t *buf, char isMqtt, struct UARTHandler *send_Handler) { - memcpy(send_Cmd.Buff_Data, buf, send_Cmd.Buff_Data_Length); + memcpy(&send_Cmd.Buff_Data, buf, send_Cmd.Buff_Data_Length); pb_ostream_t ReCmd_out_stream = { 0 }; diff --git a/BASE/Src/MSP/msp_U7.c b/BASE/Src/MSP/msp_U7.c index dcac2cf..acfa0de 100644 --- a/BASE/Src/MSP/msp_U7.c +++ b/BASE/Src/MSP/msp_U7.c @@ -6,30 +6,66 @@ */ #include "msp_U7.h" #include "DLT/DLTuc.h" - #include "BHBF_ROBOT.h" +#include "fsm.h" +#include "robot_state.h" - +uint8_t Reset_Flag = 0; SP_MSP_U7_Button *P_U7; - struct UARTHandler *U7_Sbus_Controller; void decode_U7Data(uint8_t *buffer, uint16_t length) { - if(length!=25) - { - return; - } - if(buffer[0]==0X0f&&buffer[24]==0X00) - { - Sbus_Data_Count_U7(&buffer[1], (int32_t*) (P_U7)); - - } - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"U7_sbus",1); - + if(length != 25) + { + return; + } + + if(buffer[0] == 0x0F && buffer[24] == 0x00) + { + Sbus_Data_Count_U7(&buffer[1], (int32_t*)(P_U7)); + + // =================== 开机复位检测 =================== + if (Reset_Flag == 0) // 第一次收到数据 + { + Reset_Flag = 1; + + if(!IsRemoteAtInitialPosition()) // 如果未复位 + { + system_mode = SYSTEM_RESET; + CurrentMoveState = Move_Reset; + HALT_State_Do(); // 立即停车并清空状态 + } + else + { + system_mode = SYSTEM_NORMAL; // 正常运行 + } + } + else + { + // =================== 后续检测 =================== + if(system_mode == SYSTEM_RESET) + { + if(IsRemoteAtInitialPosition()) // 按键已复位 + { + system_mode = SYSTEM_NORMAL; + CurrentMoveState = Move_Resetted; + LOG("U7 remote reset recovered, resume NORMAL mode"); + } + else + { + // 还没复位，保持 RESET + HALT_State_Do(); + } + } + } + } + + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "U7_sbus", 1); } + void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value) { @@ -84,10 +120,29 @@ void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value) void U7_Sbus_UART_Handler_intialize(struct UARTHandler* Handler) { - U7_Sbus_Controller = Handler; - U7_Sbus_Controller->UART_Decode=decode_U7Data; + U7_Sbus_Controller = Handler; + U7_Sbus_Controller->UART_Decode = decode_U7Data; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"U7_sbus",0,u7_sbus); - LOG("U7_Sbus_intialize"); + HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "U7_sbus", 0, u7_sbus); + LOG("U7_Sbus_intialize"); } + +uint8_t IsRemoteAtInitialPosition(void) +{ + if(GV.U7_Key.CH0_LYB_V != 0) return 0; + if(GV.U7_Key.CH1_LYB_H != 0) return 0; + if(GV.U7_Key.LU_Single != -1000) return 0; + if(GV.U7_Key.LU_Three != 0) return 0; + if(GV.U7_Key.LU_pulley != 0) return 0; + if(GV.U7_Key.M1 != -1000) return 0; + if(GV.U7_Key.M2 != -1000) return 0; + if(GV.U7_Key.M3 != -1000) return 0; + if(GV.U7_Key.M5 != -1000) return 0; + if(GV.U7_Key.M6 != -1000) return 0; + if(GV.U7_Key.RU_Single != -1000) return 0; + if(GV.U7_Key.RU_Three != 0) return 0; + LOG("U7_At_Initial_Posotion"); + return 1; // 全部归零 +} + diff --git a/Core/Inc/FSM.h b/Core/Inc/FSM.h index 94931ac..51ed106 100644 --- a/Core/Inc/FSM.h +++ b/Core/Inc/FSM.h @@ -55,10 +55,12 @@ typedef enum _MoveSTATE_t Move_ChgFinish, Move_Emergency, + Move_Reset, + Move_Resetted, } MoveSTATE_t; - +extern MoveSTATE_t CurrentMoveState; //设置 换道距离和设置后退距离 typedef enum _SetSTATE_t { @@ -93,11 +95,12 @@ typedef struct _transition_t void (*robotRun)(void); //执行相关动作 } transition_t; - +extern uint8_t Reset_Flag; extern int32_t* RobotSpeed; extern int32_t JontSwingSpeed; extern int32_t JontTiltSpeed; extern int Polish_Flag; -extern int Vacuum_Flag; +//extern int Vacuum_Flag; +extern int Spray_Flag; void Fsm_Init(); #endif /* INC_FSM_H_ */ diff --git a/Core/Inc/robot_state.h b/Core/Inc/robot_state.h index e751456..a3f6b24 100644 --- a/Core/Inc/robot_state.h +++ b/Core/Inc/robot_state.h @@ -25,6 +25,7 @@ typedef struct ChgRoad_Parameters int32_t change_road_distance; //换道时的距离（执行换道前进程序的次数，可以理解为执行次数越多，机器人走的越远） int32_t change_road_status_flag; //换道标志位 int32_t change_road_finish_flag; + int32_t change_road_direction; double speed; /**转弯*/ @@ -54,6 +55,13 @@ typedef struct }AutoMove_Parameters; +typedef enum { + SYSTEM_RESET, + SYSTEM_NORMAL +} SystemMode_t; + +extern SystemMode_t system_mode; + extern ChgRoad_Parameters Chg_Pa; extern int* AuTo_Flag; diff --git a/Core/Protobuf/PSource/bsp_Cmd.pb.c b/Core/Protobuf/PSource/bsp_Cmd.pb.c index 4354a69..64c0db6 100644 --- a/Core/Protobuf/PSource/bsp_Cmd.pb.c +++ b/Core/Protobuf/PSource/bsp_Cmd.pb.c @@ -6,7 +6,7 @@ #error Regenerate this file with the current version of nanopb generator. #endif -PB_BIND(Cmd, Cmd, 2) +PB_BIND(Cmd, Cmd, AUTO) diff --git a/Core/Protobuf/PSource/bsp_Cmd.pb.h b/Core/Protobuf/PSource/bsp_Cmd.pb.h index 602e930..0bdeba6 100644 --- a/Core/Protobuf/PSource/bsp_Cmd.pb.h +++ b/Core/Protobuf/PSource/bsp_Cmd.pb.h @@ -29,7 +29,7 @@ Buff_Data_Length 是下发的字节数， */ int32_t Parameter3; int32_t Parameter4; int32_t Buff_Data_Length; - pb_byte_t Buff_Data[512]; + pb_callback_t Buff_Data; } Cmd; @@ -38,8 +38,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define Cmd_init_default {0, 0, 0, 0, 0, 0, 0, {0}} -#define Cmd_init_zero {0, 0, 0, 0, 0, 0, 0, {0}} +#define Cmd_init_default {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}} +#define Cmd_init_zero {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}} /* Field tags (for use in manual encoding/decoding) */ #define Cmd_CommadNum_tag 1 @@ -60,8 +60,8 @@ X(a, STATIC, SINGULAR, INT32, Parameter2, 4) \ X(a, STATIC, SINGULAR, INT32, Parameter3, 5) \ X(a, STATIC, SINGULAR, INT32, Parameter4, 6) \ X(a, STATIC, SINGULAR, INT32, Buff_Data_Length, 7) \ -X(a, STATIC, SINGULAR, FIXED_LENGTH_BYTES, Buff_Data, 8) -#define Cmd_CALLBACK NULL +X(a, CALLBACK, SINGULAR, BYTES, Buff_Data, 8) +#define Cmd_CALLBACK pb_default_field_callback #define Cmd_DEFAULT NULL extern const pb_msgdesc_t Cmd_msg; @@ -70,8 +70,7 @@ extern const pb_msgdesc_t Cmd_msg; #define Cmd_fields &Cmd_msg /* Maximum encoded size of messages (where known) */ -#define BSP_CMD_PB_H_MAX_SIZE Cmd_size -#define Cmd_size 592 +/* Cmd_size depends on runtime parameters */ #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/bsp_Error.pb.h b/Core/Protobuf/PSource/bsp_Error.pb.h index 07e5e74..d3109e5 100644 --- a/Core/Protobuf/PSource/bsp_Error.pb.h +++ b/Core/Protobuf/PSource/bsp_Error.pb.h @@ -12,7 +12,7 @@ /* Struct definitions */ typedef struct _ErrorDataInfo { int32_t Error_Value; - pb_byte_t Error_Name[50]; + pb_callback_t Error_Name; } ErrorDataInfo; typedef struct _ErrorData { @@ -52,9 +52,9 @@ extern "C" { #endif /* Initializer values for message structs */ -#define ErrorDataInfo_init_default {0, {0}} +#define ErrorDataInfo_init_default {0, {{NULL}, NULL}} #define ErrorData_init_default {0, 0, 0, 0, 0, 0, 0, 0} -#define ErrorDataInfo_init_zero {0, {0}} +#define ErrorDataInfo_init_zero {0, {{NULL}, NULL}} #define ErrorData_init_zero {0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ @@ -72,8 +72,8 @@ extern "C" { /* Struct field encoding specification for nanopb */ #define ErrorDataInfo_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, INT32, Error_Value, 1) \ -X(a, STATIC, SINGULAR, FIXED_LENGTH_BYTES, Error_Name, 4) -#define ErrorDataInfo_CALLBACK NULL +X(a, CALLBACK, SINGULAR, BYTES, Error_Name, 4) +#define ErrorDataInfo_CALLBACK pb_default_field_callback #define ErrorDataInfo_DEFAULT NULL #define ErrorData_FIELDLIST(X, a) \ @@ -96,8 +96,8 @@ extern const pb_msgdesc_t ErrorData_msg; #define ErrorData_fields &ErrorData_msg /* Maximum encoded size of messages (where known) */ +/* ErrorDataInfo_size depends on runtime parameters */ #define BSP_ERROR_PB_H_MAX_SIZE ErrorData_size -#define ErrorDataInfo_size 63 #define ErrorData_size 95 #ifdef __cplusplus diff --git a/Core/Protobuf/PSource/bsp_GV.pb.h b/Core/Protobuf/PSource/bsp_GV.pb.h index 3768f11..693d5e0 100644 --- a/Core/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/Protobuf/PSource/bsp_GV.pb.h @@ -107,7 +107,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 884 +#define GV_struct_define_size 895 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/bsp_IV.pb.h b/Core/Protobuf/PSource/bsp_IV.pb.h index 06c9987..4f06d60 100644 --- a/Core/Protobuf/PSource/bsp_IV.pb.h +++ b/Core/Protobuf/PSource/bsp_IV.pb.h @@ -26,6 +26,9 @@ typedef struct _IV_struct_define { int32_t Robot_Error_Right; int32_t Robot_Compensation_Left; int32_t Robot_Compensation_Right; + int32_t Robot_RESET; + int32_t Robot_LaneChange_Left; + int32_t Robot_LaneChange_Right; } IV_struct_define; @@ -34,8 +37,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define IV_struct_define_Robot_Move_AutoSpeed_tag 1 @@ -52,6 +55,9 @@ extern "C" { #define IV_struct_define_Robot_Error_Right_tag 12 #define IV_struct_define_Robot_Compensation_Left_tag 13 #define IV_struct_define_Robot_Compensation_Right_tag 14 +#define IV_struct_define_Robot_RESET_tag 15 +#define IV_struct_define_Robot_LaneChange_Left_tag 16 +#define IV_struct_define_Robot_LaneChange_Right_tag 17 /* Struct field encoding specification for nanopb */ #define IV_struct_define_FIELDLIST(X, a) \ @@ -68,7 +74,10 @@ X(a, STATIC, SINGULAR, INT32, Robot_Current_Right, 10) \ X(a, STATIC, SINGULAR, INT32, Robot_Error_Left, 11) \ X(a, STATIC, SINGULAR, INT32, Robot_Error_Right, 12) \ X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Left, 13) \ -X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Right, 14) +X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Right, 14) \ +X(a, STATIC, SINGULAR, INT32, Robot_RESET, 15) \ +X(a, STATIC, SINGULAR, INT32, Robot_LaneChange_Left, 16) \ +X(a, STATIC, SINGULAR, INT32, Robot_LaneChange_Right, 17) #define IV_struct_define_CALLBACK NULL #define IV_struct_define_DEFAULT NULL @@ -79,7 +88,7 @@ extern const pb_msgdesc_t IV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_IV_PB_H_MAX_SIZE IV_struct_define_size -#define IV_struct_define_size 154 +#define IV_struct_define_size 189 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/bsp_PV.pb.h b/Core/Protobuf/PSource/bsp_PV.pb.h index ea3c3fc..e1dd02f 100644 --- a/Core/Protobuf/PSource/bsp_PV.pb.h +++ b/Core/Protobuf/PSource/bsp_PV.pb.h @@ -14,6 +14,7 @@ typedef struct _PV_struct_define { int32_t Robot_ChgLength; double Robot_AutoSpeedBase; double Robot_ManualSpeedBase; + int32_t Robot_LaneChange_Direction; } PV_struct_define; @@ -22,19 +23,21 @@ extern "C" { #endif /* Initializer values for message structs */ -#define PV_struct_define_init_default {0, 0, 0} -#define PV_struct_define_init_zero {0, 0, 0} +#define PV_struct_define_init_default {0, 0, 0, 0} +#define PV_struct_define_init_zero {0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define PV_struct_define_Robot_ChgLength_tag 1 #define PV_struct_define_Robot_AutoSpeedBase_tag 2 #define PV_struct_define_Robot_ManualSpeedBase_tag 3 +#define PV_struct_define_Robot_LaneChange_Direction_tag 4 /* Struct field encoding specification for nanopb */ #define PV_struct_define_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, INT32, Robot_ChgLength, 1) \ X(a, STATIC, SINGULAR, DOUBLE, Robot_AutoSpeedBase, 2) \ -X(a, STATIC, SINGULAR, DOUBLE, Robot_ManualSpeedBase, 3) +X(a, STATIC, SINGULAR, DOUBLE, Robot_ManualSpeedBase, 3) \ +X(a, STATIC, SINGULAR, INT32, Robot_LaneChange_Direction, 4) #define PV_struct_define_CALLBACK NULL #define PV_struct_define_DEFAULT NULL @@ -45,7 +48,7 @@ extern const pb_msgdesc_t PV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_PV_PB_H_MAX_SIZE PV_struct_define_size -#define PV_struct_define_size 29 +#define PV_struct_define_size 40 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/bsp_ReCmd.pb.c b/Core/Protobuf/PSource/bsp_ReCmd.pb.c index 073320b..a26702e 100644 --- a/Core/Protobuf/PSource/bsp_ReCmd.pb.c +++ b/Core/Protobuf/PSource/bsp_ReCmd.pb.c @@ -6,7 +6,7 @@ #error Regenerate this file with the current version of nanopb generator. #endif -PB_BIND(ReCmd, ReCmd, 2) +PB_BIND(ReCmd, ReCmd, AUTO) diff --git a/Core/Protobuf/PSource/bsp_ReCmd.pb.h b/Core/Protobuf/PSource/bsp_ReCmd.pb.h index 6d29a7c..843fe38 100644 --- a/Core/Protobuf/PSource/bsp_ReCmd.pb.h +++ b/Core/Protobuf/PSource/bsp_ReCmd.pb.h @@ -29,7 +29,7 @@ Buff_Data_Length 是下发的字节数， */ int32_t Parameter3; int32_t Parameter4; int32_t Buff_Data_Length; - pb_byte_t Buff_Data[500]; + pb_callback_t Buff_Data; } ReCmd; @@ -38,8 +38,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define ReCmd_init_default {0, 0, 0, 0, 0, 0, 0, {0}} -#define ReCmd_init_zero {0, 0, 0, 0, 0, 0, 0, {0}} +#define ReCmd_init_default {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}} +#define ReCmd_init_zero {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}} /* Field tags (for use in manual encoding/decoding) */ #define ReCmd_CommadNum_tag 1 @@ -60,8 +60,8 @@ X(a, STATIC, SINGULAR, INT32, Parameter2, 4) \ X(a, STATIC, SINGULAR, INT32, Parameter3, 5) \ X(a, STATIC, SINGULAR, INT32, Parameter4, 6) \ X(a, STATIC, SINGULAR, INT32, Buff_Data_Length, 7) \ -X(a, STATIC, SINGULAR, FIXED_LENGTH_BYTES, Buff_Data, 8) -#define ReCmd_CALLBACK NULL +X(a, CALLBACK, SINGULAR, BYTES, Buff_Data, 8) +#define ReCmd_CALLBACK pb_default_field_callback #define ReCmd_DEFAULT NULL extern const pb_msgdesc_t ReCmd_msg; @@ -70,8 +70,7 @@ extern const pb_msgdesc_t ReCmd_msg; #define ReCmd_fields &ReCmd_msg /* Maximum encoded size of messages (where known) */ -#define BSP_RECMD_PB_H_MAX_SIZE ReCmd_size -#define ReCmd_size 580 +/* ReCmd_size depends on runtime parameters */ #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/Proto/bsp_GV.proto b/Core/Protobuf/Proto/bsp_GV.proto index b935015..d527ef5 100644 --- a/Core/Protobuf/Proto/bsp_GV.proto +++ b/Core/Protobuf/Proto/bsp_GV.proto @@ -31,7 +31,6 @@ message GV_struct_define int32 Robot_ChgLength =18; int32 Robot_ForceValue = 19; int32 Robot_DynamometerValue = 20; - int32 Emergency = 21; }; diff --git a/Core/Protobuf/Proto/bsp_IV.proto b/Core/Protobuf/Proto/bsp_IV.proto index 5dd44a7..50b5f7c 100644 --- a/Core/Protobuf/Proto/bsp_IV.proto +++ b/Core/Protobuf/Proto/bsp_IV.proto @@ -13,12 +13,9 @@ message IV_struct_define{ int32 Robot_CurrentState= 8; int32 Robot_Current_Left= 9; int32 Robot_Current_Right= 10; - - int32 Robot_Error_Left= 11; int32 Robot_Error_Right= 12; - int32 Robot_Compensation_Left= 13; int32 Robot_Compensation_Right= 14; - + int32 Robot_RESET = 15; }; diff --git a/Core/Protobuf/Proto/bsp_PV.proto b/Core/Protobuf/Proto/bsp_PV.proto index 143b948..a5a2bce 100644 --- a/Core/Protobuf/Proto/bsp_PV.proto +++ b/Core/Protobuf/Proto/bsp_PV.proto @@ -2,8 +2,7 @@ syntax = "proto3"; message PV_struct_define{ int32 Robot_ChgLength= 1; - double Robot_AutoSpeedBase=2; double Robot_ManualSpeedBase=3; - + int32 Robot_LaneChange_Direction = 4; }; diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c index 9c1f108..083c2ec 100644 --- a/Core/Src/FSM.c +++ b/Core/Src/FSM.c @@ -1,16 +1,15 @@ /* * fsm.c - * + *状态机：通过遥控器的指令，判断机器人的运动状态。 * Created on: Oct 18, 2024 * Author: akeguo */ #include "fsm.h" - #include "BHBF_ROBOT.h" #include "bsp_include.h" - #include "robot_state.h" + int32_t *RobotSpeed; //int32_t JontSwingSpeed; //int32_t JontTiltSpeed; @@ -265,13 +264,15 @@ void Front_End() } } - //LU_Three控制打磨推杆升降 + //LU_Three控制打磨推杆升降:下压时，设置安全阈值，确保按压到一定值时，不再继续下压。 + // GV.Robot_ForceValue即APP显示的压力值，设置10，是考虑到按压到一定程度后，开启打磨盘的压力余量 + if(P_U7->LU_Three==1000) { GF_BSP_GPIO_SetIO(Out_Lift_2,0); GF_BSP_GPIO_SetIO(Out_Lift_1,1); } - else if(P_U7->LU_Three==-1000) + else if(P_U7->LU_Three==-1000 && GV.Robot_ForceValue < 10) { GF_BSP_GPIO_SetIO(Out_Lift_2,1); GF_BSP_GPIO_SetIO(Out_Lift_1,0); @@ -328,6 +329,7 @@ void Emergency() { GV.Emergency = 1; Operation_lock = 1; // 急停时自动锁定 + Reset_Flag = 0; } // 2急停锁定计时（仅在急停状态下计数，解决松手不同时问题） @@ -362,6 +364,7 @@ void Emergency() if (P_U7->LU_Three == 0) { Operation_lock = 0; // 满足条件，解锁 + Reset_Flag = 0; } GF_BSP_GPIO_SetIO(0,0); } @@ -386,6 +389,11 @@ void Emergency() void GF_Dispatch()//2ms调用一次 给车体速度等赋值 { + // 如果处于复位模式，直接停车，不再执行运动逻辑 + if (system_mode == SYSTEM_RESET) { + HALT_State_Do(); + return; + } // //运动部分 Move Region //左补偿、右补偿 Lcompensation_control(); diff --git a/Core/Src/main.c b/Core/Src/main.c index cbabd3c..fc67ae8 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -103,7 +103,7 @@ int main(void) /* USER CODE END 1 */ /* MPU Configuration--------------------------------------------------------*/ - MPU_Config(); + MPU_Config(); /* Enable I-Cache---------------------------------------------------------*/ SCB_EnableICache(); @@ -253,12 +253,10 @@ void CV_GV_Init() CV.TiltMoveSpeedBase = 1000; - Motor[1] = &GV.LeftFrontMotor; //Motor[1]指向GV的电机参�??????????? Motor[2] = &GV.RightFrontMotor; - RobotSpeed=&GV.Move_Speed;//RobotSpeed指向GV的移动车体�?�度 这是整体车�?? 不是单个轮的 AuTo_Flag=&GV.AuTo_Flag; @@ -280,7 +278,6 @@ void CV_GV_Init() Motor_ID_Errors[1] = &GV.SystemErrorData.Motor_1_Error; Motor_ID_Errors[2] = &GV.SystemErrorData.Motor_2_Error; - } void GF_Robot_Init() diff --git a/Core/Src/robot_state.c b/Core/Src/robot_state.c index 5412f5d..60308e3 100644 --- a/Core/Src/robot_state.c +++ b/Core/Src/robot_state.c @@ -1,10 +1,11 @@ /* - * robot_state.c + * robot_state.c 手动及自动控制 换道 PID * * Created on: Aug 2, 2024 * Author: akeguo */ #include "robot_state.h" +#include "FSM.h" #include "bsp_GPIO.h" #include #include @@ -12,7 +13,7 @@ int Swing_Count=0; int Home_Flag=0; - +SystemMode_t system_mode = SYSTEM_NORMAL; // 默认开机是正常模式 ChgRoad_Parameters Chg_Pa = { @@ -166,8 +167,9 @@ void HALT_State_Do(void) Chg_Pa.change_road_status_flag = 0; GV.Chg_Flag = 0; GV.LeftFrontMotor.Target_Velcity = 0; - GV.RightFrontMotor.Target_Velcity = 0; +// Polish_Flag = 0; +// Spray_Flag = 0; } @@ -302,7 +304,6 @@ void VehicleAutoForward(double InclinometerAngle, double offsetAngle_left, dou GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed; - // if (deltaAngle >= 0 && deltaAngle <= AM_Pa.deltaAngle1) // { // Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP1 * deltaSpeed; @@ -337,7 +338,6 @@ void VehicleAutoBackward(double InclinometerAngle, double offsetAngle_left, dou } GV.LeftFrontMotor.Target_Velcity = -GV.Robot_AutoSpeed; GV.RightFrontMotor.Target_Velcity = GV.Robot_AutoSpeed; -// GV.LeftFrontMotor.Target_Velcity=GV.LeftFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed; GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed; @@ -383,144 +383,191 @@ void VehicleAutoTurn(double InclinometerAngle, double expectationAngle, double K } - void ChgUp() { - //上换道 - double angle; - angle = GV.Robot_Angle.RF_Angle_Roll / 100.0; - if(Chg_Pa.change_road_finish_flag==0) - { - switch(Chg_Pa.change_road_status_flag) - { - case 0: - - if(angle >= -45 - && (angle <= 45)) - { - Chg_Pa.change_road_status_flag=1; - GV.Chg_Flag = 0; - Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-10; - WheelMotorSpeedInit(); - } - break; - case 1: - VehicleAutoTurn((angle), 90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 - continueTurn_up(90, (angle)); - if (Chg_Pa.isTurn) - { - Chg_Pa.change_road_status_flag=2; - GV.LeftFrontMotor.Target_Velcity=0; - GV.RightFrontMotor.Target_Velcity=0; - Chg_Pa.isTurn=false;//换完置位 + // 上换道 + double angle = GV.Robot_Angle.RF_Angle_Roll / 100.0; + + if (Chg_Pa.change_road_finish_flag == 0) + { + switch (Chg_Pa.change_road_status_flag) + { + case 0: // 初始化换道 + if (angle >= -45 && angle <= 45) + { + Chg_Pa.change_road_status_flag = 1; + GV.Chg_Flag = 0; + Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength - 10; // 使用 PV 配置的换道距离 + WheelMotorSpeedInit(); + } + break; + + case 1: // 上转 + VehicleAutoTurn(angle, 90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转到90°前 + continueTurn_up(90, angle); + + if (Chg_Pa.isTurn) + { + Chg_Pa.change_road_status_flag = 2; + GV.LeftFrontMotor.Target_Velcity = 0; + GV.RightFrontMotor.Target_Velcity = 0; + Chg_Pa.isTurn = false; // 转完置位 + + // 记录电机换道位置，根据左右换道标志调整 + if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道 + { + Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position; + Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth); + Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position; + Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth); + } + else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道 + { + Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position; + Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth); + Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position; + Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth); + } + } + break; + + case 2: // 水平后退 + if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道 + { + GV.LeftFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed); + GV.RightFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed); + } + else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道 + { + GV.LeftFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed); + GV.RightFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed); + } - //记录电机换道位置 - Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position; - Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth); - Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position; - Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth); - } - break; - case 2: //到了90°之后就开始水平后退 - GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed); - GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed); int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position; int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position; - if (m > 0 && n < 0) + if ((GV.PV.Robot_LaneChange_Direction == 0 && m > 0 && n < 0) || + (GV.PV.Robot_LaneChange_Direction == 1 && m < 0 && n > 0)) { - Chg_Pa.change_road_status_flag=3; - Chg_Pa.change_road_straight_count=0; - GV.LeftFrontMotor.Target_Velcity=0; - GV.RightFrontMotor.Target_Velcity=0; - } - break; - case 3: - VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转 - continueTurn_up(0, (angle)); - if (Chg_Pa.isTurn) - { - Chg_Pa.change_road_status_flag=0; - GV.LeftFrontMotor.Target_Velcity=0; - GV.RightFrontMotor.Target_Velcity=0; - Chg_Pa.change_road_finish_flag=1; - GV.Chg_Flag = 1; - Chg_Pa.isTurn=false; - } - break; - default: - break; - } - } - + Chg_Pa.change_road_status_flag = 3; + Chg_Pa.change_road_straight_count = 0; + GV.LeftFrontMotor.Target_Velcity = 0; + GV.RightFrontMotor.Target_Velcity = 0; + } + break; + + case 3: // 转回来 + VehicleAutoTurn(angle, 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转回0°前 + continueTurn_up(0, angle); + + if (Chg_Pa.isTurn) + { + Chg_Pa.change_road_status_flag = 0; + GV.LeftFrontMotor.Target_Velcity = 0; + GV.RightFrontMotor.Target_Velcity = 0; + Chg_Pa.change_road_finish_flag = 1; + GV.Chg_Flag = 1; + Chg_Pa.isTurn = false; + } + break; + + default: + break; + } + } } + void ChgDown() { - //上换道 - double angle; - angle = GV.Robot_Angle.RF_Angle_Roll / 100.0; - if(Chg_Pa.change_road_finish_flag==0) - { - switch(Chg_Pa.change_road_status_flag) - { - case 0: - - if(angle >= -45 - && (angle <= 45)) - { - Chg_Pa.change_road_status_flag=1; - GV.Chg_Flag = 0; - Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-10; - WheelMotorSpeedInit(); - } - break; - case 1: - VehicleAutoTurn((angle), -90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 - continueTurn_up(-90, (angle)); - if (Chg_Pa.isTurn) - { - Chg_Pa.change_road_status_flag=2; - GV.LeftFrontMotor.Target_Velcity=0; - GV.RightFrontMotor.Target_Velcity=0; - Chg_Pa.isTurn=false;//换完置位 + // 下换道 + double angle = GV.Robot_Angle.RF_Angle_Roll / 100.0; + + if (Chg_Pa.change_road_finish_flag == 0) + { + switch (Chg_Pa.change_road_status_flag) + { + case 0: // 初始化换道 + if (angle >= -45 && angle <= 45) + { + Chg_Pa.change_road_status_flag = 1; + GV.Chg_Flag = 0; + Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength - 10; // 使用 PV 配置的换道距离 + WheelMotorSpeedInit(); + } + break; + + case 1: // 下转 + VehicleAutoTurn(angle, -90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转到-90°前 + continueTurn_up(-90, angle); + + if (Chg_Pa.isTurn) + { + Chg_Pa.change_road_status_flag = 2; + GV.LeftFrontMotor.Target_Velcity = 0; + GV.RightFrontMotor.Target_Velcity = 0; + Chg_Pa.isTurn = false; // 转完置位 + + // 记录电机换道位置，根据左右换道标志调整 + if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道 + { + Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position; + Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth); + Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position; + Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth); + } + else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道 + { + Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position; + Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth); + Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position; + Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth); + } + } + break; + + case 2: // 水平后退 + if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道 + { + GV.LeftFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed); + GV.RightFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed); + } + else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道 + { + GV.LeftFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed); + GV.RightFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed); + } - //记录电机换道位置 - Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position; - Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth); - Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position; - Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth); - } - break; - case 2: //到了-90°之后就开始水平前进 - GV.LeftFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed); - GV.RightFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed); int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position; int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position; - if (m < 0 && n > 0) + if ((GV.PV.Robot_LaneChange_Direction == 0 && m > 0 && n < 0) || + (GV.PV.Robot_LaneChange_Direction == 1 && m < 0 && n > 0)) { - Chg_Pa.change_road_status_flag=3; - Chg_Pa.change_road_straight_count=0; - GV.LeftFrontMotor.Target_Velcity=0; - GV.RightFrontMotor.Target_Velcity=0; - } - break; - case 3: - VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转 - continueTurn_up(0, (angle)); - if (Chg_Pa.isTurn) - { - Chg_Pa.change_road_status_flag=0; - GV.LeftFrontMotor.Target_Velcity=0; - GV.RightFrontMotor.Target_Velcity=0; - Chg_Pa.change_road_finish_flag=1; - GV.Chg_Flag = 1; - Chg_Pa.isTurn=false; - } - break; - default: - break; - } - } + Chg_Pa.change_road_status_flag = 3; + Chg_Pa.change_road_straight_count = 0; + GV.LeftFrontMotor.Target_Velcity = 0; + GV.RightFrontMotor.Target_Velcity = 0; + } + break; + + case 3: // 转回来 + VehicleAutoTurn(angle, 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转回0°前 + continueTurn_up(0, angle); + + if (Chg_Pa.isTurn) + { + Chg_Pa.change_road_status_flag = 0; + GV.LeftFrontMotor.Target_Velcity = 0; + GV.RightFrontMotor.Target_Velcity = 0; + Chg_Pa.change_road_finish_flag = 1; + GV.Chg_Flag = 1; + Chg_Pa.isTurn = false; + } + break; + + default: + break; + } + } } @@ -595,6 +642,7 @@ void ChgLeft() } + void ChgRight() { //上换道 @@ -665,10 +713,10 @@ void ChgRight() } } + void ChgFinish() { GV.LeftFrontMotor.Target_Velcity = 0; - GV.RightFrontMotor.Target_Velcity = 0; }