Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Thread lock #110

Merged
merged 8 commits into from
Apr 26, 2023
+353 −288
Merged

Thread lock #110

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
e083269
thread lock
Aayli Apr 11, 2023
f264f9d
undo fcn naming
rafal-gorecki Apr 12, 2023
29210f1
few more lock
Aayli Apr 13, 2023
3731930
revert nodes
rafal-gorecki Apr 19, 2023
4959c30
Paweł suggestions
Aayli Apr 21, 2023
a0daef5
change
Aayli Apr 21, 2023
e8c1602
e_stop
rafal-gorecki Apr 24, 2023
49d58b9
Merge branch 'ros1' into thread-lock
rafal-gorecki Apr 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
121 changes: 64 additions & 57 deletions panther_battery/src/adc_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#!/usr/bin/python3

from threading import Lock

import math

import rospy
Expand All @@ -15,6 +17,8 @@ class ADCNode:
def __init__(self, name: str) -> None:
rospy.init_node(name, anonymous=False)

self._lock = Lock()

self._V_driv_front = None
self._V_driv_rear = None
self._I_driv_front = None
Expand Down Expand Up @@ -59,72 +63,75 @@ def __init__(self, name: str) -> None:
rospy.loginfo(f'[{rospy.get_name()}] Node started')

def _motor_controllers_state_cb(self, driver_state: DriverState) -> None:
self._V_driv_front = driver_state.front.voltage
self._V_driv_rear = driver_state.front.current
self._I_driv_front = driver_state.rear.voltage
self._I_driv_rear = driver_state.rear.current
with self._lock:
self._V_driv_front = driver_state.front.voltage
self._V_driv_rear = driver_state.front.current
self._I_driv_front = driver_state.rear.voltage
self._I_driv_rear = driver_state.rear.current

def _io_state_cb(self, io_state: IOState) -> None:
self._charger_connected = io_state.charger_connected
with self._lock:
self._charger_connected = io_state.charger_connected
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved

def _battery_timer_cb(self, *args) -> None:
try:
V_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage0_raw', LSB=0.02504255, offset=0.0
)
V_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage3_raw', LSB=0.02504255, offset=0.0
)
V_temp_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage1_raw', LSB=0.002, offset=0.0
)
V_temp_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage0_raw', LSB=0.002, offset=0.0
)
I_charge_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage3_raw', LSB=0.005, offset=0.0
)
I_charge_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage2_raw', LSB=0.005, offset=0.0
)
I_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage2_raw', LSB=0.04, offset=625.0
)
I_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage1_raw', LSB=0.04, offset=625.0
)
except:
rospy.logerr(f'[{rospy.get_name()}] Battery ADC measurement error')
return
with self._lock:
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
try:
V_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage0_raw', LSB=0.02504255, offset=0.0
)
V_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage3_raw', LSB=0.02504255, offset=0.0
)
V_temp_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage1_raw', LSB=0.002, offset=0.0
)
V_temp_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage0_raw', LSB=0.002, offset=0.0
)
I_charge_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage3_raw', LSB=0.005, offset=0.0
)
I_charge_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device0/in_voltage2_raw', LSB=0.005, offset=0.0
)
I_bat_1 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage2_raw', LSB=0.04, offset=625.0
)
I_bat_2 = self._get_adc_measurement(
path='/sys/bus/iio/devices/iio:device1/in_voltage1_raw', LSB=0.04, offset=625.0
)
except:
rospy.logerr(f'[{rospy.get_name()}] Battery ADC measurement error')
return

if self._battery_count == 2:
temp_bat_1 = self._voltage_to_deg(V_temp_bat_1)
temp_bat_2 = self._voltage_to_deg(V_temp_bat_2)
if self._battery_count == 2:
temp_bat_1 = self._voltage_to_deg(V_temp_bat_1)
temp_bat_2 = self._voltage_to_deg(V_temp_bat_2)

self._publish_battery_msg(
self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1
)
self._publish_battery_msg(
self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2
)
self._publish_battery_msg(
self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1
)
self._publish_battery_msg(
self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2
)

V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0
temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0
I_bat_sum = I_bat_1 + I_bat_2
I_charge_bat = I_charge_bat_1 + I_charge_bat_2
V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0
temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0
I_bat_sum = I_bat_1 + I_bat_2
I_charge_bat = I_charge_bat_1 + I_charge_bat_2

self._publish_battery_msg(
self._battery_pub,
V_bat_avereage,
temp_bat_average,
-I_bat_sum + I_charge_bat,
)
self._publish_battery_msg(
self._battery_pub,
V_bat_avereage,
temp_bat_average,
-I_bat_sum + I_charge_bat,
)

else:
temp_bat_1 = self._voltage_to_deg(V_temp_bat_1)
self._publish_battery_msg(
self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1
)
else:
temp_bat_1 = self._voltage_to_deg(V_temp_bat_1)
self._publish_battery_msg(
self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1
)

def _check_battery_count(self) -> int:
trials_num = 10
Expand Down
3 changes: 2 additions & 1 deletion panther_battery/src/roboteq_republisher_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,12 @@ class RoboteqRepublisherNode:
def __init__(self, name: str) -> None:
rospy.init_node(name, anonymous=False)

self._lock = Lock()

self._battery_voltage = None
self._battery_current = None
self._battery_timeout = 1.0
self._last_battery_info_time = rospy.get_time()
self._lock = Lock()

# -------------------------------
# Subscribers
Expand Down
Loading