fix more 127 -> RPM thresholds

lunabot_behavior/plunge.py

@@ -51,7 +51,7 @@ def __init__(self, excavation_publisher: rospy.Publisher = None, lin_act_publish
 
     def check_exc_stuck(self, exc_msg):
         # check if trying to go somewhere
-        if exc_msg >= 15:
+        if exc_msg >= 300:
             # check if not going anywhere
             if self.robot_sensors.exc_vel <= 0.1:
                 self.exc_stuck = True

lunabot_behavior/stuck.py

@@ -52,7 +52,7 @@ def stuck(self):
             error_publisher.publish(stuck_msg)
 
             # check if trying to go somewhere
-            if self.robot_effort.left_drive >= 15 or self.robot_effort.right_drive >= 15: #TODO RJN - these might need to be higher
+            if self.robot_effort.left_drive >= 15 or self.robot_effort.right_drive >= 300:
                 # check if not going anywhere
                 if self.robot_sensors.drive_left_vel <= 0.1 and self.robot_sensors.drive_right_vel <= 0.1:
                     if not currently_stuck:

lunabot_control/scripts/excavation_controller.py

@@ -27,7 +27,7 @@ def is_overflow(self):
 
 
 class ExcavationController:
-    FORWARD_EXCAVATE = -1
+    FORWARD_EXCAVATE = -5000
     DOWN_LEAD_SCREW = -0.2
     UP_LEAD_SCREW = 0.7
 
@@ -39,7 +39,7 @@ def __init__(self):
         self.lead_screw_curr = None
         self.retract = False
 
-        self.exc_voltage = self.FORWARD_EXCAVATE
+        self.exc_rpm = self.FORWARD_EXCAVATE
         self.lead_screw_voltage = self.DOWN_LEAD_SCREW
 
         self.exc_leaky_bucket = LeakyBucket(self.is_exc_stuck, dec=20)
@@ -73,7 +73,7 @@ def __init__(self):
                     else:
                         self.lead_screw_voltage = self.DOWN_LEAD_SCREW
                 else:
-                    self.exc_voltage = 0
+                    self.exc_rpm = 0
 
                 if lead_screw_overflow:
                     if self.lead_screw_max_extend():
@@ -116,7 +116,7 @@ def publish_effort(self):
             self.lead_screw_voltage, max_percent=self.max_lead_screw_percent
         )
         effort_msg.excavate = self.constrain_RPM(
-            self.exc_voltage, max_percent=self.max_exc_percent
+            self.exc_rpm, max_percent=self.max_exc_percent
         )
         self._effort_pub.publish(effort_msg)
 
@@ -126,7 +126,7 @@ def constrain(self, val, max_percent):
 
     def constrain_RPM(self, val, max_percent):
         val = np.clip(-1, val, 1)  # Clipping speed to not go over 100%
-        return np.int32(val * 1000 * max_percent)
+        return np.int32(val * 5000 * max_percent) #TODO RJN - check this threshold
 
 
     def stop(self):

lunabot_control/scripts/manual_controller.py

@@ -106,7 +106,7 @@ def __init__(self):
         self.latched_excavation_speed = 0
         self.excavation_is_latched = False
 
-        self.DEPOSITION_SPEED = 127
+        self.DEPOSITION_SPEED = 3000 #TODO RJN - this speed
         self.ACTUATE_SPEED = 0.8 # percentage of max power
 
         self.publish = True