PID移植，以及增加自动巡航按键SB

RBcore/BHBF.c

@@ -27,7 +27,8 @@
 /*----------------------------------------------*
  * 外部函数原型说明                             *
  *----------------------------------------------*/
-
+double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
+		double Position_KI, double Position_KD, double MaxValue);
 /*----------------------------------------------*
  * 内部函数原型说明                             *
  *----------------------------------------------*/
@@ -55,13 +56,23 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
         return;
     }
 
-    static int iSpeed = -1;
+    static int iSpeed = -1; // 表示由RD1转换来的速度值
     static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
+    static int g_dletAngle = 0; //PID最终计算补偿值
+    static int32_t g_iPIDAngle = 0; //当前角度
     switch (pstMsg->m_uiMsgID)
     {
-		case COMMON_CMD_STOP_ALL:
+		case RBCORE_CMD_STOP_ALL:
 		{
-    		MsgCenter_SendTo(MODULE_NAME_MOTOR, pstMsg->m_uiMsgID, NULL, 0);
+    		MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
+			break;
+		}
+		case RBCORE_GET_TL720D_ROLL:
+		{
+			if (pstMsg->m_uiDataLen >= sizeof(int32_t))
+			{
+				RD_MEMCPY(&g_iPIDAngle, pstMsg->m_aucData, sizeof(int32_t));
+			}
 			break;
 		}
         case RBCORE_CMD_MANUAL_FORWARD:
@@ -106,17 +117,75 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
 		}
         case RBCORE_CMD_VERTICAL:
 		{
-
-			break;
-		}
-        case RBCORE_CMD_HORIZONTAL_LEFT:
-		{
-
+			int iMode[2] = {0};//iMode[0]正数表示前进，负数表示倒退 iMode[1]是目标角度
+			if (pstMsg->m_uiDataLen >= sizeof(iMode))
+			{
+				RD_MEMCPY(&iMode, pstMsg->m_aucData, sizeof(iMode));
+				if (0 == iMode[0])
+				{
+					break;
+				}
+
+				if (abs(g_iPIDAngle - iMode[1]) <= g_stCV.PID_mid.PID_Angle)
+				{
+					if (abs(g_iPIDAngle - iMode[1]) < g_stCV.PID_low.PID_Angle)
+					{
+						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
+								g_stCV.PID_low.Kp, g_stCV.PID_low.Ki, g_stCV.PID_low.Kd, 10);
+					}
+					else
+					{
+						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
+								g_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10);
+
+					}
+					if (iMode[0] > 0)
+					{
+						aiMotorSpeed[0] = iSpeed - g_dletAngle ;
+						aiMotorSpeed[1] = iSpeed + g_dletAngle ;
+					}
+					else
+					{
+						aiMotorSpeed[0] = -iSpeed - g_dletAngle ;
+						aiMotorSpeed[1] = -iSpeed + g_dletAngle ;
+					}
+				}
+				else
+				{
+					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
+							g_stCV.PID_high.Kp, g_stCV.PID_high.Ki, g_stCV.PID_high.Kd,50);
+					aiMotorSpeed[0] = -g_dletAngle ;
+					aiMotorSpeed[1] = g_dletAngle ;
+				}
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
 			break;
 		}
-        case RBCORE_CMD_HORIZONTAL_RIGHT:
+        case RBCORE_CMD_TURN_ANGLE:
 		{
-
+			int iTargetAngle = 0;
+			if (pstMsg->m_uiDataLen >= sizeof(int))
+			{
+				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
+				if (abs(g_iPIDAngle - iTargetAngle) <= 50)
+				{
+					aiMotorSpeed[0] = 0;
+					aiMotorSpeed[1] = 0;
+				}
+				else if (abs(g_iPIDAngle - iTargetAngle) <= 1000)
+				{
+					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 1, 0,0.5, 10);
+					aiMotorSpeed[0] = -g_dletAngle ;
+					aiMotorSpeed[1] = g_dletAngle;
+				}
+				else
+				{
+					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 2, 0, 0.5, 85);
+					aiMotorSpeed[0] = -g_dletAngle ;
+					aiMotorSpeed[1] = g_dletAngle ;
+				}
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
 			break;
 		}
         case RBCORE_GET_VEHICLE_SPEED:

RBcore/TL720D.c

@@ -125,6 +125,7 @@ static void decode_TL720D(const char *buf, uint32_t _iSize)
 	{
 		uiLastSendTick = uiNowTick;
 		MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t));
+		MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t));
 	}
 }
 

RBcore/include/BHBF.h

