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

2
CMakeLists.txt
View File

@@ -10,7 +10,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
PROJECT( IVP_EXTEND )
set (CMAKE_CXX_STANDARD 11)
set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp")
set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/software/moos-ivp-main")
#=======================================================================
# Set the output directories for the binary and library files

43
missions/alder/ControlParam.json Normal file
View File

@@ -0,0 +1,43 @@
{
"speed" :
{
"Kp" : 10.0,
"Ki" : 5.0,
"Kd" : 0.0,
"LimitDelta" : 50,
"MaxOut" : 100,
"MinOut" : 0
},
"heading" :
{
"Kp" : 10.0,
"Ki" : 0.5,
"Kd" : 2.2,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"depth" :
{
"Kp" : 10.0,
"Ki" : 0.3,
"Kd" : 2.0,
"LimitDelta" : 5,
"MaxOut" : 10,
"MinOut" : -10
},
"pitch" :
{
"Kp" : 0.6,
"Ki" : 0.03,
"Kd" : 1.5,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"const_thrust" : 0,
"dead_zone" : 10,
"speedCol" : true,
"depthCol" : true,
"HeadingCol" : true
}

35
missions/alder/alder.moos
View File

@@ -16,11 +16,12 @@ ProcessConfig = ANTLER
Run = MOOSDB @ NewConsole = false
//Run = uSimMarineV22 @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pMarinePIDV22 @ NewConsole = false
//Run = pMarinePIDV22 @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pAUV150 @ NewConsole = false
Run = pAUV150 @ NewConsole = true
Run = pMotionControler @ NewConsole = false
}
//------------------------------------------
@@ -80,17 +81,17 @@ ProcessConfig = pHelmIvP
ProcessConfig = pMarinePIDV22
{
AppTick = 5
CommsTick = 5
AppTick = 10
CommsTick = 10
VERBOSE = true
DEPTH_CONTROL = false
// Yaw PID controller
YAW_PID_KP = 0.5
YAW_PID_KD = 0.0
YAW_PID_KI = 0.0
YAW_PID_INTEGRAL_LIMIT = 0.07
YAW_PID_KP = 1
YAW_PID_KD = 1
YAW_PID_KI = 1
YAW_PID_INTEGRAL_LIMIT = 1
// Speed PID controller
SPEED_PID_KP = 1.0
@@ -99,8 +100,8 @@ ProcessConfig = pMarinePIDV22
SPEED_PID_INTEGRAL_LIMIT = 0.07
//MAXIMUMS
MAXRUDDER = 100
MAXTHRUST = 100
MAXRUDDER = 35
MAXTHRUST = 35
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
@@ -157,13 +158,21 @@ ProcessConfig = pNodeReporter
ProcessConfig = pAUV150
{
AppTick = 4
CommsTick = 4
AppTick = 10
CommsTick = 10
server_host = 10.127.0.18
//server_host = 10.127.0.18
server_host = 127.0.0.1
LatOrigin = 43.825300
LongOrigin = -70.330400
ControlFrequency = 5
}
ProcessConfig = pMotionControler
{
AppTick = 5
CommsTick = 5
config_file = /home/zjk/project/prj_015/moos-ivp-extend/missions/alder/ControlParam.json
}

1
src/CMakeLists.txt
View File

@@ -19,6 +19,7 @@ ADD_SUBDIRECTORY(lib_behaviors-test)
ADD_SUBDIRECTORY(pExampleApp)
ADD_SUBDIRECTORY(pXRelayTest)
add_subdirectory(pAUV150)
# add_subdirectory(pMotionControler)
##############################################################################
# END of CMakeLists.txt
##############################################################################

58
src/pAUV150/AUV150.cpp
View File

@@ -13,7 +13,7 @@
#include <string>
#include <iostream>
// #define DEBUG_
#define DEBUG_
using namespace std;
@@ -89,14 +89,14 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
cout << "great!";
else if(key == "DESIRED_RUDDER")
{
m_CommandFrame.rudderUp = msg.GetDouble();
m_CommandFrame.rudderDown = msg.GetDouble();
m_CommandFrame.rudderUp = msg.GetDouble() + 30.0f;
m_CommandFrame.rudderDown = msg.GetDouble() + 30.0f;
m_desiredVarTime = msg.GetTime();
}
else if(key == "DESIRED_ELEVATOR")
{
m_CommandFrame.rudderLeft = msg.GetDouble();
m_CommandFrame.rudderRight = msg.GetDouble();
m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
}
else if(key == "DESIRED_THRUST")
{
@@ -205,7 +205,6 @@ void AUV150::registerVariables()
Register("DESIRED_RUDDER",0);
Register("DESIRED_THRUST",0);
Register("DESIRED_ELEVATOR",0);
// Register("FOOBAR", 0);
}
@@ -299,17 +298,16 @@ bool AUV150::WriteLoop()
double start_time = MOOSTime();
int message_count = 0;
double last_send_time = MOOSTime();
double gap;
//5Hz发送
while(!WritingThread_.IsQuitRequested()){
if(MOOSTime() - last_send_time >= m_controlCycle)
if((MOOSTime() - last_send_time) >= (m_controlCycle))
{
try
{
m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
m_controlTime = MOOSTime();
m_controlGap = m_controlTime - m_desiredVarTime;
m_controlGap = MOOSTime() - m_desiredVarTime;
m_bConnected = true;
}
catch(XPCException &e)
@@ -488,9 +486,14 @@ bool AUV150::buildReport()
// =============== Propulsion System ===============
printTable(
{"Thruster RPM"},
{"Thruster RPM", "Thruster CMD" ,"up", "down", "left", "right"},
{
doubleToString(m_status.thrusterRPM, 1) + "rpm",
doubleToString(m_CommandFrame.mainThruster, 1) + "%",
doubleToString(m_CommandFrame.rudderUp-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderDown-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderLeft-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderRight-30.0f, 1) + "deg",
}
);
// m_msgs << "---------------------------------\n";
@@ -527,25 +530,26 @@ bool AUV150::buildReport()
bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
{
m_status.navModeFb = feedbackFrame.navModeFb;
m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0;
m_status.depthSensor = feedbackFrame.depthSensor / 100.0;
m_status.trueHeading = feedbackFrame.trueHeading / 100.0;
m_status.pitch = feedbackFrame.pitch / 100.0;
m_status.roll = feedbackFrame.roll / 100.0;
m_status.velocityEast = feedbackFrame.velocityEast / 100.0;
m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0;
m_status.velocityDown = feedbackFrame.velocityDown / 100.0;
m_status.insLongitude = feedbackFrame.insLongitude * 360.0 / (1ULL << 32);
m_status.insLatitude = feedbackFrame.insLatitude * 180.0 / (1ULL << 32) - 90.0;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0;
m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0;
m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0f;
m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
m_status.trueHeading = feedbackFrame.trueHeading / 100.0f;
m_status.pitch = feedbackFrame.pitch / 100.0f;
m_status.roll = feedbackFrame.roll / 100.0f;
m_status.velocityEast = feedbackFrame.velocityEast / 100.0f;
m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f;
m_status.velocityDown = feedbackFrame.velocityDown / 100.0f;
m_status.insLongitude = feedbackFrame.insLongitude / 100000.0f - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 100000.0f - 90.0f;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;
m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0f;
m_status.thrusterRPM = feedbackFrame.thrusterRPM;
m_status.ledSwitch = feedbackFrame.ledSwitch;
m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0;
m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0f;
m_status.batteryLevel = feedbackFrame.batteryLevel;
m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0;
m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0f;
m_status.leakStatus = feedbackFrame.leakStatus;
m_status.powerModule = feedbackFrame.powerModule;
m_status.backupPower = feedbackFrame.backupPower;

22
src/pMotionControler/CMakeLists.txt Normal file
View File

@@ -0,0 +1,22 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pTaskManger
# Author(s): zjk
#--------------------------------------------------------
FILE(GLOB SRC *.cpp *.hpp)
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
ADD_EXECUTABLE(pMotionControler ${SRC})
#需要把被依赖的库放到依赖库的后面
TARGET_LINK_LIBRARIES(pMotionControler
${MOOS_LIBRARIES}
geometry
mbutil
m
pthread
jsoncpp
)

43
src/pMotionControler/ControlParam.json Normal file
View File

@@ -0,0 +1,43 @@
{
"speed" :
{
"Kp" : 10.0,
"Ki" : 5.0,
"Kd" : 0.0,
"LimitDelta" : 50,
"MaxOut" : 100,
"MinOut" : 0
},
"heading" :
{
"Kp" : 0.8,
"Ki" : 0.05,
"Kd" : 2.2,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"depth" :
{
"Kp" : 10.0,
"Ki" : 0.3,
"Kd" : 2.0,
"LimitDelta" : 5,
"MaxOut" : 10,
"MinOut" : -10
},
"pitch" :
{
"Kp" : 0.6,
"Ki" : 0.03,
"Kd" : 1.5,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"const_thrust" : 0,
"dead_zone" : 10,
"speedCol" : true,
"depthCol" : true,
"HeadingCol" : true
}

424
src/pMotionControler/Controler.cpp Normal file
View File

@@ -0,0 +1,424 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 14:04:53
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-02 15:17:38
* @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include"Controler.hpp"
Controler::Controler()
{
// Error_capacity = 10;
initVariable();
}
void Controler::initVariable()
{
ClearValTim(desired_speed);
ClearValTim(desired_heading);
ClearValTim(desired_depth);
ClearValTim(desired_pitch);
ClearValTim(current_speed);
ClearValTim(current_heading);
ClearValTim(current_pitch);
ClearValTim(current_depth);
current_time = 0;
start_time = 0;
ClearValTim(desired_thrust);
ClearValTim(desired_rudder);
ClearValTim(desired_elevator);
iterations = 0;
start_time = 0.0;
tardy_helm_thresh = 2.0;
tardy_nav_thresh = 2.0;
// current_error = 0;
// last_error = 0;
// Error.clear();
}
bool Controler::setDesSpeed(double spd, double tim)
{
desired_speed.value = spd;
desired_speed.time = tim;
return true;
}
bool Controler::setDesDepth(double dph, double tim)
{
desired_depth.value = dph;
desired_depth.time = tim;
return true;
}
bool Controler::setDesPitch(double pth, double tim)
{
desired_pitch.value = pth;
desired_pitch.time = tim;
return true;
}
bool Controler::setDesHeading(double hdg, double tim)
{
if(is_rad)
hdg = radToDegrees(hdg);
desired_heading.value = angle180(hdg);
desired_heading.time = tim;
return true;
}
bool Controler::setCurSpeed(double spd, double tim)
{
current_speed.value = spd;
current_speed.time = tim;
return true;
}
bool Controler::setCurDepth(double dph, double tim)
{
current_depth.value = dph;
current_depth.time = tim;
return true;
}
bool Controler::setCurPitch(double pth, double tim)
{
current_pitch.value = pth;
current_pitch.time = tim;
return true;
}
bool Controler::setCurHeading(double hdg, double tim)
{
current_heading.value = hdg;
current_heading.time = tim;
return true;
}
bool Controler::setDesiredValues()
{
iterations++;
desired_thrust.value = 0;
desired_rudder.value = 0;
desired_elevator.value = 0;
//=============================================================
// Part 1: Ensure all nav and helm messages have been received
// for first time. If not, we simply wait, without declaring an
// override. After all messages have been received at least
// once, staleness is checked below.
//=============================================================
runMsg.push_back("desired_heading-time:" + doubleToString(desired_heading.time));
runMsg.push_back("desired_speed-time:" + doubleToString(desired_speed.time));
runMsg.push_back("current_heading-time:" + doubleToString(current_heading.time));
runMsg.push_back("current_speed-time:" + doubleToString(current_speed.time));
runMsg.push_back("desired_depth-time:" + doubleToString(desired_depth.time));
runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
runMsg.push_back("current_pitch-time:" + doubleToString(current_pitch.time));
// runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
if(has_speedCtrl)
{
if((desired_heading.time == 0) || (desired_speed.time == 0))
return true;
}
if(has_headCtrl)
{
if((current_heading.time == 0) || (current_speed.time == 0))
return true;
}
if(has_depthCtrl)
{
if(desired_depth.time == 0)
return true;
if((current_depth.time == 0) || (current_pitch.time == 0))
return true;
}
//=============================================================
// Part 2: Critical info staleness (if not in simulation mode)
//=============================================================
//TODO: 验证信息过时操作
if(cheakStalensee)
{
bool is_stale = checkForStaleness();
if(is_stale)
{
has_override = true;
return false;
}
}
//=============================================================
// Part 3: Set the actuator values. Note: If desired thrust is
// zero others desired values will automatically be zero too.
//=============================================================
// m_desired_thrust = setDesiredThrust();
// if(m_desired_thrust > 0)
// m_desired_rudder = setDesiredRudder();
// if(m_desired_thrust > 0 && m_depth_control)
// m_desired_elevator = setDesiredElevator();
return true;
}
//TODO： 添加操控权判断
bool Controler::overrived(string sval)
{
if(tolower(sval) == "true")
{
if(has_override == false)
addPosting("HAS_CONTROL", "false");
has_override = true;
}
else if(tolower(sval) == "false")
{
// Upon lifting an override, the timestamps are reset. New values
// for all will need to be received before desired_* outputs will
// be produced. If we do not reset, it's possible we may
// interpret older pubs as being stale but they may have been
// paused also during an override.
if(has_override == true)
{
desired_speed.time = 0;
desired_heading.time = 0;
desired_depth.time = 0;
desired_pitch.time = 0;
current_speed.time = 0;
current_heading.time = 0;
current_pitch.time = 0;
current_depth.time = 0;
}
has_override = false;
addPosting("HAS_CONTROL", "true");
}
return true;
}
bool Controler::checkForStaleness()
{
bool is_stale = false;
// =========================================================
// Part 1: Check for Helm staleness
// =========================================================
double hdg_delta = (current_time - desired_heading.time);
if(hdg_delta > tardy_helm_thresh) {
string staleness = doubleToStringX(hdg_delta,3);
addPosting("PID_STALE", "Stale DesHdg:" + staleness);
is_stale = true;
}
double spd_delta = (current_time - desired_speed.time);
if(spd_delta > tardy_helm_thresh) {
string staleness = doubleToStringX(spd_delta,3);
addPosting("PID_STALE", "Stale DesSpd:" + staleness);
is_stale = true;
}
// =========================================================
// Part 2B: Check for Nav staleness
// =========================================================
double nav_hdg_delta = (current_time - current_heading.time);
if(nav_hdg_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_hdg_delta,3);
addPosting("PID_STALE", "Stale NavHdg:" + staleness);
is_stale = true;
}
double nav_spd_delta = (current_time - current_speed.time);
if(nav_spd_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_spd_delta,3);
addPosting("PID_STALE", "Stale NavSpd:" + staleness);
is_stale = true;
}
// =========================================================
// Part 2C: If depth control, check for Helm Depth staleness
// =========================================================
if(has_depthCtrl) {
double dep_delta = (current_time - desired_depth.time);
if(dep_delta > tardy_helm_thresh) {
string staleness = doubleToStringX(dep_delta,3);
addPosting("PID_STALE", "Stale DesDepth:" + staleness);
is_stale = true;
}
}
// =========================================================
// Part 2D: If depth control, check for Nav Depth staleness
// =========================================================
if(has_depthCtrl) {
double nav_dep_delta = (current_time - current_depth.time);
if(nav_dep_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_dep_delta,3);
addPosting("PID_STALE", "Stale NavDep:" + staleness);
is_stale = true;
}
double nav_pit_delta = (current_time - current_pitch.time);
if(nav_pit_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_pit_delta,3);
addPosting("PID_STALE", "Stale NavPitch:" + staleness);
is_stale = true;
}
}
return(is_stale);
}
// bool Controler::setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator)
// {
// limit_thrust[0] = min_thrush;
// limit_thrust[1] = max_thrush;
// limit_rudder[0] = min_rubber;
// limit_rudder[1] = max_rudder;
// limit_elevator[0] = min_elevator;
// limit_elevator[1] = max_elevator;
// }
bool Controler::Limit(double &a, double max, double min)
{
a = (a < min) ? min : a;
a = (a > max) ? max : a;
return true;
}
bool Controler::setCtrl(bool speed, bool heading, bool depth)
{
has_speedCtrl = speed;
has_headCtrl = heading;
has_depthCtrl = depth;
return true;
}
void Controler::addPosting(std::string var, std::string sval)
{
VarDataPair pair(var, sval);
postings.push_back(pair);
}
void Controler::addPosting(std::string var, double val)
{
VarDataPair pair(var, val);
postings.push_back(pair);
}
double Controler::getFrequency() const
{
double elapsed = current_time - start_time;
double frequency = 0;
if(elapsed > 0)
frequency = ((double)(iterations)) / elapsed;
return(frequency);
}
bool Controler::ClearValTim(vlaTim &a)
{
if(&a == NULL)
return false;
else
{
a.value = 0;
a.time = 0;
}
}
//TODO: 控制器参数配置函数
list<std::string> Controler::setConfigParams(std::list<std::string>)
{
list<string> unhandled_params;
list<string>::iterator p;
// for(p=param_lines.begin(); p!=param_lines.end(); p++) {
// string orig = *p;
// string line = tolower(orig);
// string param = biteStringX(line, '=');
// if(param == "speed_factor")
// m_config_params.push_front(orig);
// else if(param == "simulation")
// m_config_params.push_front(orig);
// else if(param == "sim_instability")
// m_config_params.push_front(orig);
// else if(param == "tardy_helm_threshold")
// m_config_params.push_front(orig);
// else if(param == "tardy_nav_threshold")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_ki_limit")
// m_config_params.push_front(orig);
// else if(param == "maxrudder")
// m_config_params.push_front(orig);
// else if(param == "heading_debug")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "maxthrust")
// m_config_params.push_front(orig);
// else if(param == "speed_debug")
// m_config_params.push_front(orig);
// else if(param == "depth_control")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "maxpitch")
// m_config_params.push_front(orig);
// else if(param == "depth_debug")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "maxelevator")
// m_config_params.push_front(orig);
// else
// unhandled_params.push_front(orig);
// }
return(unhandled_params);
}
Json::Value Controler::getConfig(string path)
{
ifstream ifs;
ifs.open(path, ios::in);
Json::Reader taskConfigureReader;
Json::Value inputJsonValue;
taskConfigureReader.parse(ifs, inputJsonValue);
ifs.close();
return inputJsonValue;
}
void Controler::setCheakStalensee(string s)
{
if(s == "true")
cheakStalensee = true;
else
cheakStalensee = false;
}

155
src/pMotionControler/Controler.hpp Normal file
View File

@@ -0,0 +1,155 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 14:05:16
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-02 14:31:16
* @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef __COBTROLLER_H
#define __CONTROLLER_H
#include<iostream>
#include<vector>
#include "VarDataPair.h"
#include "MBUtils.h"
#include "AngleUtils.h"
#include <list>
#include "json/json.h"
#include <fstream>
#include <map>
// #include "VarDataPair.h"
using namespace std;
typedef struct
{
double value;
double time;
} vlaTim;
class Controler
{
public:
Controler();
~Controler(){};
virtual int step(){return 0;}
virtual bool setConfigParams(){return false;}
virtual bool overrived(string sval);
virtual bool handleYawSettings(){return false;}
virtual bool handleSpeedSettings(){return false;};
virtual bool handleDepthSettings(){return false;};
virtual bool hasConfigSettings() const{return false;};
// bool setError(double err);
bool setDesSpeed(double spd, double tim);
bool setDesDepth(double dph, double tim);
bool setDesPitch(double pth, double tim);
bool setDesHeading(double hdg, double tim);
bool setCurSpeed(double spd, double tim);
bool setCurDepth(double dph, double tim);
bool setCurPitch(double pth, double tim);
bool setCurHeading(double hdg, double tim);
// bool setCurTime(double tim){current_time = tim;}
void updateTime(double tim){current_time = tim;}
double getCurTime(){return current_time;}
void setStartTime(double tim){start_time = tim;}
void setOverride(bool v){has_override = v;}
void setTardyHelm(double t){ tardy_helm_thresh = t;}
void setTardyNav(double t){ tardy_nav_thresh = t;}
void setCheakStalensee(string s);
void setConstThrust(double v){const_thrust = v;}
void setDeadZone(double v){dead_zone=v;}
void setDepthControl(bool v){has_depthCtrl=v;}
void setSpeedControl(bool v){has_speedCtrl=v;}
void setHeadingControl(bool v){has_headCtrl=v;}
bool setDesiredValues();
// bool setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator);
bool setCtrl(bool speed, bool heading, bool depth);
bool Limit(double &a, double max, double min);
bool checkForStaleness();
void addPosting(std::string var, std::string sval);
void addPosting(std::string var, double val);
vector<VarDataPair> getPostings() {return(postings);}
void clearPostings() {postings.clear();}
bool ClearValTim(vlaTim &a);
list<std::string> setConfigParams(std::list<std::string>); //使用MOOS风格配置参数函数
Json::Value getConfig(string path);
double getFrequency() const;
bool hasControl(){return(!has_override);}
bool hasSpdCtrl(){return has_speedCtrl;}
bool hasDphCtrl(){return has_depthCtrl;}
bool hasHdgCtrl(){return has_headCtrl;}
double getDesiredRudder() const {return(desired_rudder.value);}
double getDesiredThrust() const {return(desired_thrust.value);}
double getDesiredElevator() const {return(desired_elevator.value);}
// bool setErrorCap(int c){Error_capacity = c;}
inline void initVariable();
list<string> getConfigParams(){return config_params;}
vector<string> getConfigErrors(){return config_errors;}
vector<string> getConfigInfo(){return config_info;}
vector<string> getRunMsg(){return runMsg;}
Json::Value getReport(){return RepList;}
void ClearRunMsg(){runMsg.clear();}
protected:
vlaTim desired_speed;
vlaTim desired_heading;
vlaTim desired_depth;
vlaTim desired_pitch;
vlaTim current_speed;
vlaTim current_heading;
vlaTim current_pitch;
vlaTim current_depth;
double current_time;
double start_time;
vlaTim desired_thrust;
vlaTim desired_rudder;
vlaTim desired_elevator;
double limit_thrust[2];
double limit_rudder[2];
double limit_elevator[2];
double max_rudder;
double max_thrust;
double max_pitch;
double max_elevator;
bool has_speedCtrl = true;
bool has_headCtrl = true;
bool has_depthCtrl = true;
bool has_override = true;
bool cheakStalensee = true;
vector<VarDataPair> postings;
unsigned int iterations;
double tardy_helm_thresh;
double tardy_nav_thresh;
double const_thrust;
double dead_zone;
double is_rad = false;
list<string> config_params;
vector<string> config_errors;
vector<string> config_info;
vector<string> runMsg;
Json::Value RepList;
};
#endif

256
src/pMotionControler/MotionControler.cpp Normal file
View File

@@ -0,0 +1,256 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:27
* @LastEditors: zhaojingkui 1553836110@qq.com
* @LastEditTime: 2023-11-30 11:05:18
* @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
// #define DEBUG
#include"MotionControler.hpp"
//TODO：增加启用哪个控制器功能
bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string key = msg.m_sKey;
string sval = msg.m_sVal;
double dval = msg.m_dfVal;
double dfT;
msg.IsSkewed(m_curr_time, &dfT);
if(fabs(dfT) < ok_skew)
{
if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
pengine.overrived(sval);
else if(key == "DESIRED_HEADING")
{
pengine.setDesHeading(dval, MOOSTime());
}
else if(key == "DESIRED_SPEED")
pengine.setDesSpeed(dval, MOOSTime());
else if(key == "DESIRED_DEPTH")
pengine.setDesDepth(dval, MOOSTime());
else if(key == "NAV_HEADING")
{
pengine.setCurHeading(angle360(dval), MOOSTime());
RepList["NAV"]["Heading"] = dval;
}
else if(key == "NAV_SPEED")
{
pengine.setCurSpeed(dval, MOOSTime());
RepList["NAV"]["Speed"] = dval;
}
else if(key == "NAV_DEPTH")
{
pengine.setCurDepth(dval, MOOSTime());
RepList["NAV"]["Depth"] = dval;
}
else if(key == "NAV_PITCH")
{
pengine.setCurPitch(dval, MOOSTime());
RepList["NAV"]["Pitch"] = dval;
}
else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
{
int e = pengine.setParam(configFilePath);
pengine.setOverride(true);
if(e != 0)
faultCode = 10 + e;
else
faultCode = 0;
}
}
}
return true;
}
bool MotionControler::Iterate()
{
bool a;
AppCastingMOOSApp::Iterate();
pengine.updateTime(m_curr_time);
int i = pengine.step();
switch (i)
{
case 0:
RepList["State : "] = "Run";
break;
case 1:
RepList["State : "] = "Ready";
break;
case -1:
RepList["State : "] = "Fault";
faultCode = 1;//信息超时
break;
default:
break;
}
postPengineResults();
postPenginePostings();
postCharStatus();
Notify(MSG_FALUT,faultCode);
AppCastingMOOSApp::PostReport();
return true;
}
bool MotionControler::OnConnectToServer()
{
registerVariables();
return true;
}
bool MotionControler::OnStartUp()
{
AppCastingMOOSApp::OnStartUp();
pengine.setStartTime(MOOSTime());
STRING_LIST sParams;
m_MissionReader.GetConfiguration(GetAppName(), sParams);
//pengine.setConfigParams(sParams);
//bool handled = true;
STRING_LIST::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++) {
string orig = *p;
string line = (orig); //识别大小写
string param = biteStringX(line, '=');
string value = line;
double dval = atof(value.c_str());
if(param == "config_file")
configFilePath = value;
else if(param == "tardy_helm_thresh")
pengine.setTardyHelm(dval);
else if(param == "tardy_nav_thresh")
pengine.setTardyNav(dval);
else if(param == "cheak_stalensee")
pengine.setCheakStalensee(value);
else if(param == "AppTick")
setFrequency = dval;
else if(param == "delta_freqency")
frequency_delta = dval;
else
reportUnhandledConfigWarning(orig);
}
int e = pengine.setParam(configFilePath);
// if(e != 0)
// {
// faultCode = 10 + e;
// // return false;
// }
return true;
}
void MotionControler::registerVariables()
{
AppCastingMOOSApp::RegisterVariables();
Register("NAV_HEADING", 0);
Register("NAV_SPEED", 0);
Register("NAV_DEPTH", 0);
Register("NAV_PITCH", 0);
Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0);
Register("PID_VERBOSE", 0);
Register("SPEED_FACTOR", 0);
Register("MOOS_MANUAL_OVERIDE", 0);
Register("MOOS_MANUAL_OVERRIDE", 0);
Register(MSG_ReadConfig,0);
}
bool MotionControler::buildReport()
{
double frequency = pengine.getFrequency();
double delta_freq = 100.0*(setFrequency - frequency) / setFrequency;
if(abs(delta_freq) > frequency_delta)
faultCode = 2;
m_msgs << "Frequency Delta : " << frequency << endl;
m_msgs << "PID has_control : " << boolToString(pengine.hasControl()) << endl;
m_msgs << "Config File Path : " << configFilePath << endl;
m_msgs << "S : H : D : | " << intToString(pengine.hasSpdCtrl())+ " | "
<< intToString(pengine.hasHdgCtrl())+" | "
<< intToString(pengine.hasDphCtrl())+" |" << endl;
RepList["to BS"] = colVar;
// RepList["PID"] = pengine.getReport()["W"];
string rep = Json::writeString(RepJsBuilder, RepList);
m_msgs << rep << endl;
return(true);
}
void MotionControler::postPenginePostings()
{
vector<VarDataPair> m_postings = pengine.getPostings();
for(unsigned int i=0; i<m_postings.size(); i++)
{
VarDataPair pair = m_postings[i];
string var = pair.get_var();
if(pair.is_string() && pair.get_sdata_set())
Notify(var, pair.get_sdata());
else if(!pair.is_string() && pair.get_ddata_set())
Notify(var, pair.get_ddata());
}
pengine.clearPostings();
}
void MotionControler::postPengineResults()
{
bool all_stop = true;
if(pengine.hasControl())
all_stop = false;
if(all_stop)
{
if(allstop_posted)
return;
Notify("DESIRED_RUDDER", 0.0);
Notify("DESIRED_THRUST", 0.0);
// if(pengine.hasDphCtrl())
Notify("DESIRED_ELEVATOR", 0.0);
postColVarToBS(0,0,0);
allstop_posted = true;
}
else
{
int T=0,S=0,R=0;
if(pengine.hasHdgCtrl())
{
Notify("DESIRED_RUDDER", pengine.getDesiredRudder());
R = (int)pengine.getDesiredRudder();
}
if(pengine.hasSpdCtrl())
{
Notify("DESIRED_THRUST", pengine.getDesiredThrust());
T = (int)pengine.getDesiredThrust();
}
if(pengine.hasDphCtrl())
{
Notify("DESIRED_ELEVATOR", pengine.getDesiredElevator());
S = pengine.getDesiredElevator();
}
postColVarToBS(T,S,R);
allstop_posted = false;
}
}
void MotionControler::postCharStatus()
{
if(!verbose)
return;
if(pengine.hasControl())
cout << "$" << flush;
else
cout << "*" << flush;
}
void MotionControler::postColVarToBS(int T, int S, int R)
{
string s_msg;
colVar["mode"] = 2;
colVar["thrust"] = T;
colVar["rudder"] = R;
colVar["elevator"] = S;
s_msg = Json::writeString(RepJsBuilder,colVar);
Notify(MSG_TO_BS, s_msg);
}

