123

Core/Inc/left_right_compensation_control.h

@@ -0,0 +1,14 @@
+/*
+ * left_right_compensation_control.h
+ *
+ *  Created on: 2025年9月25日
+ *      Author: 54376
+ */
+
+#ifndef INC_LEFT_RIGHT_COMPENSATION_CONTROL_H_
+#define INC_LEFT_RIGHT_COMPENSATION_CONTROL_H_
+
+extern void Lcompensation_control();
+extern void Rcompensation_control();
+
+#endif /* INC_LEFT_RIGHT_COMPENSATION_CONTROL_H_ */

Core/Src/left_right_compensation_control.c

@@ -0,0 +1,110 @@
+/*
+ * left_right_compensation_control.c
+ *
+ *  Created on: 2025年9月25日
+ *      Author: 54376
+ */
+
+#include "BHBF_ROBOT.h"
+#include "left_right_compensation_control.h"
+#include "msp_U7.h"
+
+/*  左右补偿计时 every 2 miliseconds*/
+bool Left_Time_Start = false;
+bool Right_Time_Start = false;
+#define LeftRightCalbrationTime 100
+
+
+/* 左补偿 */
+void Lcompensation_control()
+{
+	if (abs(P_U7->LU_pulley) <= 100)
+	{
+		Left_Time_Start = false;
+		return;
+	}
+	/***************/
+	if (P_U7->LU_pulley > 100)
+	{
+		if (!Left_Time_Start)
+		{
+			Left_Time_Start = true;
+			timer_handler_3.start_timer = 1; //重新开始计时
+			return;
+		}
+		if (!CompareTimer(LeftRightCalbrationTime, &timer_handler_3)) return;
+
+		GV.Left_Compensation += 10;
+		if (GV.Left_Compensation >= 1000)
+		{
+			GV.Left_Compensation = 1000;
+		}
+		Left_Time_Start = false;
+		return;
+	}
+/***********小于-100*****************/
+	if (!Left_Time_Start)
+	{
+		Left_Time_Start = true;
+		timer_handler_3.start_timer = 1; //重新开始计时
+		return;
+	}
+	if (!CompareTimer(LeftRightCalbrationTime, &timer_handler_3)) return;
+
+	GV.Left_Compensation -= 10;
+	if (GV.Left_Compensation <= -1000)
+	{
+		GV.Left_Compensation = -1000;
+
+	}
+	Left_Time_Start = false;
+	return;
+
+}
+/* 右补偿 */
+void Rcompensation_control()
+{
+
+	/***右补偿****/
+	if (  abs(P_U7->RU_pulley)<= 100  )
+	{
+		Right_Time_Start = false;
+		return;
+	}
+	if(P_U7->RU_pulley<=-100)
+	{
+		if (!Right_Time_Start)
+		{
+			Right_Time_Start = true;
+			timer_handler_4.start_timer = 1; //重新开始计时
+			return;
+		}
+
+		if (!CompareTimer(LeftRightCalbrationTime, &timer_handler_4)) return;
+
+		GV.Right_Compensation += 10;
+
+		if (GV.Right_Compensation >= 1000)
+		{
+			GV.Right_Compensation = 1000;
+		}
+		Right_Time_Start = false;
+		return;
+	}
+	/*********大于100**************/
+	if (!Right_Time_Start)
+	{
+		Right_Time_Start = true;
+		timer_handler_4.start_timer = 1; //重新开始计时
+		return;
+	}
+	if (!CompareTimer(LeftRightCalbrationTime, &timer_handler_4)) return;
+	GV.Right_Compensation -= 10;
+	if (GV.Right_Compensation <= -1000)
+	{
+		GV.Right_Compensation = -1000;
+	}
+	Right_Time_Start = false;
+	return;
+
+}