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
Split 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. 564
      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-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" 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.g++"/>
</provider>
@ -16,7 +16,7 @@
<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.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.g++"/>
</provider>

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

@ -1,27 +1,20 @@
/*
* msp_PID.h
*
* Created on: 2024年1月22日
* Author: Administrator
* msp_PID.h - 两轮差速机器人角度追踪PID（内置参数，数组输出）
*/
#ifndef 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"
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_ */
#endif

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

@ -1,350 +1,151 @@
/*
* msp_PID.c
*
* Created on: 2024年1月18日
* Author: Administrator
* msp_PID.c - 两轮差速机器人角度追踪PID（内置参数，数组输出，float版本）
*/
#include "msp_PID.h"
#include <math.h>
void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T,
#include <float.h>
// ========== 内置固定参数 ==========
#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 Desire_Angle = 0;
double PID_KP = 50;
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,
double *Gain_speed)
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);
// ========== 函数实现 ==========
/* 将角度归一化到 [-180, 180) */
static float normalize_angle(float angle)
{
double Error;
double P_Error;
double D_Error;
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;
while (angle >= 180.0f) angle -= 360.0f;
while (angle < -180.0f) angle += 360.0f;
return angle;
}
double Kp1 = 1;
double Kp2 = 0.950;
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)
/* PD控制器：输入误差，输出控制增量（使用固定 dt = CONTROL_DT） */
static float compute_pd_output(float error,float KP,float KD)
{
// double Desire_Angle=0;
//double Horizontal_Compen_Angle=0;,
double Horizontal_Compen_Angle = 0;
if ((Current_Angle >= 45) & (Current_Angle < 135))
{
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;
float p_term = KP * error;
float d_term = KD * (error - last_error) / CONTROL_DT;
last_error = error;
return p_term + d_term;
}
/* 根据误差绝对值选择增益因子 */
static float get_gain_factor(float abs_error)
{
if (abs_error <= ANGLE_THRESHOLD1) return Kp1;
if (abs_error <= ANGLE_THRESHOLD2) return Kp2;
if (abs_error <= ANGLE_THRESHOLD3) return Kp3;
return Kp4;
}
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 TwoWheel_AngleControl(float current_angle, float desired_angle,
float base_speed,float zoom, float speeds[2])
{
//水平 前进抬车头
double Detal_Angle = fabs(Current_Angle_11 - Desire_Angle_11);
double Incre_Speed[1];
// 1. 计算归一化误差
float raw_error = desired_angle - current_angle;
float error = normalize_angle(raw_error);
float abs_error = fabsf(error);
Speedl_PID(Current_Angle_11, Desire_Angle_11, System_time, Incre_Speed);
Incre_Speed[0] = 1 * Incre_Speed[0];
// 2. PD控制量
float delta = zoom*compute_pd_output(error,PID_KP,PID_KD);
double deltaAngle0 = 1;
double deltaAngle1 = 2;
double deltaAngle2 = 5;
double deltaAngle3 = 10;
//double deltaAngle4=25;
double LeftSpeed_Con_1;
// 3. 根据误差大小选择增益因子
float factor = get_gain_factor(abs_error);
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);
}
// 4. 计算左右轮速度
float left = base_speed + factor * delta;
float right = base_speed - factor * delta;
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;
// 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;
}
W_Speed_1[0] = LeftSpeed_Con_1;
speeds[0] = left;
speeds[1] = right;
}
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 TwoWheel_AngleControl_Weld(float current_angle, float desired_angle,
float base_speed,float zoom, float speeds[2])
{
// double Desire_Angle=0;
//double Horizontal_Compen_Angle=0;
if ((Current_Angle >= 45) & (Current_Angle < 135))
{
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;
if (Desire_Angle > 180)
{
Desire_Angle = Desire_Angle - 360;
}
}
//水平 前进抬车头
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];
}
// 1. 计算归一化误差
float raw_error = desired_angle - current_angle;
float error = normalize_angle(raw_error);
float abs_error = fabsf(error);
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;
// 2. PD控制量
float delta = zoom*compute_pd_output(error,PID_KP_WELD,PID_KD_WELD);
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;
// 3. 根据误差大小选择增益因子
float factor = get_gain_factor(abs_error);
LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor;
LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor;
}
}
// 4. 计算左右轮速度
float left = base_speed + factor * delta;
float right = base_speed - factor * delta;
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;
// 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;
}
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)
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)
{
Desire_Angle = Desire_Angle_Input;
//水平 前进抬车头
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;
Weld_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed);
Incre_Speed[0] = 8 * Incre_Speed[0];
double deltaAngle1 = 0.2;
double deltaAngle2 = 5;
double deltaAngle3 = 10;
//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 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];
}
// if (Detal_Angle >= deltaAngle1)
// {
//
////小角度时，两前轮调整
// }
// 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;
@ -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;
k_1_error = Error;
return (int32_t) (delta);
}
int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP,
int Position_KI, int Position_KD, int MaxValue)
double Weld_KP = 10;
double Weld_KD = 10;
//double Sys_T=0.01;//System time
double Weld_1_error = 0;
void Weld_PID(double CurrentValue, double TargetValue, double Sys_T,
double *Gain_speed)
{
static float Bias, delta_Speed, Integral_bias, Last_Bias;
Bias = CurrentAngle - TargetAngle;
Integral_bias += Bias;
double Error;
double P_Error;
double D_Error;
Error = TargetValue - CurrentValue;
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;
P_Error = Error;
D_Error = Error - Weld_1_error;
Weld_1_error = Error;
Gain_speed[0] = Weld_KP * P_Error + Weld_KD * D_Error / Sys_T;
}
//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;
#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)
{
@ -402,10 +416,7 @@ void auto_forward_state_do(void)
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;
SetMoveMotorSpeed(Ref_Speed);
}
void auto_backward_state_do(void)
@ -420,10 +431,7 @@ void auto_backward_state_do(void)
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;
SetMoveMotorSpeed(-Ref_Speed);
}

Write Preview
Loading…
Cancel
Save