LiZongdi
/
Huangjiabao_backup
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

【编译通过】增加自动模式下的PID纠偏

master
Lizongdi 1 month ago
parent
96ccad10ec
commit
be55d615b9
4 changed files with 200 additions and 453 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
  2. 35
      diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Inc/MSP/msp_PID.h
  3. 590
      diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_PID.c
  4. 24
      diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

4
diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1256539702914033922" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-63235014752314356" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1256539702914033922" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-63235014752314356" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>

35
diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Inc/MSP/msp_PID.h
View File

@ -1,27 +1,20 @@
/* /*
* msp_PID.h * msp_PID.h - 两轮差速机器人角度追踪PID（内置参数，数组输出）
*
* Created on: 2024年1月22日
* Author: Administrator
*/ */
#ifndef INC_MSP_MSP_PID_H_ #ifndef INC_MSP_MSP_PID_H_
#define INC_MSP_MSP_PID_H_ #define INC_MSP_MSP_PID_H_
/**
* @brief 两轮差速角度控制（内置 dt=0.1s，最大速度 = 1.6 * |base_speed|）
* @param current_angle 当前角度（度）
* @param desired_angle 期望角度（度）
* @param base_speed 基础前进速度（可正可负）
* @param speeds 输出速度数组，speeds[0]=左轮，speeds[1]=右轮
*/
void TwoWheel_AngleControl(float current_angle, float desired_angle,
float base_speed,float zoom, float speeds[2]);
void TwoWheel_AngleControl_Weld(float current_angle, float desired_angle,
float base_speed,float zoom, float speeds[2]);
#include "bsp_include.h" #endif
void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1);
void GF_MSP_Auto_adj_Unicycle(double Current_Angle_11,double Desire_Angle_11, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1);
void GF_MSP_Auto_Motion_adj_Com(double Current_Angle, double Horizontal_Compen_Angle ,double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1);
void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle,double Desire_Angle_Input, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1);
int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP,
int Position_KI, int Position_KD, int MaxValue);
#endif /* INC_MSP_MSP_PID_H_ */

590
diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_PID.c
View File

