diff --git a/shout-fll-scrimmage/bioglow_helpers.py b/shout-fll-scrimmage/bioglow_helpers.py index 47a200d..21dc5bd 100644 --- a/shout-fll-scrimmage/bioglow_helpers.py +++ b/shout-fll-scrimmage/bioglow_helpers.py @@ -179,4 +179,35 @@ async def multilift_down(rotation_angle): right_arm.run_angle(1000,-1*rotation_angle), left_arm.run_angle(1000,rotation_angle) ) + +async def drive_until_stalled(max_distance): + stalled_flag = False + + async def drive_task(): + await drive_base.straight(max_distance) + + async def stall_watch(): + nonlocal stalled_flag + while not (left_motor.control.stalled() and right_motor.control.stalled()): + await wait(10) + stalled_flag = True + drive_base.stop() + print("Both wheels stalled — robot is stuck!") + + await multitask(drive_task(), stall_watch(), race=True) + return stalled_flag + +def reset_arms(): + left_arm.reset_angle(0) + right_arm.reset_angle(0) + print('limits before:') + print(left_arm.control.limits()) + print(left_arm.control.limits()) + + left_arm.control.limits(speed=1100, acceleration=2000) + right_arm.control.limits(speed=1100, acceleration=2000) + + print('limits after:') + print(left_arm.control.limits()) + print(left_arm.control.limits())