From 5f708a2002f0c188e9f10fcf2452fc35fafc6dca Mon Sep 17 00:00:00 2001 From: David Date: Fri, 30 May 2025 03:02:01 +0000 Subject: [PATCH] feat: add rev motor feedback for core --- src/core_pkg/core_pkg/core_node.py | 36 ++++++++++++++++++++++-------- src/ros2_interfaces_pkg | 2 +- 2 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 6c650e9..15a505c 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/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 @@ -205,25 +205,43 @@ class SerialRelay(Node): #if parts length is at least 5 then we should have altitude, this is a temporary check until fully implemented if len(parts) >= 5: self.core_feedback.gps_alt = round(float(parts[4]), 2) - elif output.startswith("can_relay_fromvic,core,51"):#Gyro x,y,z + elif output.startswith("can_relay_fromvic,core,51"): #Gyro x,y,z self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) self.core_feedback.imu_calib = round(float(parts[6])) - elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 + elif output.startswith("can_relay_fromvic,core,52"): #Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) 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) - elif output.startswith("can_relay_fromvic,core,54"):#bat, 12, 5, 3, Voltage readings * 100 + elif output.startswith("can_relay_fromvic,core,53"): #Rev motor feedback + 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 self.core_feedback.voltage_5 = float(parts[5]) / 100.0 self.core_feedback.voltage_3 = float(parts[6]) / 100.0 - elif output.startswith("can_relay_fromvic,core,56"):#BMP Temp, Altitude, Pressure + elif output.startswith("can_relay_fromvic,core,56"): #BMP Temp, Altitude, Pressure self.core_feedback.bmp_temp = float(parts[3]) self.core_feedback.bmp_alt = float(parts[4]) self.core_feedback.bmp_pres = float(parts[5]) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index da69f94..be99083 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit da69f941c284feae3f34763e09ee6b4188dd7dd4 +Subproject commit be99083bee2db986c6de0af89595ac79e2e4f493