Added function to check stalled motors
This commit is contained in:
@@ -180,3 +180,34 @@ async def multilift_down(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