31 lines
1.1 KiB
Python
31 lines
1.1 KiB
Python
|
|
"""
|
||
|
|
arm_calibrate.py — find the real angles for "up", "down", and any other arm position.
|
||
|
|
|
||
|
|
How to use:
|
||
|
|
1. Open this file in Pybricks Code and press Run.
|
||
|
|
2. Each arm pushes gently to its hard stop (that spot becomes 0).
|
||
|
|
If an arm spins the wrong way, stop the program and flip home_backward in robot.py.
|
||
|
|
3. The arms go limp. Move them BY HAND to each position you want.
|
||
|
|
4. Read the angles printed below and copy them into positions={...} in robot.py.
|
||
|
|
5. Press the hub's center button (or Stop) to finish.
|
||
|
|
"""
|
||
|
|
|
||
|
|
import robot
|
||
|
|
|
||
|
|
|
||
|
|
async def calibrate():
|
||
|
|
print("Homing arms...")
|
||
|
|
left_ok, right_ok = await robot.multitask(robot.left_arm.home(), robot.right_arm.home())
|
||
|
|
print("Left arm found its stop:", left_ok, " Right arm found its stop:", right_ok)
|
||
|
|
|
||
|
|
robot.left_arm.stop() # go limp so you can move them by hand
|
||
|
|
robot.right_arm.stop()
|
||
|
|
print("Move the arms by hand. Angles:")
|
||
|
|
|
||
|
|
while True:
|
||
|
|
print("left_arm:", robot.left_arm.angle(), " right_arm:", robot.right_arm.angle())
|
||
|
|
await robot.wait(500)
|
||
|
|
|
||
|
|
|
||
|
|
robot.run_task(calibrate())
|