aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: add rev motor feedback for core

Browse Source
This commit is contained in:
David
2025-05-30 03:02:01 +00:00
parent baefd0661e
commit 5f708a2002
2 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
src/core_pkg/core_pkg/core_node.py
View File

@@ -169,7 +169,7 @@ class SerialRelay(Node):
# Convert the 0-1 range into a value in the right range. # Convert the 0-1 range into a value in the right range.
return str(rightMin + (valueScaled * rightSpan)) 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 #can_relay_tovic,core,19, left_stick, right_stick
if(msg.turn_to_enable): if(msg.turn_to_enable):
command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 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) self.send_cmd(command)
#print(f"[Sys] Relaying: {command}") #print(f"[Sys] Relaying: {command}")
def send_cmd(self, msg): def send_cmd(self, msg: str):
if self.launch_mode == 'anchor': if self.launch_mode == 'anchor':
#self.get_logger().info(f"[Core to Anchor Relay] {msg}") #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
output = String()#Convert to std_msg string 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 parts length is at least 5 then we should have altitude, this is a temporary check until fully implemented
if len(parts) >= 5: if len(parts) >= 5:
self.core_feedback.gps_alt = round(float(parts[4]), 2) 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.x = float(parts[3])
self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.y = float(parts[4])
self.core_feedback.bno_gyro.z = float(parts[5]) self.core_feedback.bno_gyro.z = float(parts[5])
self.core_feedback.imu_calib = round(float(parts[6])) 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.x = float(parts[3])
self.core_feedback.bno_accel.y = float(parts[4]) self.core_feedback.bno_accel.y = float(parts[4])
self.core_feedback.bno_accel.z = float(parts[5]) self.core_feedback.bno_accel.z = float(parts[5])
self.core_feedback.orientation = float(parts[6]) / 10.0 self.core_feedback.orientation = float(parts[6]) / 10.0
elif output.startswith("can_relay_fromvic,core,53"):#Rev motor feedback elif output.startswith("can_relay_fromvic,core,53"): #Rev motor feedback
pass motorId = int(parts[3])
#self.updateMotorFeedback(output) temp = float(parts[4]) / 10.0
elif output.startswith("can_relay_fromvic,core,54"):#bat, 12, 5, 3, Voltage readings * 100 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.bat_voltage = float(parts[3]) / 100.0
self.core_feedback.voltage_12 = float(parts[4]) / 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_5 = float(parts[5]) / 100.0
self.core_feedback.voltage_3 = float(parts[6]) / 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_temp = float(parts[3])
self.core_feedback.bmp_alt = float(parts[4]) self.core_feedback.bmp_alt = float(parts[4])
self.core_feedback.bmp_pres = float(parts[5]) self.core_feedback.bmp_pres = float(parts[5])

2
src/ros2_interfaces_pkg

Submodule src/ros2_interfaces_pkg updated: da69f941c2...be99083bee