增加自动作业模式；增加安卓app

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/BSP/bsp_client_setting.c

@@ -58,13 +58,6 @@ void UpdateIV()
 
 void decode_received_data_from_client(uint8_t *buffer, uint16_t length)
 {
-	if(length>100)
-	{
-		return;
-	}
-	uint8_t data[100];
-	memcpy(data,buffer,length);
-
 	//if (*buffer == 0x55 && *(buffer + 1) == 0x55 && length >= 4)
 	if (buffer[0] == 0x55 && buffer[1] == 0x55 && length >= 4)
 	{
@@ -84,7 +77,7 @@ void decode_received_data_from_client(uint8_t *buffer, uint16_t length)
 				pb_decode(&i_pv_stream, PV_struct_define_fields, &decoded_PV);
 				//将CV写入EEPROM
 				//CV_struct_define saved_cV = GF_BSP_EEPROM_Get_CV();
-				CV.PV = decoded_PV;
+				GV.PV = decoded_PV;
 				//GF_BSP_EEPROM_Set_CV(CV);
 			}
 			else if (*(buffer + 2) == 0x02 && *(buffer + 3) == 0x01) //设置PV

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_Strong_grinding_machine.c

@@ -36,10 +36,13 @@ const uint8_t strong_grinding_machine_stop_cmd[8] = {0x01, 0x06, 0x00, 0x01, 0x0
 
 static uint8_t strong_grinding_machine_cmd_flag = 0;
 uint8_t strong_grinding_machine_cmd = 0;
-const uint16_t strong_grinding_machine_speed = 20;
+// 测试，转速20rpm
+int32_t strong_grinding_machine_speed = 10;
 
 void Strong_grinding_machine_cmd(void)
 {
+	strong_grinding_machine_speed = GV.PV.main_axis_speed;
+
 	switch(strong_grinding_machine_cmd_flag)
 	{
 		case 0:

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h

@@ -15,6 +15,7 @@ typedef enum _MoveSTATE_t
 	Move_HALT=0,
 	Manual_State,
 	low_Speed_Manual_State,
+	AUTO_FORWARD,
 
 } MoveSTATE_t;
 
@@ -23,6 +24,8 @@ typedef enum _Front_MoveSTATE_t
 	HALT_STATE = 0,
 	Manual_UP_STATE,
 	Manual_DOWN_STATE,
+	MANUAL_LOW_SPEED_UP_STATE,
+	MANUAL_LOW_SPEED_DOWN_STATE
 
 } Front_MoveSTATE_t;
 
@@ -33,6 +36,13 @@ typedef enum _Strong_Grinding_Machine_MoveSTATE_t
 
 } Strong_Grinding_Machine_MoveSTATE_t;
 
+typedef enum _Robot_Operation_Mode_t
+{
+	MANUAL_OPERATION = 1,
+	AUTO_OPERATION,
+
+} Robot_Operation_Mode_t;
+
 typedef struct _transition_t
 {
 	int State; //状态

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h

@@ -18,12 +18,15 @@ extern double Act_Speed;
 extern void Manual_State_Do(void);
 extern void low_Speed_Manual_State_Do(void);
 extern void HALT_State_Do(void);
+extern void auto_forward_state_do(void);
 
 
 //  ********前端升降电机***********
 extern void Manual_Up_State_Do(void);
 extern void Manual_Down_State_Do(void);
 extern void FrontEnd_Halt_State_Do(void);
+extern void Manual_Low_Speed_Up_State_Do(void);
+extern void Manual_low_speed_Down_State_Do(void);
 
 // ********强磨机运动控制***********
 extern void Strong_Grinding_Machine_Halt_State_Do(void);

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_CV.pb.h

@@ -68,7 +68,7 @@ extern const pb_msgdesc_t CV_struct_define_msg;
 
 /* Maximum encoded size of messages (where known) */
 #define BSP_CV_PB_H_MAX_SIZE                     CV_struct_define_size
-#define CV_struct_define_size                    101
+#define CV_struct_define_size                    123
 
 #ifdef __cplusplus
 } /* extern "C" */

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_GV.pb.h

@@ -16,13 +16,14 @@
 #include "bsp_IO.pb.h"
 #include "msp_LS_MotorParameters.pb.h"
 #include "msp_zhr29_200_laser_sensor.pb.h"
+#include "bsp_PV.pb.h"
 
 #if PB_PROTO_HEADER_VERSION != 40
 #error Regenerate this file with the current version of nanopb generator.
 #endif
 
 /* Struct definitions */
-typedef struct _GV_struct_define { /* 洗舱项目 */
+typedef struct _GV_struct_define {
     bool has_LeftFrontMotor;
     MotorParameters LeftFrontMotor;
     bool has_LeftBackMotor;
@@ -46,6 +47,8 @@ typedef struct _GV_struct_define { /* 洗舱项目 */
     LS_MotorParameters LS_FrontEnd_Motor;
     bool has_ZHR29_200_measure_results;
     zhr29_200_struct_define ZHR29_200_measure_results;
+    bool has_PV;
+    PV_struct_define PV;
 } GV_struct_define;
 
 
@@ -54,8 +57,8 @@ extern "C" {
 #endif
 
 /* Initializer values for message structs */
-#define GV_struct_define_init_default            {false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, SP_MSP_MK32_Button_init_default, 0, false, ErrorData_init_default, false, MSP_TL720DParameters_init_default, false, LS_MotorParameters_init_default, false, zhr29_200_struct_define_init_default}
+#define GV_struct_define_init_default            {false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, SP_MSP_MK32_Button_init_default, 0, false, ErrorData_init_default, false, MSP_TL720DParameters_init_default, false, LS_MotorParameters_init_default, false, zhr29_200_struct_define_init_default, false, PV_struct_define_init_default}
-#define GV_struct_define_init_zero               {false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, SP_MSP_MK32_Button_init_zero, 0, false, ErrorData_init_zero, false, MSP_TL720DParameters_init_zero, false, LS_MotorParameters_init_zero, false, zhr29_200_struct_define_init_zero}
+#define GV_struct_define_init_zero               {false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, SP_MSP_MK32_Button_init_zero, 0, false, ErrorData_init_zero, false, MSP_TL720DParameters_init_zero, false, LS_MotorParameters_init_zero, false, zhr29_200_struct_define_init_zero, false, PV_struct_define_init_zero}
 
 /* Field tags (for use in manual encoding/decoding) */
 #define GV_struct_define_LeftFrontMotor_tag      1
@@ -70,6 +73,7 @@ extern "C" {
 #define GV_struct_define_TL720DParameters_tag    11
 #define GV_struct_define_LS_FrontEnd_Motor_tag   12
 #define GV_struct_define_ZHR29_200_measure_results_tag 13
+#define GV_struct_define_PV_tag                  14
 
 /* Struct field encoding specification for nanopb */
 #define GV_struct_define_FIELDLIST(X, a) \
@@ -84,7 +88,8 @@ X(a, STATIC,   SINGULAR, INT32,    Move_Speed,        8) \
 X(a, STATIC,   OPTIONAL, MESSAGE,  SystemErrorData,  10) \
 X(a, STATIC,   OPTIONAL, MESSAGE,  TL720DParameters,  11) \
 X(a, STATIC,   OPTIONAL, MESSAGE,  LS_FrontEnd_Motor,  12) \
-X(a, STATIC,   OPTIONAL, MESSAGE,  ZHR29_200_measure_results,  13)
+X(a, STATIC,   OPTIONAL, MESSAGE,  ZHR29_200_measure_results,  13) \
+X(a, STATIC,   OPTIONAL, MESSAGE,  PV,               14)
 #define GV_struct_define_CALLBACK NULL
 #define GV_struct_define_DEFAULT NULL
 #define GV_struct_define_LeftFrontMotor_MSGTYPE MotorParameters
@@ -98,6 +103,7 @@ X(a, STATIC,   OPTIONAL, MESSAGE,  ZHR29_200_measure_results,  13)
 #define GV_struct_define_TL720DParameters_MSGTYPE MSP_TL720DParameters
 #define GV_struct_define_LS_FrontEnd_Motor_MSGTYPE LS_MotorParameters
 #define GV_struct_define_ZHR29_200_measure_results_MSGTYPE zhr29_200_struct_define
+#define GV_struct_define_PV_MSGTYPE PV_struct_define
 
 extern const pb_msgdesc_t GV_struct_define_msg;
 
@@ -106,7 +112,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                    1634
+#define GV_struct_define_size                    1669
 
 #ifdef __cplusplus
 } /* extern "C" */

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h

@@ -13,6 +13,7 @@
 typedef struct _IV_struct_define {
     double Robot_Move_Speed;
     int32_t RF_Angle_Roll;
+    int32_t Remote_Status;
     int32_t laser_sensor_1_measure_distance;
     int32_t laser_sensor_2_measure_distance;
     int32_t laser_sensor_3_measure_distance;
@@ -24,23 +25,25 @@ extern "C" {
 #endif
 
 /* Initializer values for message structs */
-#define IV_struct_define_init_default            {0, 0, 0, 0, 0}
+#define IV_struct_define_init_default            {0, 0, 0, 0, 0, 0}
-#define IV_struct_define_init_zero               {0, 0, 0, 0, 0}
+#define IV_struct_define_init_zero               {0, 0, 0, 0, 0, 0}
 
 /* Field tags (for use in manual encoding/decoding) */
 #define IV_struct_define_Robot_Move_Speed_tag    1
 #define IV_struct_define_RF_Angle_Roll_tag       2
-#define IV_struct_define_laser_sensor_1_measure_distance_tag 3
+#define IV_struct_define_Remote_Status_tag       3
-#define IV_struct_define_laser_sensor_2_measure_distance_tag 4
+#define IV_struct_define_laser_sensor_1_measure_distance_tag 4
-#define IV_struct_define_laser_sensor_3_measure_distance_tag 5
+#define IV_struct_define_laser_sensor_2_measure_distance_tag 5
+#define IV_struct_define_laser_sensor_3_measure_distance_tag 6
 
 /* Struct field encoding specification for nanopb */
 #define IV_struct_define_FIELDLIST(X, a) \
 X(a, STATIC,   SINGULAR, DOUBLE,   Robot_Move_Speed,   1) \
 X(a, STATIC,   SINGULAR, INT32,    RF_Angle_Roll,     2) \
-X(a, STATIC,   SINGULAR, INT32,    laser_sensor_1_measure_distance,   3) \
+X(a, STATIC,   SINGULAR, INT32,    Remote_Status,     3) \
-X(a, STATIC,   SINGULAR, INT32,    laser_sensor_2_measure_distance,   4) \
+X(a, STATIC,   SINGULAR, INT32,    laser_sensor_1_measure_distance,   4) \
-X(a, STATIC,   SINGULAR, INT32,    laser_sensor_3_measure_distance,   5)
+X(a, STATIC,   SINGULAR, INT32,    laser_sensor_2_measure_distance,   5) \
+X(a, STATIC,   SINGULAR, INT32,    laser_sensor_3_measure_distance,   6)
 #define IV_struct_define_CALLBACK NULL
 #define IV_struct_define_DEFAULT NULL
 
@@ -51,7 +54,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                    53
+#define IV_struct_define_size                    64
 
 #ifdef __cplusplus
 } /* extern "C" */

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_PV.pb.h

@@ -11,7 +11,9 @@
 
 /* Struct definitions */
 typedef struct _PV_struct_define {
+    int32_t robot_operation_mode;
     int32_t knife_descending_height;
+    int32_t main_axis_speed;
 } PV_struct_define;
 
 
@@ -20,15 +22,19 @@ extern "C" {
 #endif
 
 /* Initializer values for message structs */
-#define PV_struct_define_init_default            {0}
+#define PV_struct_define_init_default            {0, 0, 0}
-#define PV_struct_define_init_zero               {0}
+#define PV_struct_define_init_zero               {0, 0, 0}
 
 /* Field tags (for use in manual encoding/decoding) */
-#define PV_struct_define_knife_descending_height_tag 1
+#define PV_struct_define_robot_operation_mode_tag 1
+#define PV_struct_define_knife_descending_height_tag 2
+#define PV_struct_define_main_axis_speed_tag     3
 
 /* Struct field encoding specification for nanopb */
 #define PV_struct_define_FIELDLIST(X, a) \
-X(a, STATIC,   SINGULAR, INT32,    knife_descending_height,   1)
+X(a, STATIC,   SINGULAR, INT32,    robot_operation_mode,   1) \
+X(a, STATIC,   SINGULAR, INT32,    knife_descending_height,   2) \
+X(a, STATIC,   SINGULAR, INT32,    main_axis_speed,   3)
 #define PV_struct_define_CALLBACK NULL
 #define PV_struct_define_DEFAULT NULL
 
@@ -39,7 +45,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                    11
+#define PV_struct_define_size                    33
 
 #ifdef __cplusplus
 } /* extern "C" */

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_GV.proto

@@ -11,11 +11,10 @@ import "bsp_DAM.proto";
 import "bsp_IO.proto";
 import "msp_LS_MotorParameters.proto";
 import "msp_zhr29_200_laser_sensor.proto";
+import "bsp_PV.proto";
 
 message GV_struct_define
 {
-	//洗舱项目
-	
 	MotorParameters LeftFrontMotor=1;
 	MotorParameters LeftBackMotor=2;
 	MotorParameters RightBackMotor=3;
@@ -28,7 +27,7 @@ message GV_struct_define
 	MSP_TL720DParameters TL720DParameters= 11;	
 	LS_MotorParameters LS_FrontEnd_Motor = 12;
 	zhr29_200_struct_define ZHR29_200_measure_results = 13;
-
+	PV_struct_define PV = 14;
  
 };
 

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto

@@ -7,8 +7,9 @@ message IV_struct_define{
 
 	  double Robot_Move_Speed= 1;
 	  int32 RF_Angle_Roll = 2;
-	  int32 laser_sensor_1_measure_distance = 3;
+	  int32 Remote_Status = 3;
-	  int32 laser_sensor_2_measure_distance = 4;
+	  int32 laser_sensor_1_measure_distance = 4;
-	  int32 laser_sensor_3_measure_distance = 5;
+	  int32 laser_sensor_2_measure_distance = 5;
+	  int32 laser_sensor_3_measure_distance = 6;
 	  
 };

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_PV.proto

@@ -1,5 +1,7 @@
 syntax = "proto3";
 message PV_struct_define{
  
-	  int32 knife_descending_height = 1;
+	int32 robot_operation_mode = 1;
+ 	int32 knife_descending_height = 2;
+ 	int32 main_axis_speed = 3;
 };

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c

@@ -35,6 +35,7 @@ transition_t MoveTransitions[] =
 	{ Move_HALT, HALT_State_Do },
 	{ Manual_State, Manual_State_Do },
 	{ low_Speed_Manual_State, low_Speed_Manual_State_Do},
+	{ AUTO_FORWARD, auto_forward_state_do}
 
 };
 
@@ -43,6 +44,8 @@ transition_t FrontendMoveTransitions[] =
 	{ HALT_STATE, FrontEnd_Halt_State_Do},
 	{ Manual_UP_STATE, Manual_Up_State_Do},
 	{ Manual_DOWN_STATE, Manual_Down_State_Do},
+	{ MANUAL_LOW_SPEED_UP_STATE, Manual_Low_Speed_Up_State_Do},
+	{ MANUAL_LOW_SPEED_DOWN_STATE, Manual_low_speed_Down_State_Do},
 
 };
 
@@ -54,8 +57,6 @@ transition_t StrongGrindingMachineTransitions[] =
 };
 
 
-
-
 void Fsm_Init()
 {
 	GF_BSP_Interrupt_Add_CallBack(
@@ -67,16 +68,12 @@ void GF_Dispatch()
 {
 	IV_Control();
 
-	// 机器人本体控制
-	Robot_Control();
-
-	// 前端控制
-	Frontend_Control();
 
-	//强磨机控制
-	Strong_Grinding_Machine_Control();
 
+	Mode_Control();
 
+	// 限位安全检测
+	Limit_Detection();
 
 	action_perfrom(MoveTransitions,sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);
 	action_perfrom(FrontendMoveTransitions,
@@ -85,11 +82,153 @@ void GF_Dispatch()
 						sizeof(StrongGrindingMachineTransitions) / sizeof(transition_t), StrongGrindingMachineCurrentState);
 }
 
+uint8_t upper_limit_position = 0;
+uint8_t lower_limit_position = 0;
+
+void Limit_Detection()
+{
+//	upper_limit_position = GF_BSP_GPIO_ReadIO(2);
+//	lower_limit_position = GF_BSP_GPIO_ReadIO(3);
+}
+
+
+uint8_t Mode_Select_State = MANUAL_OPERATION;
+
+void Mode_Control()
+{
+	Mode_Select_State = GV.PV.robot_operation_mode;
+
+	switch(Mode_Select_State)
+	{
+		case MANUAL_OPERATION:
+			// 机器人本体控制
+			Robot_Control();
+
+			// 前端雷赛电机控制
+			Frontend_Control();
+
+			// 前端强磨机控制
+			Strong_Grinding_Machine_Control();
+			break;
+		case AUTO_OPERATION:
+
+			// 自动作业任务
+			Automatic_Operation();
+
+			break;
+		default:
+			CurrentMoveState = Move_HALT;
+			CurrentFrontEndState = HALT_STATE;
+			StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+			break;
+	}
+
+}
+
+void IO_Control()
+{
+	GF_BSP_GPIO_SetIO(4, 1);
+}
+
+uint8_t IsAllowRotation_AutoMode = 0;
+int32_t knife_descent_height = 0;
+// 自动作业停止后默认上升高度为5mm
+uint8_t knife_rising_height = 5;
+int32_t knife_current_altitude = 0;
+uint16_t delay_counts = 0;
+uint8_t auto_work_mode_states = 5;
+uint8_t auto_work_halt_states = 10;
+
+void Automatic_Operation()
+{
+	knife_descent_height = GV.PV.knife_descending_height;
+
+	if(GV.MK32_Key.CH7_SD == -1000)
+	{
+		switch(auto_work_mode_states)
+		{
+			case 5:
+				knife_current_altitude = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;
+				auto_work_mode_states = 10;
+				break;
+			case 10:
+				CurrentFrontEndState = Manual_DOWN_STATE;
+
+				if(knife_current_altitude - GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance
+						>= knife_descent_height)
+				{
+					CurrentFrontEndState = HALT_STATE;
+					auto_work_halt_states = 10;
+					auto_work_mode_states = 20;
+				}
+				break;
+			case 20:
+				IsAllowRotation_AutoMode = Knife_Detection();
+				if(IsAllowRotation_AutoMode == 1)
+				{
+					StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_MOTION_STATE;
+					delay_counts = delay_counts + 1;
+					if(delay_counts >= 500)
+					{
+						delay_counts = 0;
+						CurrentMoveState = AUTO_FORWARD;
+					}
+				}
+				else{
+					StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+				}
+
+				break;
+			case 30:
+				break;
+			default:
+				CurrentMoveState = Move_HALT;
+				CurrentFrontEndState = HALT_STATE;
+				StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+				break;
+
+		}
+	}
+	else{
+		delay_counts = 0;
+		CurrentMoveState = Move_HALT;
+		StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+
+		if(auto_work_mode_states == 10)
+		{
+			CurrentFrontEndState = HALT_STATE;
+			auto_work_mode_states = 5;
+		}
+		else if(auto_work_mode_states == 20)
+		{
+			switch(auto_work_halt_states)
+			{
+				case 10:
+					knife_current_altitude = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;
+					auto_work_halt_states = 20;
+					break;
+				case 20:
+					CurrentFrontEndState = Manual_UP_STATE;
+
+					if(GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance - knife_current_altitude
+							>= knife_rising_height)
+					{
+						auto_work_mode_states = 5;
+						CurrentFrontEndState = HALT_STATE;
+					}
+					break;
+			}
+		}
+		else{
+			auto_work_mode_states = 5;
+		}
+	}
+}
+
 
 uint8_t IsAllowRotation = 0;
 void Strong_Grinding_Machine_Control()
 {
-	GF_BSP_GPIO_SetIO(4, 1);
 	IsAllowRotation = Knife_Detection();
 	if(IsAllowRotation == 1)
 	{
@@ -99,11 +238,15 @@ void Strong_Grinding_Machine_Control()
 		}
 		else{
 			StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+			// 只有在刀到位且不转的时候和刀未到位的时候可以控制
+			IO_Control();
 		}
 
 	}
 	else{
 		StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+		// 只有在刀到位且不转的时候和刀未到位的时候可以控制
+		IO_Control();
 	}
 
 }
@@ -141,37 +284,81 @@ void Frontend_Control()
 	int angle;
 	angle = atan2(GV.MK32_Key.CH1_RY_V, GV.MK32_Key.CH0_RY_H) * 180 / M_PI;
 
-	// 前进
+	// 正常速度控制前端雷赛电机
-	if((angle >= 45) && (angle <= 135))
+	if(GV.MK32_Key.CH5_SB == 0)
 	{
-		CurrentFrontEndState = Manual_UP_STATE;
+		// 前进
-		return;
+		if((angle >= 45) && (angle <= 135))
-	}
+		{
-	else if(P_MK32->CH3_LY_H > 100)
+			CurrentFrontEndState = Manual_UP_STATE;
-	{
+			return;
-		// 右转
+		}
-		if((angle >= -45) && (angle <=45))
+		else if(P_MK32->CH3_LY_H > 100)
 		{
+			// 右转
+			if((angle >= -45) && (angle <=45))
+			{
 
-		return;
+			return;
+			}
+		}
+		// 后退
+		else if((angle >= -135) && (angle < -45))
+		{
+			CurrentFrontEndState = Manual_DOWN_STATE;
+			return;
+		}
+		// 左转
+		else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179))
+		{
+
+			return;
+		}
+		else {
+			CurrentFrontEndState = HALT_STATE;
 		}
 	}
-	// 后退
+	// 低速控制前端雷赛电机
-	else if((angle >= -135) && (angle < -45))
+	else if(GV.MK32_Key.CH5_SB == -1000)
-	{
-		CurrentFrontEndState = Manual_DOWN_STATE;
-		return;
-	}
-	// 左转
-	else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179))
 	{
+		// 前进
+		if((angle >= 45) && (angle <= 135))
+		{
+			CurrentFrontEndState = MANUAL_LOW_SPEED_UP_STATE;
+			return;
+		}
+		else if(P_MK32->CH3_LY_H > 100)
+		{
+			// 右转
+			if((angle >= -45) && (angle <=45))
+			{
+
+			return;
+			}
+		}
+		// 后退
+		else if((angle >= -135) && (angle < -45))
+		{
+			CurrentFrontEndState = MANUAL_LOW_SPEED_DOWN_STATE;
+			return;
+		}
+		// 左转
+		else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179))
+		{
+
+			return;
+		}
+		else {
+			CurrentFrontEndState = HALT_STATE;
+		}
 