66
src/pMotionControler/MotionControler.hpp Normal file
View File

@@ -0,0 +1,66 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 15:57:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-07 11:33:04
* @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef TESTAPP
#define TESEAPP
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <iostream>
#include"pidControl.hpp"
#include<vector>
// #include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
using namespace std;
class MotionControler : public AppCastingMOOSApp
{
public:
MotionControler(){};
~MotionControler(){};
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void registerVariables();
bool buildReport();
void postPenginePostings();
void postPengineResults();
void postColVarToBS(int T, int S, int R);
void postCharStatus();
const string MSG_FALUT = "uMotion_fault_fb";
const string MSG_ReadConfig = "uMotion_config_cmd";
const string MSG_TO_BS = "uMotion_control_cmd";
private:
bool ignore_nav_yaw;
bool allstop_posted;
bool verbose;
bool override;
double ok_skew = 2;
int faultCode = 0;
double setFrequency;
double frequency_delta;
Json::Value RepList;
Json::StreamWriterBuilder RepJsBuilder;
Json::Value colVar;
pidControl pengine;
string configFilePath;
int e;
};
#endif
//1.读取参数配置错误 faultCode = 10~
//2.信息过时错误 faultCode=1
//3.频率相差过大错误 faultCode=2

47
src/pMotionControler/a.moos Normal file
View File

