diff --git a/CMakeLists.txt b/CMakeLists.txt index 03a4b63..e3c922e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/missions/alder/ControlParam.json b/missions/alder/ControlParam.json new file mode 100644 index 0000000..9e82667 --- /dev/null +++ b/missions/alder/ControlParam.json @@ -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 +} diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos index e328b37..852cc64 100644 --- a/missions/alder/alder.moos +++ b/missions/alder/alder.moos @@ -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 +} \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 241cb01..1fe6556 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -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 ############################################################################## diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index e8c80c1..cf8d9fc 100644 --- a/src/pAUV150/AUV150.cpp +++ b/src/pAUV150/AUV150.cpp @@ -13,7 +13,7 @@ #include #include -// #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; diff --git a/src/pMotionControler/CMakeLists.txt b/src/pMotionControler/CMakeLists.txt new file mode 100644 index 0000000..bfae561 --- /dev/null +++ b/src/pMotionControler/CMakeLists.txt @@ -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 + ) + diff --git a/src/pMotionControler/ControlParam.json b/src/pMotionControler/ControlParam.json new file mode 100644 index 0000000..9204381 --- /dev/null +++ b/src/pMotionControler/ControlParam.json @@ -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 +} diff --git a/src/pMotionControler/Controler.cpp b/src/pMotionControler/Controler.cpp new file mode 100644 index 0000000..6f534a4 --- /dev/null +++ b/src/pMotionControler/Controler.cpp @@ -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 Controler::setConfigParams(std::list) +{ + list unhandled_params; + list::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; +} \ No newline at end of file diff --git a/src/pMotionControler/Controler.hpp b/src/pMotionControler/Controler.hpp new file mode 100644 index 0000000..7aee711 --- /dev/null +++ b/src/pMotionControler/Controler.hpp @@ -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 +#include +#include "VarDataPair.h" +#include "MBUtils.h" +#include "AngleUtils.h" +#include +#include "json/json.h" +#include +#include + +// #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 getPostings() {return(postings);} + void clearPostings() {postings.clear();} + bool ClearValTim(vlaTim &a); + list setConfigParams(std::list); //使用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 getConfigParams(){return config_params;} + vector getConfigErrors(){return config_errors;} + vector getConfigInfo(){return config_info;} + vector 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 postings; + unsigned int iterations; + + double tardy_helm_thresh; + double tardy_nav_thresh; + double const_thrust; + double dead_zone; + + + + double is_rad = false; + + list config_params; + vector config_errors; + vector config_info; + vector runMsg; + Json::Value RepList; +}; + +#endif \ No newline at end of file diff --git a/src/pMotionControler/MotionControler.cpp b/src/pMotionControler/MotionControler.cpp new file mode 100644 index 0000000..d1141fc --- /dev/null +++ b/src/pMotionControler/MotionControler.cpp @@ -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 m_postings = pengine.getPostings(); + for(unsigned int i=0; i +#include"pidControl.hpp" +#include +// #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 \ No newline at end of file diff --git a/src/pMotionControler/a.moos b/src/pMotionControler/a.moos new file mode 100644 index 0000000..281ba8d --- /dev/null +++ b/src/pMotionControler/a.moos @@ -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 +} \ No newline at end of file diff --git a/src/pMotionControler/alpha.bhv b/src/pMotionControler/alpha.bhv new file mode 100644 index 0000000..65b258e --- /dev/null +++ b/src/pMotionControler/alpha.bhv @@ -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 + } \ No newline at end of file diff --git a/src/pMotionControler/alpha.moos b/src/pMotionControler/alpha.moos new file mode 100644 index 0000000..1d506b7 --- /dev/null +++ b/src/pMotionControler/alpha.moos @@ -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 +} \ No newline at end of file diff --git a/src/pMotionControler/main.cpp b/src/pMotionControler/main.cpp new file mode 100644 index 0000000..ab9c11e --- /dev/null +++ b/src/pMotionControler/main.cpp @@ -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 +using namespace std; +#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +// #include "VarDataPair.h" +#include"MotionControler.hpp" +#include +#include +#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::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; +} \ No newline at end of file diff --git a/src/pMotionControler/pidControl.hpp b/src/pMotionControler/pidControl.hpp new file mode 100644 index 0000000..67254c5 --- /dev/null +++ b/src/pMotionControler/pidControl.hpp @@ -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 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 \ No newline at end of file diff --git a/src/pMotionControler/simMat.moos b/src/pMotionControler/simMat.moos new file mode 100644 index 0000000..f345908 --- /dev/null +++ b/src/pMotionControler/simMat.moos @@ -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 +} \ No newline at end of file