diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index a5d68cb..1a14e24 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -109,6 +109,6 @@ namespace Modelec void GetPID(); void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg); - void SetPID(std::string name, float p, float i, float d); + void SetPID(std::string name, float p, float i, float d, std::optional min = std::nullopt, std::optional max = std::nullopt); }; } // namespace Modelec diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 4b6469a..0284368 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -63,7 +63,10 @@ namespace Modelec SetStart(true); - SetPID("THETA", 8, 0, 0.1); + SetPID("THETA", 14, 0, 0); + SetPID("POS", 10, 0, 0); + SetPID("LEFT", 5, 0, 0); + SetPID("RIGHT", 5, 0, 0); } else { @@ -424,7 +427,7 @@ namespace Modelec SetPID(msg->name, msg->p, msg->i, msg->d); } - void PCBOdoInterface::SetPID(std::string name, float p, float i, float d) + void PCBOdoInterface::SetPID(std::string name, float p, float i, float d, std::optional min, std::optional max) { std::vector data = { name, @@ -433,6 +436,16 @@ namespace Modelec std::to_string(d) }; + if (min.has_value()) + { + data.push_back(std::to_string(min.value())); + } + + if (max.has_value()) + { + data.push_back(std::to_string(max.value())); + } + SendOrder("PID", data); } } // Modelec