-		return;
 	}
-	else {
+	else{
-		CurrentFrontEndState = HALT_STATE;
+
 	}
 
+
+
 }
 
 
@@ -181,6 +368,7 @@ void IV_Control()
 	real_speed = (Act_Speed*360/100/101/6*3.1415926*0.325);
 	IV.Robot_Move_Speed = real_speed;
 	IV.RF_Angle_Roll = SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll;
+	IV.Remote_Status = GV.MK32_Key.IsOnline;
 	IV.laser_sensor_1_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;
 	IV.laser_sensor_2_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance;
 	IV.laser_sensor_3_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance;

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/main.c

@@ -157,7 +157,7 @@ int main(void)
   GF_BSP_GPIO_SetIO(5,1);
   DLT_LOG_ENABLE_LEVEL=0;//7 send all information  0 send nothing
   Error_Detect_Intialzie(3000);//every 1 seconds
-
+  HAL_Delay(5000);
   GF_Robot_Init();
   GV.Move_Speed=3000;
  // udp_client_init();
@@ -274,6 +274,8 @@ void CV_GV_Init()
 
 	// 强磨机默认发送停止命令
 	strong_grinding_machine_cmd = 0;
+	// 电磁阀默认关闭
+	GF_BSP_GPIO_SetIO(4, 1);
 
 	// �?光测距传感器指针同步
 	g_zhr29_200_laser_sensor_1 = &GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

