diff --git a/hazmat_scrimmage/bioglow_mission_2.py b/hazmat_scrimmage/bioglow_mission_2.py index 5067724..493b9a2 100644 --- a/hazmat_scrimmage/bioglow_mission_2.py +++ b/hazmat_scrimmage/bioglow_mission_2.py @@ -6,53 +6,43 @@ 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), - right_arm.run_angle(300,90) - ) - - # 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(rev_move), - right_arm.run_angle(500,-90), - left_arm.run_angle(500,-80) - ) - # 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(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(-800), - right_arm.run_angle(300,90) - ) + drive_base.stop() + drive_base.reset(0,0) + helper.set_speed(800,400,200,100) + await drive_base.straight(20, then=Stop.NONE) + await drive_base.arc(150,None,115, then=Stop.NONE) + await drive_base.straight(270, then=Stop.HOLD) + await left_arm.run_angle(300,70) + await drive_base.straight(-30) + await left_arm.run_angle(100,20) + await drive_base.straight(-300, then=Stop.COAST), + await multitask( + right_arm.run_angle(500,-110), + left_arm.run_angle(500,-100), + drive_base.turn(-40, then=Stop.HOLD) + ) + await drive_base.arc(610,70,None, then=Stop.HOLD) + await drive_base.turn(-78, then=Stop.HOLD) + helper.set_speed(100,200,300,200) + await drive_base.straight(120, then=Stop.COAST) + await right_arm.run_angle(1000,70) + await drive_base.straight(-130, then=Stop.COAST) + helper.set_speed(800,400,200,100) + await multitask( + drive_base.turn(95, then=Stop.HOLD), + right_arm.run_angle(500,-40) + ) + await multitask( + drive_base.straight(160, then=Stop.COAST), + right_arm.run_angle(500,110) + ) + await drive_base.straight(-400, then=Stop.COAST) + await drive_base.turn(100, then=Stop.NONE) + await drive_base.straight(700, then=Stop.COAST) + await 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