305 lines
10 KiB
C++
305 lines
10 KiB
C++
/*
|
|
* @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;
|
|
} |