From 8060e518c1cac6268e4588d831be9d752bc3113f Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 7 May 2025 13:06:21 -0400 Subject: [PATCH] some bug but found a huge bug with serial comm :/ --- simulated_pcb/odo.py | 9 +++++- .../include/modelec_com/pcb_odo_interface.hpp | 3 ++ src/modelec_com/src/pcb_alim_interface.cpp | 3 -- src/modelec_com/src/pcb_odo_interface.cpp | 3 -- .../include/modelec_gui/pages/odo_page.hpp | 3 +- src/modelec_gui/src/pages/odo_page.cpp | 29 +++++++++++++++---- src/modelec_strat/data/config.xml | 6 ++-- src/modelec_strat/src/pami_manager.cpp | 10 +++++-- 8 files changed, 48 insertions(+), 18 deletions(-) diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 77e15d9..0a454d9 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -96,25 +96,31 @@ class SimulatedPCB: def handle_message(self, msg): if msg == "GET;POS": + print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}') self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode()) elif msg == "GET;SPEED": + print(f'[TX] SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}') self.ser.write(f'SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n'.encode()) elif msg.startswith("GET;DIST;"): n = int(msg.split(';')[2]) dist = self.tof.get(n, 0) + print(f'[TX] SET;DIST;{n};{dist}') self.ser.write(f'SET;DIST;{n};{dist}\n'.encode()) elif msg == "GET;PID": + print(f'[TX] SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}') self.ser.write(f'SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n'.encode()) elif msg.startswith("SET;PID"): - self.pid = list(map(int, msg.split(';')[2:5])) + self.pid = list(map(float, msg.split(';')[2:5])) + print(f'[TX] OK;PID;1') self.ser.write(f'OK;PID;1\n'.encode()) elif msg.startswith("SET;START"): self.start = msg.split(';')[2] == '1' + print(f'[TX] OK;START;1') self.ser.write(f'OK;START;1\n'.encode()) elif msg.startswith("SET;WAYPOINT"): @@ -137,6 +143,7 @@ class SimulatedPCB: self.x = float(parts[2]) self.y = float(parts[3]) self.theta = float(parts[4]) + print(f'[TX] OK;POS;1') self.ser.write(b'OK;POS;1\n') def stop(self): 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 038e4f7..c8b2834 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -130,6 +130,9 @@ private: void ResolveSetPIDRequest(bool success); void ResolveAddWaypointRequest(bool success); + int timeout_ms = 1000; + int attempt = 5; + public: void SendToPCB(const std::string &data) const; void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}) const; diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 5e35371..4cbf22e 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -38,9 +38,6 @@ namespace Modelec { if (res->success) { - RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str()); - RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str()); - pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::SubscriptionOptions options; diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index f7104c8..c4bbeab 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -38,9 +38,6 @@ namespace Modelec { if (res->success) { - RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str()); - RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str()); - pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::SubscriptionOptions options; diff --git a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp index 7d4ae86..2bb25c0 100644 --- a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -33,7 +34,7 @@ namespace ModelecGUI { QHBoxLayout *posLayout_; QPushButton *askPID_; - QLineEdit *pPIDBox_, *iPIDBox_, *dPIDBox_; + QDoubleSpinBox *pPIDBox_, *iPIDBox_, *dPIDBox_; QHBoxLayout *pidLayout_; QPushButton *setPID_; diff --git a/src/modelec_gui/src/pages/odo_page.cpp b/src/modelec_gui/src/pages/odo_page.cpp index 05917f3..bc73a4b 100644 --- a/src/modelec_gui/src/pages/odo_page.cpp +++ b/src/modelec_gui/src/pages/odo_page.cpp @@ -51,18 +51,35 @@ namespace ModelecGUI if (auto res = response.get()) { RCLCPP_INFO(node_->get_logger(), "PID values received: p=%f, i=%f, d=%f", res->p, res->i, res->d); QMetaObject::invokeMethod(this, [this, res]() { - pPIDBox_->setText(QString("%1").arg(res->p)); - iPIDBox_->setText(QString("%1").arg(res->i)); - dPIDBox_->setText(QString("%1").arg(res->d)); + pPIDBox_->setValue(res->p); + iPIDBox_->setValue(res->i); + dPIDBox_->setValue(res->d); }); } else { RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request."); } }); }); - pPIDBox_ = new QLineEdit(""); - iPIDBox_ = new QLineEdit(""); - dPIDBox_ = new QLineEdit(""); + pPIDBox_ = new QDoubleSpinBox(this); + pPIDBox_->setMinimum(0); + pPIDBox_->setMaximum(30); + pPIDBox_->setSingleStep(.1); + pPIDBox_->setValue(0); + pPIDBox_->setDecimals(2); + + iPIDBox_ = new QDoubleSpinBox(this); + iPIDBox_->setMinimum(0); + iPIDBox_->setMaximum(30); + iPIDBox_->setSingleStep(.1); + iPIDBox_->setValue(0); + iPIDBox_->setDecimals(2); + + dPIDBox_ = new QDoubleSpinBox(this); + dPIDBox_->setMinimum(0); + dPIDBox_->setMaximum(30); + dPIDBox_->setSingleStep(.1); + dPIDBox_->setValue(0); + dPIDBox_->setDecimals(2); pidLayout_ = new QHBoxLayout; pidLayout_->addWidget(pPIDBox_); diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 23e34ad..7a91bf0 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -43,15 +43,17 @@ pcb_odo - /dev/pts/5 + /dev/pts/7 115200 pcb_alim - /dev/pts/9 + /dev/pts/11 115200 + 300 + 5 20 diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index 1fdef95..2f079fe 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -80,8 +80,14 @@ namespace Modelec reset_strat_sub_ = create_subscription( "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - timer_add_->cancel(); - timer_remove_->cancel(); + if (timer_add_) + { + timer_add_->cancel(); + } + if (timer_remove_) + { + timer_remove_->cancel(); + } }); }