From 2e2a39ccb6043677cf643584bfa4289a63f03d9c Mon Sep 17 00:00:00 2001 From: Swapnil Pande Date: Sat, 4 May 2019 12:02:18 -0500 Subject: [PATCH] Organized digging functions #2 --- .../launch/full_system.launch | 3 + .../scripts/digging_control.py | 261 ++++++++++-------- 2 files changed, 155 insertions(+), 109 deletions(-) diff --git a/robot_mission_control/launch/full_system.launch b/robot_mission_control/launch/full_system.launch index 2ff7ccd..7211a41 100644 --- a/robot_mission_control/launch/full_system.launch +++ b/robot_mission_control/launch/full_system.launch @@ -75,6 +75,9 @@ + + + diff --git a/robot_motor_control/scripts/digging_control.py b/robot_motor_control/scripts/digging_control.py index 5eeaf67..876ad4b 100755 --- 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__":