@ -1,350 +1,151 @@
/* /*
* msp_PID.c * msp_PID.c - 两轮差速机器人角度追踪PID（内置参数，数组输出，float版本）
*
* Created on: 2024年1月18日
* Author: Administrator
*/ */
#include "msp_PID.h" #include "msp_PID.h"
#include <math.h> #include <math.h>
#include <float.h>
void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T,
// ========== 内置固定参数 ==========
#define CONTROL_DT 0.1f // 固定采样时间 0.1秒
#define MAX_SPEED_FACTOR 1.6f // 最大速度 = 基础速度 * 1.6
// ========== 全局可调参数 ==========
float PID_KP = 0.5f; // 比例系数
float PID_KD = 0.012f; // 微分系数
float PID_KP_WELD = 0.1f; // 比例系数
float PID_KD_WELD = 0.025f; // 微分系数
float Kp1 = 1.0f; // 小角度增益因子
float Kp2 = 0.95f; // 中小角度增益因子
float Kp3 = 0.80f; // 中大角度增益因子
float Kp4 = 0.50f; // 大角度增益因子
double Desire_Angle = 0; //用于焊缝跟踪的期望角度
// 误差分段阈值（度）
static const float ANGLE_THRESHOLD1 = 2.0f;
static const float ANGLE_THRESHOLD2 = 5.0f;
static const float ANGLE_THRESHOLD3 = 10.0f;
// ========== 内部静态变量 ==========
static float last_error = 0.0f; // 上一次误差（用于微分）
// ========== 内部函数声明 ==========
static float normalize_angle(float angle);
static float compute_pd_output(float error,float KP,float KD);
static float get_gain_factor(float abs_error);
void Weld_PID(double CurrentValue, double TargetValue, double Sys_T,
double *Gain_speed); double *Gain_speed);
double Desire_Angle = 0; void GF_MSP_PID_Now_Der_adj_Com_Weld(double Current_Angle, double Desire_Angle_Input,
double PID_KP = 50; double Auto_Speed, double Auto_Speed_Max, double System_time, double *W_Speed_1);
double PID_KD = 12;
//double Sys_T=0.01;//System time // ========== 函数实现 ==========
double k_1_error = 0;
void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T, /* 将角度归一化到 [-180, 180) */
double *Gain_speed) static float normalize_angle(float angle)
{ {
double Error; while (angle >= 180.0f) angle -= 360.0f;
double P_Error; while (angle < -180.0f) angle += 360.0f;
double D_Error; return angle;
Error = TargetValue - CurrentValue;
if (Error >= 180)
{
Error = Error - 360;
}
else if (Error <= -180)
{
Error = Error + 360;
}
P_Error = Error;
D_Error = Error - k_1_error;
k_1_error = Error;
Gain_speed[0] = PID_KP * P_Error + PID_KD * D_Error / Sys_T;
} }
double Kp1 = 1; /* PD控制器：输入误差，输出控制增量（使用固定 dt = CONTROL_DT） */
double Kp2 = 0.950; static float compute_pd_output(float error,float KP,float KD)
double Kp3 = 0.80;
double Kp4 = 0.50;
/* Input:
*/
//void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1)
void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed,
double Auto_Speed_Max, double System_time, double *W_Speed_1)
{ {
// double Desire_Angle=0; float p_term = KP * error;
//double Horizontal_Compen_Angle=0;, float d_term = KD * (error - last_error) / CONTROL_DT;
double Horizontal_Compen_Angle = 0; last_error = error;
if ((Current_Angle >= 45) & (Current_Angle < 135)) return p_term + d_term;
{
Desire_Angle = 90 + Horizontal_Compen_Angle;
}
else if ((Current_Angle <= -45) & (Current_Angle >= -135))
{
Desire_Angle = -90 + Horizontal_Compen_Angle;
}
else if ((Current_Angle > -45) & (Current_Angle < 45))
{
Desire_Angle = 0 + Horizontal_Compen_Angle;
}
else
{
Desire_Angle = 180 + Horizontal_Compen_Angle;
}
//水平 前进抬车头
double Detal_Angle = fabs(Current_Angle - Desire_Angle);
double Incre_Speed[1];
Speedl_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed);
Incre_Speed[0] = 1 * Incre_Speed[0];
double deltaAngle1 = 2;
double deltaAngle2 = 5;
double deltaAngle3 = 10;
//double deltaAngle4=25;
double LeftSpeed_Con_1;
double RightSpeed_Con_1;
double LeftSpeed_Con_2;
double RightSpeed_Con_2;
if (Detal_Angle <= deltaAngle1)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0];
//小角度时，两前轮调整
}
else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0];
//中小角度时，两前轮差速，两后轮跟踪调整
}
else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0];
}
else
{
LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0];
}
double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1);
double factor = 0;
if (Jud_value >= 0)
{
if (LeftSpeed_Con_1 > Auto_Speed_Max)
{
factor = Auto_Speed_Max / LeftSpeed_Con_1;
LeftSpeed_Con_1 = Auto_Speed_Max;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
RightSpeed_Con_1 = RightSpeed_Con_1 * factor;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
}
else if (LeftSpeed_Con_1 < -Auto_Speed_Max)
{
factor = -Auto_Speed_Max / LeftSpeed_Con_1;
LeftSpeed_Con_1 = -Auto_Speed_Max;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
RightSpeed_Con_1 = RightSpeed_Con_1 * factor;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
}
}
else
{
if (RightSpeed_Con_1 > Auto_Speed_Max)
{
factor = Auto_Speed_Max / RightSpeed_Con_1;
RightSpeed_Con_1 = Auto_Speed_Max;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
}
else if (LeftSpeed_Con_2 < -Auto_Speed_Max)
{
factor = -Auto_Speed_Max / RightSpeed_Con_1;
RightSpeed_Con_1 = -Auto_Speed_Max;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
}
}
W_Speed_1[0] = LeftSpeed_Con_1;
W_Speed_1[1] = RightSpeed_Con_1;
W_Speed_1[2] = LeftSpeed_Con_2;
W_Speed_1[3] = RightSpeed_Con_2;
} }
void GF_MSP_Auto_adj_Unicycle(double Current_Angle_11, double Desire_Angle_11, /* 根据误差绝对值选择增益因子 */
double Auto_Speed, double Auto_Speed_Max, double System_time, static float get_gain_factor(float abs_error)
double *W_Speed_1)
{ {
//水平 前进抬车头 if (abs_error <= ANGLE_THRESHOLD1) return Kp1;
double Detal_Angle = fabs(Current_Angle_11 - Desire_Angle_11); if (abs_error <= ANGLE_THRESHOLD2) return Kp2;
double Incre_Speed[1]; if (abs_error <= ANGLE_THRESHOLD3) return Kp3;
return Kp4;
Speedl_PID(Current_Angle_11, Desire_Angle_11, System_time, Incre_Speed);
Incre_Speed[0] = 1 * Incre_Speed[0];
double deltaAngle0 = 1;
double deltaAngle1 = 2;
double deltaAngle2 = 5;
double deltaAngle3 = 10;
//double deltaAngle4=25;
double LeftSpeed_Con_1;
if (Detal_Angle <= deltaAngle0)
{
LeftSpeed_Con_1 = 0;
}
else if (Detal_Angle <= deltaAngle1)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0] / 2);
}
else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0] / 2);
}
else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0] / 2);
}
else
{
LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0] / 2);
}
if (LeftSpeed_Con_1 > Auto_Speed_Max)
{
LeftSpeed_Con_1 = Auto_Speed_Max;
}
else if (LeftSpeed_Con_1 < -Auto_Speed_Max)
{
LeftSpeed_Con_1 = -Auto_Speed_Max;
}
else
{
//LeftSpeed_Con_1=LeftSpeed_Con_1;
}
W_Speed_1[0] = LeftSpeed_Con_1;
} }
void GF_MSP_Auto_Motion_adj_Com(double Current_Angle, /* 两轮差速角度控制（对外接口） */
double Horizontal_Compen_Angle, double Auto_Speed, void TwoWheel_AngleControl(float current_angle, float desired_angle,
double Auto_Speed_Max, double System_time, double *W_Speed_1) float base_speed,float zoom, float speeds[2])
{ {
// double Desire_Angle=0; // 1. 计算归一化误差
//double Horizontal_Compen_Angle=0; float raw_error = desired_angle - current_angle;
if ((Current_Angle >= 45) & (Current_Angle < 135)) float error = normalize_angle(raw_error);
{ float abs_error = fabsf(error);
Desire_Angle = 90 + Horizontal_Compen_Angle;
} // 2. PD控制量
else if ((Current_Angle <= -45) & (Current_Angle >= -135)) float delta = zoom*compute_pd_output(error,PID_KP,PID_KD);
{
Desire_Angle = -90 + Horizontal_Compen_Angle; // 3. 根据误差大小选择增益因子
} float factor = get_gain_factor(abs_error);
else if ((Current_Angle > -45) & (Current_Angle < 45))
{ // 4. 计算左右轮速度
Desire_Angle = 0 + Horizontal_Compen_Angle; float left = base_speed + factor * delta;
} float right = base_speed - factor * delta;
else
{ // 5. 内置最大速度 = 1.6 * |base_speed|
Desire_Angle = 180 + Horizontal_Compen_Angle; float max_speed = fabsf(base_speed) * MAX_SPEED_FACTOR;
if (Desire_Angle > 180)
{ // 6. 限幅：等比缩放至最大速度以内
Desire_Angle = Desire_Angle - 360; float max_abs = fmaxf(fabsf(left), fabsf(right));
} if (max_abs > max_speed && max_abs > FLT_MIN) {
} float scale = max_speed / max_abs;
//水平 前进抬车头 left *= scale;
double Detal_Angle = fabs(Current_Angle - Desire_Angle); right *= scale;
double Incre_Speed[1]; }
Speedl_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed);
Incre_Speed[0] = 1 * Incre_Speed[0]; speeds[0] = left;
double deltaAngle1 = 2; speeds[1] = right;
double deltaAngle2 = 5; }
double deltaAngle3 = 10;
//double deltaAngle4=25;
double LeftSpeed_Con_1;
double RightSpeed_Con_1;
double LeftSpeed_Con_2;
double RightSpeed_Con_2;
if (Detal_Angle <= deltaAngle1)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0];
//小角度时，两前轮调整
}
else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0];
//中小角度时，两前轮差速，两后轮跟踪调整
}
else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3)
{
LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0];
}
else
{
LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]);
LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0];
}
double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1);
double factor = 0;
if (Jud_value >= 0)
{
if (LeftSpeed_Con_1 > Auto_Speed_Max)
{
factor = Auto_Speed_Max / LeftSpeed_Con_1;
LeftSpeed_Con_1 = Auto_Speed_Max;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
RightSpeed_Con_1 = RightSpeed_Con_1 * factor;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
}
else if (LeftSpeed_Con_1 < -Auto_Speed_Max)
{
factor = -Auto_Speed_Max / LeftSpeed_Con_1;
LeftSpeed_Con_1 = -Auto_Speed_Max;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
RightSpeed_Con_1 = RightSpeed_Con_1 * factor;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
}
}
else
{
if (RightSpeed_Con_1 > Auto_Speed_Max)
{
factor = Auto_Speed_Max / RightSpeed_Con_1;
RightSpeed_Con_1 = Auto_Speed_Max;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
}
else if (LeftSpeed_Con_2 < -Auto_Speed_Max)
{
factor = -Auto_Speed_Max / RightSpeed_Con_1;
RightSpeed_Con_1 = -Auto_Speed_Max;
RightSpeed_Con_2 = RightSpeed_Con_2 * factor;
LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor; /* 两轮差速角度控制（对外接口） */
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; void TwoWheel_AngleControl_Weld(float current_angle, float desired_angle,
} float base_speed,float zoom, float speeds[2])
} {
// 1. 计算归一化误差
float raw_error = desired_angle - current_angle;
float error = normalize_angle(raw_error);
float abs_error = fabsf(error);
// 2. PD控制量
float delta = zoom*compute_pd_output(error,PID_KP_WELD,PID_KD_WELD);
// 3. 根据误差大小选择增益因子
float factor = get_gain_factor(abs_error);
// 4. 计算左右轮速度
float left = base_speed + factor * delta;
float right = base_speed - factor * delta;
// 5. 内置最大速度 = 1.6 * |base_speed|
float max_speed = fabsf(base_speed) * MAX_SPEED_FACTOR;
// 6. 限幅：等比缩放至最大速度以内
float max_abs = fmaxf(fabsf(left), fabsf(right));
if (max_abs > max_speed && max_abs > FLT_MIN) {
float scale = max_speed / max_abs;
left *= scale;
right *= scale;
}
speeds[0] = left;
speeds[1] = right;
}
W_Speed_1[0] = LeftSpeed_Con_1;
W_Speed_1[1] = RightSpeed_Con_1;
W_Speed_1[2] = LeftSpeed_Con_2;
W_Speed_1[3] = RightSpeed_Con_2;
}
void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input, void GF_MSP_PID_Now_Der_adj_Com_Weld(double Current_Angle, double Desire_Angle_Input, double Auto_Speed, double Auto_Speed_Max, double System_time, double *W_Speed_1)
double Auto_Speed, double Auto_Speed_Max, double System_time,
double *W_Speed_1)
{ {
Desire_Angle = Desire_Angle_Input; Desire_Angle = Desire_Angle_Input;
//水平 前进抬车头 //水平 前进抬车头
double Detal_Angle = fabs(Current_Angle - Desire_Angle); double Detal_Angle = fabs(Current_Angle - Desire_Angle);
double Incre_Speed[1]; double Incre_Speed[1];
Speedl_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed); Weld_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed);
Incre_Speed[0] = 1 * Incre_Speed[0]; Incre_Speed[0] = 8 * Incre_Speed[0];
double deltaAngle1 = 2; double deltaAngle1 = 0.2;
double deltaAngle2 = 5; double deltaAngle2 = 5;
double deltaAngle3 = 10; double deltaAngle3 = 10;
//double deltaAngle4=25; //double deltaAngle4=25;
@ -352,36 +153,38 @@ void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input,
double RightSpeed_Con_1; double RightSpeed_Con_1;
double LeftSpeed_Con_2; double LeftSpeed_Con_2;
double RightSpeed_Con_2; double RightSpeed_Con_2;
if (Detal_Angle <= deltaAngle1)
{ LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]);
LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]); RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]);
RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]); LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0];
LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0]; RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0];
RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0]; // if (Detal_Angle >= deltaAngle1)
//小角度时，两前轮调整 // {
} //
else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2) ////小角度时，两前轮调整
{ // }
LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]); // else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2)
RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]); // {
LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0]; // LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]);
RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0]; // RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]);
//中小角度时，两前轮差速，两后轮跟踪调整 // LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0];
} // RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0];
else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3) ////中小角度时，两前轮差速，两后轮跟踪调整
{ // }
LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]); // else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3)
RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]); // {
LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0]; // LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]);
RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0]; // RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]);
} // LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0];
else // RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0];
{ // }
LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]); // else
RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]); // {
LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0]; // LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]);
RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0]; // RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]);
} // LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0];
// RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0];
// }
double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1); double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1);
double factor = 0; double factor = 0;
@ -433,78 +236,21 @@ void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input,
} }
//喷漆机器人PID算法
int32_t PositionalPID(double pidTargetValue, double pidCurrentValue, double Kp,
double Kd)
{
double Error = 0;
double P_Error = 0;
double I_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; double Weld_KP = 10;
double Weld_KD = 10;
k_1_error = Error; //double Sys_T=0.01;//System time
double Weld_1_error = 0;
return (int32_t) (delta); void Weld_PID(double CurrentValue, double TargetValue, double Sys_T,
} double *Gain_speed)
int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP,
int Position_KI, int Position_KD, int MaxValue)
{ {
static float Bias, delta_Speed, Integral_bias, Last_Bias; double Error;
Bias = CurrentAngle - TargetAngle; double P_Error;
Integral_bias += Bias; double D_Error;
Error = TargetValue - CurrentValue;
delta_Speed = Position_KP * Bias + Position_KI * Integral_bias P_Error = Error;
+ Position_KD * (Bias - Last_Bias), Last_Bias = Bias; D_Error = Error - Weld_1_error;
if (delta_Speed >= MaxValue) Weld_1_error = Error;
{ Gain_speed[0] = Weld_KP * P_Error + Weld_KD * D_Error / Sys_T;
delta_Speed = MaxValue;
}
if (delta_Speed <= -MaxValue)
{
delta_Speed = -MaxValue;
}
return (int) delta_Speed;
} }
//int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP,
// int Position_KI, int Position_KD, int MaxValue)
//{
// static float Bias, delta_Speed, Integral_bias, Last_Bias;
// 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 (int) delta_Speed;
//}

