diff --git a/shout-fll-scrimmage/bioglow_mission_2.py b/shout-fll-scrimmage/bioglow_mission_2.py index 066d1b6..5067724 100644 --- a/shout-fll-scrimmage/bioglow_mission_2.py +++ b/shout-fll-scrimmage/bioglow_mission_2.py @@ -6,34 +6,53 @@ import bioglow_helpers as helper from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings async def solve_missions(): # Exploding Seeds - Johannes - + + drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns await multitask( drive_base.arc(150,None,115), - left_arm.run_angle(300,40) + left_arm.run_angle(300,40), + right_arm.run_angle(300,90) ) - await drive_base.straight(280) - await left_arm.run_angle(300,-37) - await drive_base.straight(-55) - await left_arm.run_angle(100,-100) + + # drive to mission until robot will stall + dist_to_model = 350 + rev_move = -1 * dist_to_model + + reach_mission = await helper.drive_until_stalled(dist_to_model) + if reach_mission: + print("Exploding seeds reach.Solving now...") + else: + print(f"Drove full {dist_to_model}, no stall") + + await left_arm.run_angle(100,-40) + #await left_arm.run_until_stalled(-100, then=Stop.HOLD, duty_limit=5) + await drive_base.straight(-60) + await left_arm.run_angle(300,-90) await multitask( - drive_base.straight(-300), - right_arm.run_angle(500,-60), - left_arm.run_angle(500,-50) + drive_base.straight(rev_move), + right_arm.run_angle(500,-90), + left_arm.run_angle(500,-80) ) - await drive_base.turn(-22) - await drive_base.straight(700) + # Go towards tangled mission + helper.set_default_speed() + #helper.set_speed(600,200,100,100) + await drive_base.turn(-23) + await drive_base.straight(700,Stop.COAST) # move fast + + #slowing down here helper.set_speed(100,200,300,200) - await drive_base.straight(100) - await right_arm.run_angle(500,50) - await drive_base.straight(-100) + await drive_base.straight(120) + + await right_arm.run_angle(700,70) #raise arm to untangled + + await drive_base.straight(-100,Stop.COAST) # moving back helper.set_speed(800,500,300,200) + await multitask( - drive_base.straight(-900), - right_arm.run_angle(500,-50) + drive_base.straight(-800), + right_arm.run_angle(300,90) ) - await drive_base.stop - - + drive_base.stop() if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py run_task(solve_missions()) \ No newline at end of file