Added run until stalled
This commit is contained in:
@@ -7,33 +7,52 @@ from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... al
|
|||||||
|
|
||||||
async def solve_missions(): # Exploding Seeds - Johannes
|
async def solve_missions(): # Exploding Seeds - Johannes
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
await multitask(
|
await multitask(
|
||||||
drive_base.arc(150,None,115),
|
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)
|
# drive to mission until robot will stall
|
||||||
await drive_base.straight(-55)
|
dist_to_model = 350
|
||||||
await left_arm.run_angle(100,-100)
|
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(
|
await multitask(
|
||||||
drive_base.straight(-300),
|
drive_base.straight(rev_move),
|
||||||
right_arm.run_angle(500,-60),
|
right_arm.run_angle(500,-90),
|
||||||
left_arm.run_angle(500,-50)
|
left_arm.run_angle(500,-80)
|
||||||
)
|
)
|
||||||
await drive_base.turn(-22)
|
# Go towards tangled mission
|
||||||
await drive_base.straight(700)
|
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)
|
helper.set_speed(100,200,300,200)
|
||||||
await drive_base.straight(100)
|
await drive_base.straight(120)
|
||||||
await right_arm.run_angle(500,50)
|
|
||||||
await drive_base.straight(-100)
|
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)
|
helper.set_speed(800,500,300,200)
|
||||||
|
|
||||||
await multitask(
|
await multitask(
|
||||||
drive_base.straight(-900),
|
drive_base.straight(-800),
|
||||||
right_arm.run_angle(500,-50)
|
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
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
run_task(solve_missions())
|
run_task(solve_missions())
|
||||||
Reference in New Issue
Block a user