@@ -74,24 +74,25 @@ extern "C"{
 
 typedef enum {
 	COMMON_CMD_SHOW_INFO,        // 终端信息展示
-    COMMON_CMD_STOP_ALL,         // 机器人停止
+    COMMON_CMD_STOP_ALL,         // 机器人停止（建议各个控制硬件的模块都要实现）
     MOTOR_START = 0x0100,        // *电机命令开始*
     MOTOR_SET_SPEED,		     // 设置速度
     RBCORE_START = 0x0200,       // *机器人共性命令开始*
 
+	RBCORE_CMD_STOP_ALL,		 // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL
     RBCORE_CMD_MANUAL_FORWARD,   // 机器人手动前进
     RBCORE_CMD_MANUAL_BACKWARD,  // 机器人手动后退
 	RBCORE_CMD_MANUAL_TURNLEFT,  // 机器人手动左转
 	RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转
-	RBCORE_CMD_VERTICAL,  		 // 机器人竖直前进（带PID）
-	RBCORE_CMD_HORIZONTAL_LEFT,  // 机器人水平向左（带PID）
-	RBCORE_CMD_HORIZONTAL_RIGHT, // 机器人水平向右（带PID）
+	RBCORE_CMD_VERTICAL,         // 机器人竖直前进（带PID）
+	RBCORE_CMD_TURN_ANGLE,  	 // 机器人转向目标角度（带PID）
+	RBCORE_GET_TL720D_ROLL,      // 获取陀螺仪横滚角（PID计算）
 	RBCORE_GET_VEHICLE_SPEED,    // 获取机器人速度（内部速度归一化）
 	CUSTOM_START = 0x0300,       // *机器人特性命令开始*
 
 	CUSTOM_GET_PV,				 // 获取PV
 	CUSTOM_GET_MK32,		     // 获取MK32
-	CUSTOM_GET_TL720D_ROLL,		 // 获取陀螺仪横滚角
+	CUSTOM_GET_TL720D_ROLL,		 // 获取陀螺仪横滚角（IV显示）
     CUSTOM_GET_SPEED,            // 获取速度（电机回码）
     CUSTOM_GET_FAULT_CODE,       // 获取错误码（电机回码）
 	CUSTOM_CMD_PAINTGUN,         // 控制喷枪

RBcore/msp_PID.c

@@ -0,0 +1,95 @@
+/*
+ * msp_PID.c
+ *
+ *  Created on: 2024年1月18日
+ *      Author: Administrator
+ */
+
+#include <math.h>
+#include <stdint.h>
+
+void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T,
+        double *Gain_speed);
+double Desire_Angle = 0;
+double PID_KP = 50;
+double PID_KD = 12;
+//double Sys_T=0.01;//System time
+double k_1_error = 0;
+
+
+/* PID
+ * CurrentAngle :0.01度
+ * TargetAngle	:0.01度
+ * MaxValue	:0.1rpm
+ * */
+double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
+		double Position_KI, double Position_KD, double MaxValue)
+{
+	static double Bias, delta_Speed, Integral_bias, Last_Bias;
+	/*
+	 if (TargetValue - CurrentValue >= 180)
+	 {
+		 Bias = (TargetValue - CurrentValue) - 360;
+	 }
+	 if (pidTargetValue - pidCurrentValue <= -180)
+	 {
+		 Bias = TargetValue - CurrentValue + 360;
+	 }
+	*/
+
+	Bias = CurrentAngle - TargetAngle;
+	Integral_bias += Bias;
+
+
+	delta_Speed = Position_KP * Bias + Position_KI * Integral_bias
+	        + Position_KD * (Bias - Last_Bias);
+	Last_Bias = Bias;
+	if (delta_Speed >= MaxValue)
+	{
+		delta_Speed = MaxValue;
+	}
+	if (delta_Speed <= -MaxValue)
+	{
+		delta_Speed = -MaxValue;
+	}
+
+	return  delta_Speed;
+}
+
+
+
+//喷漆机器人PID算法
+int32_t PositionalPID(double pidTargetValue, double pidCurrentValue, double Kp,
+        double Kd)
+{
+	double Error = 0;
+	double P_Error = 0;
+	double D_Error = 0;
+	double delta = 0;
+
+	Error = pidTargetValue - pidCurrentValue;
+	//
+	if (pidTargetValue - pidCurrentValue >= 180)
+	{
+		Error = (pidTargetValue - pidCurrentValue) - 360;
+	}
+	if (pidTargetValue - pidCurrentValue <= -180)
+	{
+		Error = pidTargetValue - pidCurrentValue + 360;
+	}
+	//计算误差
+	P_Error = Error;                                            //比例环节
+
+	D_Error = Error - k_1_error;          //微分环节
+
+	//deltaSpeed = Kp * P_Error + Ki * I_Error + Kd * D_Error;      //计算Speed输出值
+
+	delta = Kp * P_Error + Kd * D_Error;
+
+	k_1_error = Error;
+
+	return (int32_t) (delta);
+}
+
+
+

project/paint_robot_new/paint_robot_new.c

@@ -143,9 +143,20 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int));
 			}
 
+			if (g_stMK32.CH5_SB == -1000)
+			{
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
+				break;
+			}
+			else if(g_stMK32.CH5_SB == 1000)
+			{
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
+				break;
+			}
+
 			if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, COMMON_CMD_STOP_ALL, NULL, 0);
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
 				break;
 			}
 
@@ -169,7 +180,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			}
 			else
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, COMMON_CMD_STOP_ALL, NULL, 0);
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
 			}
 			break;
 		}