@@ -0,0 +1,47 @@
ServerHost = localhost
ServerPort = 9000
ProcessConfig = pMotionControler
{
AppTick = 5
CommsTick = 5
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 10
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 10
// Speed PID controller
speed_pid_kp = 20.0
speed_pid_kd = 0.0
speed_pid_ki = 1
speed_pid_integral_limit = 100
maxpitch = 15
maxelevator = 30
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS MAXRUDDER
maxrudder = 30
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}

181
src/pMotionControler/alpha.bhv Normal file
View File

@@ -0,0 +1,181 @@
//-------- FILE: alpha.bhv -------------
initialize RUN = false
initialize TaskNum=t1
initialize SendTask=false
//--------------模式判断------------------------
set MODE = ACTIVE{
RUN=true
} INACTIVE
set MODE = T1{
MODE=ACTIVE
TaskNum = t1
}
//----------路径点任务----------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100 //优先权重
condition = MODE==T1
//endflag = START=false
endflag = END_WayPoint=true
configflag = CRUISE_SPD = $[SPEED]
//configflag = OSPOS = $[OSX],$[OSY]
activeflag = INFO=$[OWNSHIP]
activeflag = INFO=$[BHVNAME]
activeflag = INFO=$[BHVTYPE]
//cycleflag = CINFO=$[OSX],$[OSY]
wptflag = CurrentPointComplete=true
wptflag = PREV=$(PX),$(PY)
wptflag = NEXT=$(NX),$(NY)
wptflag = TEST=$(X),$(Y)
wptflag = OSPOS=$(OSX),$(OSY)
//wptflag_on_start = true
updates = WPT_UPDATE
//perpetual = true
templating = spawn
// speed_alt = 1.2
//use_alt_speed = true
lead = 8
lead_damper = 1
lead_to_start = false
speed = 1 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40
order = normal
//repeat = 3
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//--------------定深任务------------------
Behavior=BHV_ConstantDepth
{
name = const_depth
pwt = 100
//condition = DEPLOY = true
condition = MODE==T1
duration = no-time-limit
updates = DEPTH_UPDATE
depth = 0
}
//--------------定向任务--------------------
Behavior=BHV_ConstantHeading
{
name = const_heading
pwt = 100
//condition = START_TURN = true
//condition = DEPLOY = true
condition = MODE==T3
perpetual = true
activeflag = TURN = started
//endflag = TURN = done
//endflag = RETURN = true
//endflag = START_TURN = false
endflag = START=false
heading = 225
complete_thresh = 5
duration = no-time-limit
}
//--------------定速任务--------------------
Behavior=BHV_ConstantSpeed
{
name = const_speed
pwt = 1000
condition = MODE==T1
perpetual = true
updates = SPEED_UPDATE
//endflag = START=false
speed = 5
duration = no-time-limit
//peakwidth = 0.5
//basewidth = 0.5
}
//----------------安全模式-----------------------
//----------------计时器---------------------
Behavior = BHV_Timer
{
name = mtime
condition = MODE==T1
pwt = 100
templating = spawn
//duration_status = MSTATUS
//duration = 10
endflag = TIME_OUT=true
updates = TIMER_UPDATES
//perpetual = true
}
//-------------最大深度限制--------------------------
Behavior = BHV_MaxDepth
{
name = maxdepth
pwt = 200
condition = MODE==ACTIVE
updates = MAXDEEP_UPDATES
max_depth = 20
tolerance = 0
duration = no-time-limit
}
//--------------安全区域设置-----------------------
Behavior = BHV_OpRegion
{
// General Behavior Parameters
// ---------------------------
name = op_region // example
pwt = 300 // default
condition = MODE==T5
updates = OPREGION_UPDATES // example
// Parameters specific to this behavior
// ------------------------------------
max_time = 20 // default (seconds)
max_depth = 25 // default (meters)
min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds)
polygon = pts={-80,-00:-30,-175:150,-100:95,25}
breached_altitude_flag = TaskFault = AltitudeOut
breached_depth_flag = TaskFault = DepthOut
breached_poly_flag = TaskFault = RegionOut
breached_time_flag = TaskFault = TimeOut
visual_hints = vertex_color = brown // default
visual_hints = vertex_size = 3 // default
visual_hints = edge_color = aqua // default
visual_hints = edge_size = 1 // default
}

