From f064bcfc698015cc38f1307361af42ac7366d70c Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Fri, 11 Apr 2025 15:41:02 +0200
Subject: [PATCH] Updated firmware for new pcb

---
 firmware/base/src/config.h            |  8 +++----
 firmware/base/src/main.c              |  4 ++--
 mg_control/src/mg_control.cpp         |  4 ++--
 mg_odometry/src/mg_odom_publisher.cpp | 34 ++++++++++++++++++++-------
 4 files changed, 34 insertions(+), 16 deletions(-)

diff --git a/firmware/base/src/config.h b/firmware/base/src/config.h
index 5058e7c..3f3e339 100644
--- a/firmware/base/src/config.h
+++ b/firmware/base/src/config.h
@@ -1,10 +1,10 @@
 #pragma once
 
 //############## CONFIG STEPPERS ################
-#define DIR_PIN_A   16
-#define DIR_PIN_B   26
-#define PULSE_PIN_A 17
-#define PULSE_PIN_B 27
+#define DIR_PIN_A   26
+#define DIR_PIN_B   16
+#define PULSE_PIN_A 27
+#define PULSE_PIN_B 17
 
 #define SM_A 0
 #define SM_B 1
diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c
index 9273865..95ffc5d 100644
--- a/firmware/base/src/main.c
+++ b/firmware/base/src/main.c
@@ -59,8 +59,8 @@ bool update_pos_cb() {
 
     vel_l /=64 * ENCODER_CPR;
     vel_r /=64 * ENCODER_CPR;
-    vel_l *=  WHEEL_RADIUS * 2 * M_PI * wheel_ratio_l;
-    vel_r *= -WHEEL_RADIUS * 2 * M_PI * wheel_ratio_r;
+    vel_l *=  WHEEL_RADIUS * 2 * M_PI;
+    vel_r *= -WHEEL_RADIUS * 2 * M_PI;
 
     const double linear = (vel_l + vel_r) / 2;
     const double angular = (vel_r - vel_l) / wheel_separation;
diff --git a/mg_control/src/mg_control.cpp b/mg_control/src/mg_control.cpp
index ad36632..e9612ac 100644
--- a/mg_control/src/mg_control.cpp
+++ b/mg_control/src/mg_control.cpp
@@ -73,9 +73,9 @@ hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time
 
     try {
         odrive_serial_interface.Write("s;");
-        value.data = left_wheel_vel_cmd / (2 * M_PI);
+        value.data = -left_wheel_vel_cmd / (2 * M_PI);
         for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
-        value.data = -right_wheel_vel_cmd / (2 * M_PI);
+        value.data = right_wheel_vel_cmd / (2 * M_PI);
         for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
 
     } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp
index 3c5d0ff..6d0119f 100644
--- a/mg_odometry/src/mg_odom_publisher.cpp
+++ b/mg_odometry/src/mg_odom_publisher.cpp
@@ -8,7 +8,7 @@
 #include <libserial/SerialStream.h>
 #include <sys/types.h>
 
-#include "mg_msgs/srv/set_width.hpp"
+#include "mg_msgs/srv/send_double.hpp"
 #include "std_srvs/srv/empty.hpp"
 
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -25,8 +25,8 @@
 
 constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
 
-using SetWidthSrv = mg_msgs::srv::SetWidth;
-using ZeroSrv     = std_srvs::srv::Empty;
+using SendDoubleSrv = mg_msgs::srv::SendDouble;
+using ZeroSrv       = std_srvs::srv::Empty;
 
 class MgOdomPublisher : public rclcpp::Node {
   public:
@@ -40,8 +40,10 @@ class MgOdomPublisher : public rclcpp::Node {
         timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
                                          std::bind(&MgOdomPublisher::timer_callback, this));
 
-        set_width_service_ = create_service<SetWidthSrv>(
+        set_width_service_ = create_service<SendDoubleSrv>(
           "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
+        set_ratio_service_ = create_service<SendDoubleSrv>(
+          "/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
 
         zero_service_
           = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@@ -49,26 +51,42 @@ class MgOdomPublisher : public rclcpp::Node {
 
   private:
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-    rclcpp::Service<SetWidthSrv>::SharedPtr        set_width_service_;
+    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_width_service_;
+    rclcpp::Service<SendDoubleSrv>::SharedPtr      set_ratio_service_;
     rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
     rclcpp::TimerBase::SharedPtr                   timer_;
     LibSerial::SerialStream                        enc_serial_port_;
 
-    void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
-        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
+    void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
+        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
 
         if (enc_serial_port_.IsOpen()) {
             union {
                 std::array<u_char, sizeof(double)> bytes;
                 double                             data;
             } value{};
-            value.data = request->width;
+            value.data = request->data;
 
             enc_serial_port_ << "w;";
             for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
         }
     }
 
+    void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
+        RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);
+
+        if (enc_serial_port_.IsOpen()) {
+            union {
+                std::array<u_char, sizeof(double)> bytes;
+                double                             data;
+            } value{};
+            value.data = request->data;
+
+            enc_serial_port_ << "r;";
+            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+        }
+    }
+
     void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
         RCLCPP_INFO(get_logger(), "Zeroing odometry");
 
-- 
2.49.0