Encoder Feedback Starter Pack

RoboJackets · May 29, 2024 · 29425a1 · 29425a1
1 parent 1ac37b9
commit 29425a1
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 15 deletions.
diff --git a/urc_hw/include/async_sockets/udpsocket.hpp b/urc_hw/include/async_sockets/udpsocket.hpp
@@ -133,7 +133,6 @@ class UDPSocket : public BaseSocket
     this->Connect(host.c_str(), port, onError);
   }
 
-private:
   static void Receive(UDPSocket * udpSocket)
   {
     char tempBuffer[BUFFER_SIZE + 1];
@@ -155,6 +154,7 @@ class UDPSocket : public BaseSocket
     }
   }
 
+private:
   static void ReceiveFrom(UDPSocket * udpSocket)
   {
     sockaddr_in hostAddr;

diff --git a/urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp b/urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp
@@ -73,9 +73,13 @@ class RoverDrivetrain : public hardware_interface::SystemInterface
   std::string udp_address;
   std::string udp_port;
 
-  // nanopb
+  // nanopb drive encoders message
   uint8_t buffer[DriveEncodersMessage_size];
   size_t message_length;
+
+  // nanopb encoder feedback message
+  uint8_t buffer2[DrivetrainResponse_size];
+  size_t message_length2;
 };
 
 }  // namespace urc_hardware::hardware_interfaces

diff --git a/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp b/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp
@@ -109,12 +109,24 @@ hardware_interface::CallbackReturn RoverDrivetrain::on_deactivate(const rclcpp_l
 }
 
 hardware_interface::return_type RoverDrivetrain::read(
-  const rclcpp::Time &,
-  const rclcpp::Duration &)
+  const rclcpp::Time & time,
+  const rclcpp::Duration & duration)
 {
-  // RCLCPP_INFO(
-  //  rclcpp::get_logger(
-  //      "test"), "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);
+  if (duration.seconds() < 0.001) {
+    return hardware_interface::return_type::OK;
+  }
+
+  udp_->Receive(udp_.get());
+
+  // Decode the received protobuf message
+  DrivetrainResponse message = DrivetrainResponse_init_zero;
+  pb_istream_t stream = pb_istream_from_buffer((uint8_t*)buffer2, message_length2);
+
+  pb_decode(&stream, DrivetrainResponse_fields, &message);
+
+  velocity_rps_states[0] = (message.m1Feedback + message.m2Feedback + message.m3Feedback) / 3.0;
+  velocity_rps_states[1] = (message.m4Feedback + message.m5Feedback + message.m4Feedback) / 3.0;
+
 
   return hardware_interface::return_type::OK;
 }

diff --git a/urc_nanopb/proto/urc.proto b/urc_nanopb/proto/urc.proto
@@ -103,14 +103,19 @@ message ArmClawRequest {
     optional int32 clawVelTicksPerSecond = 1;
 }
 
-// Rover -> ROS
-message DriveFeedback {
-    optional int64 leftPosTicks = 1;
-    optional int64 rightPosTicks = 2;
-    optional int64 leftVelTicksPerSecond = 3;
-    optional int64 rightVelTicksPerSecond = 4;
-    optional int64 leftCurrentAmps = 5;
-    optional int64 rightCurrentAmps = 6;
+message DrivetrainResponse {
+    required int32 m1Feedback           = 1;
+    required uint32 m1Current           = 2;
+    required int32 m2Feedback           = 3;
+    required uint32 m2Current           = 4;
+    required int32 m3Feedback           = 5;
+    required uint32 m3Current           = 6;
+    required int32 m4Feedback           = 7;
+    required uint32 m4Current           = 8;
+    required int32 m5Feedback           = 9;
+    required uint32 m5Current           = 10;
+    required int32 m6Feedback           = 11;
+    required uint32 m6Current           = 12;
 }
 
 message ArmFeedback {