285
src/pMotionControler/alpha.moos Normal file
View File

@@ -0,0 +1,285 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pLogger @ NewConsole = false
//Run = uSimMarineV22 @ NewConsole = false
//Run = pMarinePIDV22 @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pRealm @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
Run = pEmulator @ NewConsole = true
//Run = uTimerScript @ NewConsole = false
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
}
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
matlab_host = 192.168.0.11
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
//------------------------------------------
// pLogger config block
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
ProcessConfig = uSimMarineV22
{
AppTick = 4
CommsTick = 4
start_pos = x=0, y=-20, heading=180, speed=5
prefix = NAV
turn_rate = 40
thrust_map = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
//thrust_reflect = true
buoyancy_rate = 0.075
max_depth_rate = 5
max_depth_rate_speed = 2.0
default_water_depth = 400
}
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMotionControler
{
AppTick = 10
CommsTick = 10
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 3.0
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 5.0
// Speed PID controller
speed_pid_kp = 50.0
speed_pid_kd = 0.0
speed_pid_ki = 10.0
speed_pid_integral_limit = 500.0
maxpitch = 10
maxelevator = 30
pitch_pid_kp = 10.0
pitch_pid_kd = 0
pitch_pid_ki = 0.1
pitch_pid_integral_limit = 5.0
z_to_pitch_pid_kp = 10.0
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.1
z_to_pitch_pid_integral_limit = 1.0
//MAXIMUMS MAXRUDDER
maxrudder = 35
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
//button_one = MOOS_MANUAL_OVERRIDE=false
button_two = STOP # START=false
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}
ProcessConfig = uTimerScript
{
AppTick = 4
CommsTick = 4
condition = DEPLOY = true
randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset
event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120
reset_max = nolimit reset_time = all-posted
}