@@ -221,8 +221,10 @@ int32_t SliderSpeed_mmps_2_pps(int32_t mmps)
 	return mmps * 10000 / 5;
 }
 
-// 定义前端升降电机速度，单位mm/s
+
-static int32_t SliderSpeed = 50;
+
+// 定义前端升降电机速度，单位0.1mm/s
+static const int32_t SliderSpeed = 50;
 const uint8_t up_limit = 215;
 const uint8_t down_limit = 150;
 
@@ -255,6 +257,42 @@ void Manual_Down_State_Do(void)
 
 }
 
+// 定义前端升降电机速度，单位0.1mm/s
+static const uint8_t leisai_low_speed = 10;
+
+void Manual_Low_Speed_Up_State_Do(void)
+{
+	int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(leisai_low_speed);
+	GV.LS_FrontEnd_Motor.Target_Position = -10000000;
+	GV.LS_FrontEnd_Motor.Target_Velcity = SliderSpeed_PPS;
+
+	if((GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance >= up_limit)
+		|| (GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance >= up_limit)
+		|| (GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance >= up_limit))
+	{
+		GV.LS_FrontEnd_Motor.Target_Velcity = 0;
+	}
+}
+
+void Manual_low_speed_Down_State_Do(void)
+{
+	int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(leisai_low_speed);
+	GV.LS_FrontEnd_Motor.Target_Position = 10000000;
+	GV.LS_FrontEnd_Motor.Target_Velcity = SliderSpeed_PPS;
+
+	if((GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance <= down_limit)
+		|| (GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance <= down_limit)
+		|| (GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance <= down_limit))
+	{
+		GV.LS_FrontEnd_Motor.Target_Velcity = 0;
+	}
+
+}
+
+
+
+
+
 void FrontEnd_Halt_State_Do(void)
 {
 	GV.LS_FrontEnd_Motor.Target_Velcity = 0;
@@ -270,8 +308,23 @@ void Strong_Grinding_Machine_Motion_State_Do(void)
 	strong_grinding_machine_cmd = 1;
 }
 
+void auto_forward_state_do(void)
+{
+	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 1000))
+	{
+		speedAdj = -995.0f + (GV.MK32_Key.CH10_LD1 + 1000.0f) * 0.005f;
+		Speed_Ctrl =  ((speedAdj + 1000) * 360 / 1000);
+	}
 
+	Ref_Speed =   Speed_Ctrl;
+	Act_Speed =Speed_Ctrl;
+	if(Ref_Speed >= 5000) Ref_Speed = 5000;
 
+	GV.LeftFrontMotor.Target_Velcity = Ref_Speed;
+	GV.RightFrontMotor.Target_Velcity = -Ref_Speed;
+	GV.LeftBackMotor.Target_Velcity = Ref_Speed;
+	GV.RightBackMotor.Target_Velcity = -Ref_Speed;
+}