Working hardware?

RoboJackets · May 30, 2024 · 3f7afeb · 3f7afeb
1 parent 907f271
commit 3f7afeb
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 16 deletions.
diff --git a/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp b/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp
@@ -113,8 +113,8 @@ hardware_interface::return_type RoverDrivetrain::read(
   const rclcpp::Duration &)
 {
 
-  RCLCPP_INFO(rclcpp::get_logger("test"),
-    "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);
+  // RCLCPP_INFO(rclcpp::get_logger("test"),
+  //   "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);
 
   return hardware_interface::return_type::OK;
 }
@@ -131,13 +131,22 @@ hardware_interface::return_type RoverDrivetrain::write(
   // DrivetrainRequest drivetrainRequest = DrivetrainRequest_init_zero;
   pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
 
-  message.payload.driveEncodersMessage.m1Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
-  message.payload.driveEncodersMessage.m2Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
-  message.payload.driveEncodersMessage.m3Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
+  message.m1Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
+  message.m2Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
+  message.m3Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
 
-  message.payload.driveEncodersMessage.m4Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
-  message.payload.driveEncodersMessage.m5Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
-  message.payload.driveEncodersMessage.m6Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+  message.m4Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+  message.m5Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+  message.m6Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+
+
+  // Junk
+  message.redEnabled = 0;
+  message.blueEnabled = 0;
+  message.greenEnabled = 0;
+  message.redBlink = 0;
+  message.blueBlink = 0;
+  message.greenBlink = 0;
 
   message.messageID = 0;
 

diff --git a/urc_hw/src/hardware_interfaces/status_light.cpp b/urc_hw/src/hardware_interfaces/status_light.cpp
@@ -122,15 +122,22 @@ hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const r
   NewStatusLightCommand statusMessage = NewStatusLightCommand_init_zero;
   pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
 
-  statusMessage.redEnabled = (signals[0] == 0);
+  statusMessage.redEnabled = 1;
   statusMessage.redBlink = (signals[0] == 0 && signals[1] == 1);
-  statusMessage.blueEnabled = (signals[0] == 1);
+  statusMessage.blueEnabled = 1;
   statusMessage.blueBlink = (signals[0] == 1 && signals[1] == 1);
-  statusMessage.greenEnabled = (signals[0] == 2);
+  statusMessage.greenEnabled = 1;
   statusMessage.greenBlink = (signals[0] == 2 && signals[1] == 1);
 
+  // Junk
+  message.m1Setpoint = 0;
+  message.m2Setpoint = 0;
+  message.m3Setpoint = 0;
+  message.m4Setpoint = 0;
+  message.m5Setpoint = 0;
+  message.m6Setpoint = 0;
+
   message.messageID = 1;
-  message.payload.statusLightCommand = statusMessage;
 
   bool status = pb_encode(&stream, TeensyMessage_fields, &message);
   message_length = stream.bytes_written;

diff --git a/urc_nanopb/proto/urc.proto b/urc_nanopb/proto/urc.proto
@@ -30,10 +30,18 @@ message NewStatusLightCommand {
 
 message TeensyMessage {
     required int32 messageID = 1;
-    oneof payload {
-        DrivetrainRequest driveEncodersMessage = 2;
-        NewStatusLightCommand statusLightCommand = 3;
-    }
+    required int32 m1Setpoint           = 2;  
+    required int32 m2Setpoint           = 3;
+    required int32 m3Setpoint           = 4;
+    required int32 m4Setpoint           = 5;
+    required int32 m5Setpoint           = 6;
+    required int32 m6Setpoint           = 7;      
+    required int32 redEnabled = 8;
+    required int32 blueEnabled = 9;
+    required int32 greenEnabled = 10;
+    required int32 redBlink = 11;
+    required int32 blueBlink = 12;
+    required int32 greenBlink = 13;
 }
 
 message RequestMessage {