61
src/pMotionControler/main.cpp Normal file
View File

@@ -0,0 +1,61 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:06
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-10-24 15:11:21
* @FilePath: /moos-ivp-extend/src/pMotionControler/main.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include<iostream>
using namespace std;
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
// #include "VarDataPair.h"
#include"MotionControler.hpp"
#include <iostream>
#include<string>
#include "MBUtils.h"
#include "ColorParse.h"
// #include "MBUtils.h"
// #include "ColorParse.h"
int main(int argc, char *argv[])
{
string mission_file;
string run_command = argv[0];
for(int i=1; i<argc; i++) {
string argi = argv[i];
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
// showReleaseInfoAndExit();
cout << " Version : 0.01" << endl;
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
// showExampleConfigAndExit();
cout << " example : NULL" << endl;
else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
// showHelpAndExit();
cout << " Low leave Control for UUV " << endl;
else if((argi=="-i") || (argi == "--interface"))
// showInterfaceAndExit();
cout << " UUV Motion Control " << endl;
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
mission_file = argv[i];
else if(strBegins(argi, "--alias="))
run_command = argi.substr(8);
else if(i == 2)
run_command = argi;
}
if(mission_file == "")
// showHelpAndExit();
cout << termColor("green");
cout << "pMarinePIDV22 launching as " << run_command << endl;
cout << termColor() << endl;
MotionControler marinePID;
marinePID.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
return(0);
}

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;
}

