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