Organized digging functions #2

vanderbiltrobotics · May 4, 2019 · 2e2a39c · 2e2a39c
1 parent e81c6e5
commit 2e2a39c
Show file tree

Hide file tree

Showing 2 changed files with 155 additions and 109 deletions.
diff --git a/robot_mission_control/launch/full_system.launch b/robot_mission_control/launch/full_system.launch
@@ -75,6 +75,9 @@
 
         <!-- Start state machine -->
         <!-- param name="dig_angle" value="unkown"/ -->
+        <!-- param name="dig_velocity" value="unknown"/ -->
+        <!-- param name="dig_complete" value="false"/ -->
+
         <!-- node name="DIG CONTROLLER" pkg="DIG CONTROLLER" type="DIG CONTROLLER" / -->
 
 

diff --git a/robot_motor_control/scripts/digging_control.py b/robot_motor_control/scripts/digging_control.py
@@ -6,7 +6,7 @@
 
 class DigManager:
 
-    def __init__(self, ):
+    def __init__(self):
 
         # Current dig mode - can be "teleop" or "auto"
         self.dig_mode = "teleop"
@@ -88,165 +88,208 @@ def move_to_digging_angle():
         # Use set_hinge_angle function to move to desired location
         return
 
-    # Runs one of the primary digging operations
-    def run_dig_operation(self, operation):
+    def set_lead_screw_position():
+        # Read in the desired lead screw position
+        return
 
+    def set_lead_screw_velocity():
+        # Read in the desired lead screw velocity
+        return
 
-        # Motors off
-        if self.dig_mode == "off":
-            self.update_speeds()
+    def set_bucket_chain_velocity():
+        # Read in the desired bucket chain velocity
+        return
 
-        # Raise hinge angle (make more vertical)
-        if self.dig_mode == "raise_hinge":
 
-            ##TODO switch to hinge_to_dig_position()
-            # hinge_to_dig_position()
+    # Motors off
+    def dig_op_off():
+        self.update_speeds()
 
-            # Run hinge motor until correect upright angle achieved
-            start_time = rospy.Time.now()
+    # Raise hinge angle (make more vertical)
+    def dig_op_raise_hinge():
 
-            # Turn hinge motor on
-            self.update_speeds(
-                hng=0.2
-            )
+        ##TODO switch to hinge_to_dig_position()
+        # hinge_to_dig_position()
 
-            # Define loop rate
-            loop_rate = rospy.Rate(10)
+        # Run hinge motor until correect upright angle achieved
+        start_time = rospy.Time.now()
 
-            # Spin motors for the specified duration
-            while rospy.Time.now() - start_time < 3.0:
-                loop_rate.sleep()
+        # Turn hinge motor on
+        self.update_speeds(
+            hng=0.2
+        )
 
-            # Turn motors off
-            self.update_speeds()
+        # Define loop rate
+        loop_rate = rospy.Rate(10)
 
-        # Lower bucket chain and dig - not collecting material yet
-        if self.dig_mode == "dig":
+        # Spin motors for the specified duration
+        while rospy.Time.now() - start_time < 3.0:
+            loop_rate.sleep()
+
+        # Turn motors off
+        self.update_speeds()
 
-            # Run bucket chain motor and lead screw motor
-            start_time = rospy.Time.now()
+    # Lower bucket chain and dig - not collecting material yet
+    def dig_op_dig():
 
-            # Define loop rate
-            loop_rate = rospy.Rate(10)
+        # Run bucket chain motor and lead screw motor
+        start_time = rospy.Time.now()
 
-            # --- LOWER BUCKET CHAIN TO GRAVEL DEPTH --- #
+        # Define loop rate
+        loop_rate = rospy.Rate(10)
 
-            # Turn bucket chain motor, lead screw motor, conveyor motors on
+        # --- LOWER BUCKET CHAIN TO GRAVEL DEPTH --- #
+        # Use set_bucket_chain_velocity()
+
+        # Turn bucket chain motor, lead screw motor, conveyor motors on
+        self.update_speeds(
+            lsc=0.2,
+            bkt=0.2,
+            cnv=0.2
+        )
+
+        # Wait until gravel depth reached
+        while rospy.Time.now() - start_time < 3.0:
+            loop_rate.sleep()
+
+        # --- COLLECT MATERIAL --- #
+
+        collection_steps = 10
+        pulse_conveyor_time = 1.0
+        pulse_collect_time = 2.0
+
+        # Advance conveyor in 10 steps
+        for i in range(collection_steps):
+
+            # Turn conveyor on
             self.update_speeds(
-                lsc=0.2,
+                lsc=0.1,
                 bkt=0.2,
                 cnv=0.2
             )
 
-            # Wait until gravel depth reached
-            while rospy.Time.now() - start_time < 3.0:
+            # Wait for specified pulse duration
+            pulse_start = rospy.Time.now()
+
+            while rospy.Time.now() - pulse_start < pulse_conveyor_time:
                 loop_rate.sleep()
 
-            # --- COLLECT MATERIAL --- #
+            # Turn conveyor off
+            self.update_speeds(
+                lsc=0.1,
+                bkt=0.2
+            )
 
-            collection_steps = 10
-            pulse_conveyor_time = 1.0
-            pulse_collect_time = 2.0
+            # Collect material for specified collect duration
+            pulse_start = rospy.Time.now()
 
-            # Advance conveyor in 10 steps
-            for i in range(collection_steps):
+            while rospy.Time.now() - pulse_start < pulse_collect_time:
+                loop_rate.sleep()
 