24
diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
View File

@ -20,6 +20,20 @@ double RP_Speed_Ctrl = 0;
float speedAdj = 0; float speedAdj = 0;
#define MOTOR_TO_COUNT 164.433
void SetMoveMotorSpeed(double Speed)
{
double Inspeed = (Speed*360/100/101/6*3.1415926*0.325);//转换为m/min进入PID
float Outspeed[2] = {0};
TwoWheel_AngleControl(GV.TL720DParameters.RF_Angle_Roll, 0, Inspeed, 1, Outspeed);
GV.LeftFrontMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT;
GV.RightFrontMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT;
GV.LeftBackMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT;
GV.RightBackMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT;
}
void Manual_State_Do(void) void Manual_State_Do(void)
{ {
@ -402,10 +416,7 @@ void auto_forward_state_do(void)
Act_Speed =Speed_Ctrl; Act_Speed =Speed_Ctrl;
if(Ref_Speed >= 5000) Ref_Speed = 5000; if(Ref_Speed >= 5000) Ref_Speed = 5000;
GV.LeftFrontMotor.Target_Velcity = Ref_Speed; SetMoveMotorSpeed(Ref_Speed);
GV.RightFrontMotor.Target_Velcity = -Ref_Speed;
GV.LeftBackMotor.Target_Velcity = Ref_Speed;
GV.RightBackMotor.Target_Velcity = -Ref_Speed;
} }
void auto_backward_state_do(void) void auto_backward_state_do(void)
@ -420,10 +431,7 @@ void auto_backward_state_do(void)
Act_Speed =Speed_Ctrl; Act_Speed =Speed_Ctrl;
if(Ref_Speed >= 5000) Ref_Speed = 5000; if(Ref_Speed >= 5000) Ref_Speed = 5000;
GV.LeftFrontMotor.Target_Velcity = -Ref_Speed; SetMoveMotorSpeed(-Ref_Speed);
GV.RightFrontMotor.Target_Velcity = Ref_Speed;
GV.LeftBackMotor.Target_Velcity = -Ref_Speed;
GV.RightBackMotor.Target_Velcity = Ref_Speed;
} }

Write Preview
Loading…
Cancel
Save