Idk

RoboJackets · May 30, 2024 · 5fa7b00 · 5fa7b00
1 parent 907f271
commit 5fa7b00
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 19 deletions.
diff --git a/urc_bringup/launch/bringup.launch.py b/urc_bringup/launch/bringup.launch.py
@@ -151,7 +151,7 @@ def generate_launch_description():
             load_robot_state_publisher,
             load_joint_state_broadcaster,
             load_drivetrain_controller,
-            load_status_light_controller,
+            # load_status_light_controller,
             # load_arm_controller,
             # load_gripper_controller_left,
             # load_gripper_controller_right,

diff --git a/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp b/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp
@@ -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 = (int32_t)(velocity_rps_commands[0] * ENCODER_CPR * -1);
+  // message.m2Setpoint = (int32_t)(velocity_rps_commands[0] * ENCODER_CPR * -1);
+  // message.m3Setpoint = (int32_t)(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 = (int32_t)(velocity_rps_commands[1] * ENCODER_CPR * -1);
+  // message.m5Setpoint = (int32_t)(velocity_rps_commands[1] * ENCODER_CPR * -1);
+  // message.m6Setpoint = (int32_t)(velocity_rps_commands[1] * ENCODER_CPR * -1);
+
+
+  message.m1Setpoint = 1;
+  message.m2Setpoint = 1;
+  message.m3Setpoint = 1;
+
+  message.m4Setpoint = 1;
+  message.m5Setpoint = 1;
+  message.m6Setpoint = 1;
 
   message.messageID = 0;
 

diff --git a/urc_hw/src/hardware_interfaces/status_light.cpp b/urc_hw/src/hardware_interfaces/status_light.cpp
@@ -119,18 +119,24 @@ hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const r
 {
 
   TeensyMessage message = TeensyMessage_init_zero;
-  NewStatusLightCommand statusMessage = NewStatusLightCommand_init_zero;
   pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
 
-  statusMessage.redEnabled = (signals[0] == 0);
-  statusMessage.redBlink = (signals[0] == 0 && signals[1] == 1);
-  statusMessage.blueEnabled = (signals[0] == 1);
-  statusMessage.blueBlink = (signals[0] == 1 && signals[1] == 1);
-  statusMessage.greenEnabled = (signals[0] == 2);
-  statusMessage.greenBlink = (signals[0] == 2 && signals[1] == 1);
+  // message.redEnabled = (signals[0] == 0);
+  // message.redBlink = (signals[0] == 0 && signals[1] == 1);
+  // message.blueEnabled = (signals[0] == 1);
+  // message.blueBlink = (signals[0] == 1 && signals[1] == 1);
+  // message.greenEnabled = (signals[0] == 2);
+  // message.greenBlink = (signals[0] == 2 && signals[1] == 1);
+
+
+  message.redEnabled = 1;
+  message.redBlink = 1;
+  message.blueEnabled = 1;
+  message.blueBlink = 1;
+  message.greenEnabled = 1;
+  message.greenBlink = 1;
 
   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
@@ -29,11 +29,21 @@ message NewStatusLightCommand {
 }
 
 message TeensyMessage {
+    // 0 = drivetrain
+    // 1 = status light
     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 {