Added function to check stalled motors
This commit is contained in:
@@ -179,4 +179,35 @@ async def multilift_down(rotation_angle):
|
|||||||
right_arm.run_angle(1000,-1*rotation_angle),
|
right_arm.run_angle(1000,-1*rotation_angle),
|
||||||
left_arm.run_angle(1000,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())
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user