From 280d56877cd859d41f4a261f1994b1a8eb7c4606 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Wed, 22 Feb 2023 21:41:48 -0800 Subject: [PATCH] Init experimental branch for pimu hw id 2 --- stretch_core/nodes/stretch_driver | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 65897ba..14bdc26 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -146,6 +146,11 @@ class StretchDriverNode: self.charging_state = BatteryState.POWER_SUPPLY_STATUS_CHARGING elif robot_status['pimu']['charger_connected'] == False and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING: self.charging_state = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + elif pimu_hardware_id == 2: + if robot_status['pimu']['charger_connected'] == True and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING: + self.charging_state = BatteryState.POWER_SUPPLY_STATUS_CHARGING + elif robot_status['pimu']['charger_connected'] == False and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING: + self.charging_state = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING i = float(robot_status['pimu']['current']) if self.charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING: @@ -166,7 +171,7 @@ class StretchDriverNode: # misuse the 'present' flag to indicated whether the barrel jack button is pressed (i.e. charger is present, but may or may not be providing power) if pimu_hardware_id == 0: battery_state.present = False - elif pimu_hardware_id == 1: + elif pimu_hardware_id == 1 or pimu_hardware_id == 2: battery_state.present = robot_status['pimu']['charger_connected'] self.power_pub.publish(battery_state)