77
src/pMotionControler/pidControl.hpp Normal file
View File

@@ -0,0 +1,77 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 15:14:26
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-02 18:03:14
* @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef _PIDCONTROL_H
#define _PIDCONTROL_H
#include"Controler.hpp"
typedef struct
{
double kp;
double ki;
double kd;
double error_0;
double error_1;
double error_2;
double limit_delta;
double max_out;
double min_out;
double delta_output;
double output;
} pidInc;
class pidControl : public Controler
{
private:
/* data */
pidInc pid_speed;
pidInc pid_heading;
pidInc pid_pitch;
pidInc pid_depth;
double current_error;
double last_error;
vector<double> Error;
int Error_capacity;
public:
pidControl(/* args */);
~pidControl(){};
int step();
bool setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid);
bool setParam(char n, double param, pidInc *pid_t);
int setParam(string filePath);
bool setParam(Json::Value param, pidInc &pid);
bool hasConfigSettings() const;
double pidStep(double error, pidInc &pid);
//double picStep_p(double error, pidInc &pid);
bool overrived(string svar);
inline void pidClear(pidInc &pid)
{
pid.error_0 = 0;
pid.error_1 = 0;
pid.error_2 = 0;
pid.delta_output = 0;
pid.output = 0;
}
};
#endif

260
src/pMotionControler/simMat.moos Normal file
View File

@@ -0,0 +1,260 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pEmulator @ NewConsole = false
//Run = pLogger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
}
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
}
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
matlab_host = 192.168.0.11
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMotionControler
{
AppTick = 5
CommsTick = 5
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 10
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 10
// Speed PID controller
speed_pid_kp = 20.0
speed_pid_kd = 0.0
speed_pid_ki = 1
speed_pid_integral_limit = 100
maxpitch = 15
maxelevator = 30
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS MAXRUDDER
maxrudder = 30
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
button_one = START # START=true
//button_one = MOOS_MANUAL_OVERRIDE=false
button_two = STOP # START=false
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}