Added function to check stalled motors

This commit is contained in:
2026-10-20 01:34:16 +00:00
parent c54feaad7d
commit eead961fa0

View File

@@ -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())