feat: add rev motor feedback for core

src/core_pkg/core_pkg/core_node.py

@@ -169,7 +169,7 @@ class SerialRelay(Node):
         # Convert the 0-1 range into a value in the right range.
         return str(rightMin + (valueScaled * rightSpan))
 
-    def send_controls(self, msg):
+    def send_controls(self, msg: CoreControl):
         #can_relay_tovic,core,19, left_stick, right_stick 
         if(msg.turn_to_enable):
             command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
@@ -182,7 +182,7 @@ class SerialRelay(Node):
         self.send_cmd(command)
 
         #print(f"[Sys] Relaying: {command}")
-    def send_cmd(self, msg):
+    def send_cmd(self, msg: str):
         if self.launch_mode == 'anchor':
             #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
             output = String()#Convert to std_msg string
@@ -216,8 +216,26 @@ class SerialRelay(Node):
             self.core_feedback.bno_accel.z = float(parts[5])
             self.core_feedback.orientation = float(parts[6]) / 10.0
         elif output.startswith("can_relay_fromvic,core,53"):  #Rev motor feedback
-            pass
-            #self.updateMotorFeedback(output)
+            motorId = int(parts[3])
+            temp = float(parts[4]) / 10.0
+            voltage = float(parts[5]) / 10.0
+            current = float(parts[6]) / 10.0
+            if motorId == 1:
+                self.core_feedback.fl_temp = temp
+                self.core_feedback.fl_voltage = voltage
+                self.core_feedback.fl_current = current
+            elif motorId == 2:
+                self.core_feedback.bl_temp = temp
+                self.core_feedback.bl_voltage = voltage
+                self.core_feedback.bl_current = current
+            elif motorId == 3:
+                self.core_feedback.fr_temp = temp
+                self.core_feedback.fr_voltage = voltage
+                self.core_feedback.fr_current = current
+            elif motorId == 4:
+                self.core_feedback.br_temp = temp
+                self.core_feedback.br_voltage = voltage
+                self.core_feedback.br_current = current
         elif output.startswith("can_relay_fromvic,core,54"):  #bat, 12, 5, 3, Voltage readings * 100
             self.core_feedback.bat_voltage = float(parts[3]) / 100.0
             self.core_feedback.voltage_12 = float(parts[4]) / 100.0