20260609

Core/Src/fsm.c

@@ -25,6 +25,7 @@ int Forward_Flag_Robot_Manual=1;
 //int Markkk = 0;
 int32_t GetLaneChangeDistance();
 int32_t GetVehicleSpeed();
+void Pressure_Adaptive_Function_Uptata2(void);
 void Vertical_Left_Right_Lane_Change_Control(int *LaneChangeCmd,
         int left_or_right);
 void Horizontal_Up_Down_Lane_Change_Control(int *LaneChangeCmd, int up_or_down,
@@ -733,7 +734,7 @@ void Horizontal_Operatin_Main_Func()
 		UltraStopReverse(Forward_Flag_Robot_Auto);
 		Change_Road_Jude_Flag=0;
 		Forward_Flag_Robot_Manual=1;
-		Pressure_Adaptive_Function_Uptata(200);
+		Pressure_Adaptive_Function_Uptata2();
 	}
 	else if(P_MK32->CH4_SA==-1000)//换道
 	{
@@ -764,6 +765,7 @@ void Horizontal_Operatin_Main_Func()
 		PushRod_Contronl();
 		UltraStopReverse_Manually_Backward();
 	}
+
 }
 
 
@@ -920,7 +922,7 @@ void Vertical_Operatin_Main_Func_Left()
 		CRLU_Flag=0;
 		Forward_Flag_Robot_Manual=1;
 		Forward_Flag_Robot_Manual_Count=0;
-		Pressure_Adaptive_Function_Uptata(200);
+		Pressure_Adaptive_Function_Uptata2();
 	}
 	else if(P_MK32->CH4_SA==-1000)//从右向左换道
 	{
@@ -976,7 +978,7 @@ void Vertical_Operatin_Main_Func_Right()
 		CRLU_Flag=0;
 		Forward_Flag_Robot_Manual=1;
 		Forward_Flag_Robot_Manual_Count=0;
-		Pressure_Adaptive_Function_Uptata(200);
+		Pressure_Adaptive_Function_Uptata2();
 	}
 //	else if(P_MK32->CH4_SA==-1000)//从右向左换道
 //	{
@@ -4238,6 +4240,26 @@ void Pressure_Adaptive_Function(double Press_Dis_Value)
 }
 
 
+void Pressure_Adaptive_Function_Uptata2(void)
+{
+//	double Desired_Presss=(double)CV.PV.Robot_Press_Set;
+//	Desired_Presss=-500;
+	//向下压为负值，向上运动为正值
+	double Actual_Value=GV.Force_Value_mN.CMCU_Measuring_value;
+	Desired_Presss=-GV_Robot_Press_Set;
+	D_value=Actual_Value-Desired_Presss;
+
+	if(abs(Actual_Value)>abs(Desired_Presss))
+	{
+		Current_PushRod_STATE = PushRod_HALT;
+	}
+
+	action_perfrom(Set_PushRod_States,
+	        sizeof(Set_PushRod_States) / sizeof(transition_t), Current_PushRod_STATE);
+}
+
+
+
 void Pressure_Adaptive_Function_Uptata(double Press_Dis_Value)
 {
 //	double Desired_Presss=(double)CV.PV.Robot_Press_Set;