zjk/AUV_150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init

Browse Source
This commit is contained in:
zjk
2025-06-16 11:36:21 +08:00
parent 2e9214ee4e
commit f7c8f108b3
18 changed files with 2280 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

305
src/pMotionControler/pidControl.cpp Normal file
View File

@@ -0,0 +1,305 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 15:14:15
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-03 11:30:38
* @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include"pidControl.hpp"
pidControl::pidControl()
{
pidClear(pid_depth);
pidClear(pid_heading);
pidClear(pid_pitch);
pidClear(pid_speed);
setCurDepth(0,0);
setDesDepth(0,0);
setCurHeading(0,0);
setDesHeading(0,0);
setCurPitch(0,0);
setDesPitch(0,0);
setCurSpeed(0,0);
setDesSpeed(0,0);
const_thrust = 0;
}
int pidControl::step()
{
if(!setDesiredValues())
{
//这里不对时间进行重置，保持故障状态
pidClear(pid_depth);
pidClear(pid_heading);
pidClear(pid_pitch);
pidClear(pid_speed);
return -1; //Falut
}
if(has_override)
{
//由手动移交的控制权，需要对时间进行重置，防止进入故障状态
pidClear(pid_depth);
pidClear(pid_heading);
pidClear(pid_pitch);
pidClear(pid_speed);
setCurDepth(0,0);
setDesDepth(0,0);
setCurHeading(0,0);
setDesHeading(0,0);
setCurPitch(0,0);
setDesPitch(0,0);
setCurSpeed(0,0);
setDesSpeed(0,0);
RepList["Waring"] = int(has_override);
return 1; //Ready
}
RepList["Waring"] = int(has_override);
string rpt = "";
if(has_speedCtrl)
{
if(const_thrust == 0)
{
double speed_error = desired_speed.value - current_speed.value;
desired_thrust.value = pidStep(speed_error, pid_speed);
desired_thrust.time = current_time;
//Limit(desired_thrust.value, max_thrust, 0);
rpt = "PID_SPEED: ";
rpt += " (Want):" + doubleToString(desired_speed.value);
rpt += " (Curr):" + doubleToString(current_speed.value);
rpt += " (Diff):" + doubleToString(speed_error);
rpt += " (Delt):" + doubleToString(pid_speed.delta_output);
rpt += " THRUST:" + doubleToString(desired_thrust.value);
addPosting("PID_SPD_DEBUG", rpt);
RepList["spd_pid"]["Want"] = doubleToString(desired_speed.value,2);
RepList["spd_pid"]["Curr"] = doubleToString(current_speed.value,2);
RepList["spd_pid"]["Diff"] = doubleToString(speed_error,2);
RepList["spd_pid"]["Delt"] = doubleToString(pid_speed.delta_output,2);
RepList["spd_pid"]["THRUST"] = doubleToString(desired_thrust.value,2);
}
else
{
desired_thrust.value = const_thrust;
desired_thrust.time = current_time;
}
}
if(desired_thrust.value <= dead_zone)
{
desired_rudder.value = 0;
desired_rudder.time = current_time;
desired_elevator.value = 0;
desired_elevator.time = current_time;
return true;
}
if(has_headCtrl)
{
double heading_error = desired_heading.value - current_heading.value;
heading_error = angle180(heading_error);
desired_rudder.value = pidStep(heading_error, pid_heading);
desired_rudder.time = current_time;
//Limit(desired_rudder.value, max_rudder, -max_rudder);
// Limit();
rpt = "PID_COURSE: ";
rpt += " (Want):" + doubleToString(desired_heading.value);
rpt += " (Curr):" + doubleToString(current_heading.value);
rpt += " (Diff):" + doubleToString(heading_error);
rpt += " (Delt):" + doubleToString(pid_heading.delta_output);
rpt += " RUDDER:" + doubleToString(desired_rudder.value);
addPosting("PID_HDG_DEBUG", rpt);
RepList["hdg_pid"]["Want"] = doubleToString(desired_heading.value,2);
RepList["hdg_pid"]["Curr"] = doubleToString(current_heading.value,2);
RepList["hdg_pid"]["Diff"] = doubleToString(heading_error,2);
RepList["hdg_pid"]["Delt"] = doubleToString(pid_heading.delta_output,2);
RepList["hdg_pid"]["RUDDER"] = doubleToString(desired_rudder.value,2);
}
if(has_depthCtrl)
{
double depth_error = desired_depth.value - current_depth.value;
desired_pitch.value = -pidStep(depth_error, pid_depth);
desired_pitch.time = current_time;
//Limit(desired_pitch.value, max_pitch, -max_pitch);
double pitch_error = desired_pitch.value - current_pitch.value;
desired_elevator.value = pidStep(pitch_error, pid_pitch);
desired_elevator.time = current_time;
//Limit(desired_elevator.value, max_elevator, -max_elevator);
rpt = "PID_DEPTH: ";
rpt += " (Want):" + doubleToString(desired_depth.value);
rpt += " (Curr):" + doubleToString(current_depth.value);
rpt += " (Diff):" + doubleToString(depth_error);
rpt += " ELEVATOR:" + doubleToString(desired_elevator.value);
addPosting("PID_DEP_DEBUG", rpt);
RepList["dep_pid"]["Want"] = doubleToString(desired_depth.value,2);
RepList["dep_pid"]["Curr"] = doubleToString(current_depth.value,2);
RepList["dep_pid"]["Diff"] = doubleToString(depth_error,2);
RepList["dep_pid"]["Delt"] = doubleToString(pid_depth.delta_output,2);
RepList["pth_pid"]["Want"] = doubleToString(desired_pitch.value,2);
RepList["pth_pid"]["Curr"] = doubleToString(current_pitch.value,2);
RepList["pth_pid"]["Diff"] = doubleToString(pitch_error,2);
RepList["pth_pid"]["Delt"] = doubleToString(pid_pitch.delta_output,2);
RepList["pth_pid"]["ELEVATOR"] = doubleToString(desired_elevator.value,2);
}
// Limit();
return 0;
}
bool pidControl::setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid)
{
// pid.kp = p + i + d;
// pid.ki = -p - 2*d;
// pid.kd = d;
pid.kp = p;
pid.ki = i;
pid.kd = d;
pid.limit_delta = limitDelta;
pid.output = 0;
pid.max_out = max;
pid.min_out = min;
return true;
}
double pidControl::pidStep(double error, pidInc &pid)
{
double deltaOutPut = 0;
pid.error_0 = error;
deltaOutPut
= pid.kp * (pid.error_0 - pid.error_1 )
+ pid.ki * (pid.error_0 )
+ pid.kd * (pid.error_0 -2.0*pid.error_1 + pid.error_2);
// RepList["W"]["d"] = deltaOutPut;
// RepList["W"]["e1"] = pid.kp;
// RepList["W"]["e2"] = pid.ki;
// RepList["W"]["e3"] = pid.kd;
Limit(deltaOutPut, pid.limit_delta, -pid.limit_delta);
pid.output += deltaOutPut;
Limit(pid.output, pid.max_out, pid.min_out);
pid.delta_output = deltaOutPut;
pid.error_2 = pid.error_1;
pid.error_1 = pid.error_0;
return pid.output;
}
// double pidControl::picStep_p(double error, pidInc &pid)
// {
// pid.delta_output =
// }
bool pidControl::overrived(string svar)
{
Controler::overrived(svar);
return true;
}
//TODO: 快速检测当前参数是具有与当前控制器相关
bool pidControl::hasConfigSettings() const
{
if(config_params.size() == 0)
return(false);
list<string>::const_iterator p;
for(p=config_params.begin(); p!=config_params.end(); p++)
{
string line = *p;
string param = tolower(biteStringX(line, '='));
// if(param == "yaw_pid_kp")
// return(true);
// else if(param == "yaw_pid_kd")
// return(true);
// else if(param == "yaw_pid_ki")
// return(true);
// else if(param == "yaw_pid_integral_limit")
// return(true);
// else if(param == "yaw_pid_ki_limit")
// return(true);
return true;
}
return(false);
}
bool pidControl::setParam(char n, double param, pidInc *pid_t)
{
if(pid_t == NULL)
return false;
switch (n)
{
case 'p':
pid_t->kp = param;
break;
case 'i':
pid_t->ki = param;
break;
case 'd':
pid_t->kd = param;
break;
default:
return false;
break;
}
return true;
}
int pidControl::setParam(string filePath)
{
Json::Value paramList = getConfig(filePath);
if(paramList.empty())
return 1;
if(paramList["speed"].empty() || paramList["pitch"].empty()
|| paramList["depth"].empty() || paramList["pitch"].empty()
|| paramList["speedCol"].empty() || paramList["depthCol"].empty()
|| paramList["HeadingCol"].empty())
return 2; //重要参数不全
Json::Value param = paramList["speed"];
if(!setParam(param, pid_speed))
return 3;
RepList["speed-param"] = param;
param = paramList["heading"];
if(!setParam(param, pid_heading))
return 4;
RepList["heading-param"] = param;
param = paramList["pitch"];
if(!setParam(param, pid_pitch))
return 5;
RepList["pitch-param"] = param;
param = paramList["depth"];
if(!setParam(param, pid_depth))
return 6;
RepList["depth-param"] = param;
setSpeedControl(paramList["speedCol"].asBool());
setDepthControl(paramList["depthCol"].asBool());
setHeadingControl(paramList["HeadingCol"].asBool());
//次要参数
if(!paramList["dead_zone"].empty())
{
dead_zone = paramList["dead_zone"].asDouble();
RepList["dead_zone"] = dead_zone;
}
if(!paramList["const_thrust"].empty())
{
const_thrust = paramList["const_thrust"].asDouble();
RepList["const_thrust"] = const_thrust;
}
return 0;
}
bool pidControl::setParam(Json::Value param, pidInc &pid)
{
if(param.empty())
return false;
if(param["Kp"].empty() || param["Ki"].empty() || param["Kd"].empty())
return false;
pid.kp = param["Kp"].asDouble();
pid.ki = param["Ki"].asDouble();
pid.kd = param["Kd"].asDouble();
if(param["LimitDelta"].empty() || param["MaxOut"].empty() || param["MinOut"].empty())
return false;
pid.limit_delta = param["LimitDelta"].asDouble();
pid.max_out = param["MaxOut"].asDouble();
pid.min_out = param["MinOut"].asDouble();
return true;
}