Merge pull request #2 from robomechanics/percentage

Arm battery
robomechanics · Jun 18, 2024 · 36719de · 36719de
2 parents c011e47 + ef42450
commit 36719de
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git a/arm_control/scripts/arm_control.py b/arm_control/scripts/arm_control.py
@@ -44,6 +44,11 @@ def __init__(self, family_name, module_names, hrdf_relative_path):
         self.num_joints = self.group.size
         self.group_fbk = hebi.GroupFeedback(self.num_joints)
         self.batt_voltage = 0.0
+        #maximum and minimum based off of 
+        #https://maxamps.com/products/lipo-3250-8s-29-6v-battery-pack
+        self.min_lipo_voltage = 25.6 #Uses the cutoff voltage for safety
+        self.max_lipo_voltage = 33.6  
+
         # gainsCmd.mstop_strategy = 2
 
     def get_batt_voltage(self):
@@ -120,6 +125,10 @@ def __init__(self, robot_controller: RobotController):
                                                         String, 
                                                         queue_size=1)
 
+        self.lipo_battery_percentage_pub = rospy.Publisher(self._lipo_battery_percentage_topic, 
+                                                           String,
+                                                           queue_size=1)
+        #now need to check how self.lipo_battery_voltage_pub was used
         self._batt_voltage_timer = rospy.Timer(rospy.Duration(30), self.publish_lipo_battery_voltage)
 
 
@@ -131,10 +140,18 @@ def load_ros_params(self):
             "is_arm_in_home_pose_param_name")
         self._lipo_battery_voltage_topic = rospy.get_param(
             "lipo_battery_voltage_topic")
+        self._lipo_battery_percentage_topic = rospy.get_param(
+            "lipo_battery_percentage_topic")
 
     def publish_lipo_battery_voltage(self, data):
+        #publishes voltage and percentage simultaneously
         self.robot_controller.get_batt_voltage()
         self.lipo_battery_voltage_pub.publish(f"{self.robot_controller.batt_voltage:.1f}")
+        #lipo_battery_percentage = (difference / total range) * 100
+        lipo_battery_percentage = (((self.robot_controller.batt_voltage - 
+                                  self.robot_controller.min_lipo_voltage) * 100) /
+                                  (self.robot_controller.max_lipo_voltage - self.robot_controller.min_lipo_voltage))
+        self.lipo_battery_percentage_pub.publish(f"{lipo_battery_percentage:.1f}")
 
     def get_forward_waypoints(self, traj_type = 0):
         if traj_type == 0:

diff --git a/autonomy_manager/config/constants.yaml b/autonomy_manager/config/constants.yaml
@@ -45,6 +45,7 @@ cmd_vel_managed_topic: '/cmd_vel'
 
 # Arm
 lipo_battery_voltage_topic: 'arm/lipo_battery_voltage'
+lipo_battery_percentage_topic: 'arm/lipo_battery_percentage'
 lipo_current_topic: 'arm/lipo_motor_current'
 lipo_battery_refresh_interval: 30  # In secs
 lower_arm_service_name: 'arm/lower'