-                # Turn conveyor on
-                self.update_speeds(
-                    lsc=0.1,
-                    bkt=0.2,
-                    cnv=0.2
-                )
+            # Turn motors off
+            self.update_speeds()
 
-                # Wait for specified pulse duration
-                pulse_start = rospy.Time.now()
+        # Turn motors off
+        self.update_speeds()
 
-                while rospy.Time.now() - pulse_start < pulse_conveyor_time:
-                    loop_rate.sleep()
+    # Raise bucket chain, don't dig
+    def dig_op_raise_bucket_chain():
 
-                # Turn conveyor off
-                self.update_speeds(
-                    lsc=0.1,
-                    bkt=0.2
-                )
+        # Run bucket chain motor and lead screw motor
+        start_time = rospy.Time.now()
 
-                # Collect material for specified collect duration
-                pulse_start = rospy.Time.now()
+        # Define loop rate
+        loop_rate = rospy.Rate(10)
 
-                while rospy.Time.now() - pulse_start < pulse_collect_time:
-                    loop_rate.sleep()
+        # --- LOWER BUCKET CHAIN TO GRAVEL DEPTH --- #
+        # Use set_bucket_chain_velocity
 
-                # Turn motors off
-                self.update_speeds()
+        # Retract bucket chain by running lead screw motor backwards
+        self.update_speeds(
+            lsc=-0.2
+        )
 
-            # Turn motors off
-            self.update_speeds()
+        # Wait until we've reached retracted position
+        while rospy.Time.now() - start_time < 3.0:
+            loop_rate.sleep()
 
-        # Raise bucket chain, don't dig
-        if self.dig_mode == "raise_bucket_chain":
+        # Turn motors off
+        self.update_speeds()
 
-            # Run bucket chain motor and lead screw motor
-            start_time = rospy.Time.now()
+    # Lower hinge angle (make less vertical)
+    def dig_op_lower_hinge():
 
-            # Define loop rate
-            loop_rate = rospy.Rate(10)
+        # Run hinge motor until correect upright angle achieved
+        start_time = rospy.Time.now()
 
-            # --- LOWER BUCKET CHAIN TO GRAVEL DEPTH --- #
+        # Turn hinge motor on
+        self.update_speeds(
+            hng=-0.2
+        )
 
-            # Retract bucket chain by running lead screw motor backwards
-            self.update_speeds(
-                lsc=-0.2
-            )
+        # Define loop rate
+        loop_rate = rospy.Rate(10)
 
-            # Wait until we've reached retracted position
-            while rospy.Time.now() - start_time < 3.0:
-                loop_rate.sleep()
+        # Spin motors for the specified duration
+        while rospy.Time.now() - start_time < 3.0:
+            loop_rate.sleep()
 
-            # Turn motors off
-            self.update_speeds()
+        # Turn motors off
+        self.update_speeds()
 
-        # Lower hinge angle (make less vertical)
-        if self.dig_mode == "lower_hinge":
+    # Depositing material - conveyor only
+    def dig_op_deposit_material():
 
-            # Run hinge motor until correect upright angle achieved
-            start_time = rospy.Time.now()
+        # Run conveyor for 5 seconds to empty all material
+        start_time = rospy.Time.now()
 
-            # Turn hinge motor on
-            self.update_speeds(
-                hng=-0.2
-            )
+        # Turn motors on
+        self.update_speeds(
+            cnv=0.2
+        )
 
-            # Define loop rate
-            loop_rate = rospy.Rate(10)
+        # Define loop rate
+        loop_rate = rospy.Rate(10)
 
-            # Spin motors for the specified duration
-            while rospy.Time.now() - start_time < 3.0:
-                loop_rate.sleep()
+        # Spin motors for the specified duration
+        while rospy.Time.now() - start_time < 5.0:
 
-            # Turn motors off
-            self.update_speeds()
+            loop_rate.sleep()
 
-        # Depositing material - conveyor only
-        if self.dig_mode == "deposit_material":
+        # Turn motors off
+        self.update_speeds()
 
-            # Run conveyor for 5 seconds to empty all material
-            start_time = rospy.Time.now()
+    # Runs one of the primary digging operations
+    def run_dig_operation(self, operation):
 
-            # Turn motors on
-            self.update_speeds(
-                cnv=0.2
-            )
 
-            # Define loop rate
-            loop_rate = rospy.Rate(10)
+        # Motors off
+        if self.dig_mode == "off":
+            self.dig_op_off()
 
-            # Spin motors for the specified duration
-            while rospy.Time.now() - start_time < 5.0:
+        # Raise hinge angle (make more vertical)
+        elif self.dig_mode == "raise_hinge":
+            self.dig_op_raise_hinge()
 
-                loop_rate.sleep()
+        # Lower bucket chain and dig - not collecting material yet
+        elif self.dig_mode == "dig":
+            self.dig_op_dig()
 
-            # Turn motors off
-            self.update_speeds()
+        # Raise bucket chain, don't dig
+        elif self.dig_mode == "raise_bucket_chain":
+            self.dig_op_raise_bucket_chain()
+
+        # Lower hinge angle (make less vertical)
+        elif self.dig_mode == "lower_hinge":
+            self.dig_op_lower_hinge()
+
+        # Depositing material - conveyor only
+        elif self.dig_mode == "deposit_material":
+            self.dig_op_deposit_material()
+
+        else:
+            # Error or something
+            return
 
 
 if __name__ == "__main__":