135 lines
5.7 KiB
Python
135 lines
5.7 KiB
Python
|
|
"""
|
||
|
|
arm.py — easy arm moves with % speed, named positions and homing.
|
||
|
|
|
||
|
|
Set up each arm ONCE (in robot.py):
|
||
|
|
from arm import Arm
|
||
|
|
left_arm = Arm(Motor(Port.C, ...), speed=100,
|
||
|
|
positions={"down": 0, "up": 270},
|
||
|
|
home_backward=True)
|
||
|
|
|
||
|
|
Use it in any mission (always with await):
|
||
|
|
await robot.left_arm.home() # push to the hard stop, call it 0
|
||
|
|
await robot.left_arm.move_to("up") # go to a named position
|
||
|
|
await robot.left_arm.move_to(135) # go to an exact angle
|
||
|
|
await robot.left_arm.move_by(-45) # move 45 degrees from where it is
|
||
|
|
await robot.left_arm.move_for_seconds(1) # run for 1 second
|
||
|
|
await robot.left_arm.move_until_stalled() # run until it's blocked
|
||
|
|
robot.left_arm.reset() # call where it is now 0
|
||
|
|
|
||
|
|
Speed is a PERCENT of this arm's top speed (it's geared, so that's slow).
|
||
|
|
Leave speed out to use the arm's default speed.
|
||
|
|
"""
|
||
|
|
|
||
|
|
from pybricks.parameters import Stop
|
||
|
|
from pybricks.tools import wait, multitask
|
||
|
|
|
||
|
|
|
||
|
|
class Arm:
|
||
|
|
def __init__(self, motor, speed=50, positions=None,
|
||
|
|
home_backward=True, home_angle=0, home_speed=30, home_power=40):
|
||
|
|
"""
|
||
|
|
motor: the Motor(...) for this arm.
|
||
|
|
speed: default % speed when a move gives no speed.
|
||
|
|
positions: names for angles, like {"down": 0, "up": 270}.
|
||
|
|
Angles are measured from the home spot.
|
||
|
|
home_backward: which way the arm turns to reach its hard stop.
|
||
|
|
Check this on the robot before using home()!
|
||
|
|
home_angle: what to call the angle at the hard stop (usually 0).
|
||
|
|
home_speed: % speed for homing. Slow is gentle.
|
||
|
|
home_power: % power limit for homing, so it pushes softly.
|
||
|
|
"""
|
||
|
|
self.motor = motor
|
||
|
|
self.top_speed = motor.control.limits()[0] # deg/s after the gears = 100%
|
||
|
|
self.positions = positions or {}
|
||
|
|
self.home_backward = home_backward
|
||
|
|
self.home_angle = home_angle
|
||
|
|
self.home_speed = home_speed
|
||
|
|
self.home_power = home_power
|
||
|
|
self.set_default_speed(speed)
|
||
|
|
|
||
|
|
# ------------------------------------------------------------ helpers
|
||
|
|
def set_default_speed(self, speed):
|
||
|
|
"""Change this arm's default % speed from now on."""
|
||
|
|
self._deg_s(speed) # just checks it's 1-100
|
||
|
|
self.default_speed = speed
|
||
|
|
|
||
|
|
def _deg_s(self, percent):
|
||
|
|
if percent is None:
|
||
|
|
percent = self.default_speed
|
||
|
|
if not 1 <= percent <= 100:
|
||
|
|
raise ValueError("arm speed is a percent from 1 to 100, but got %s" % percent)
|
||
|
|
return max(1, int(self.top_speed * percent / 100))
|
||
|
|
|
||
|
|
def _target(self, target):
|
||
|
|
if isinstance(target, str):
|
||
|
|
if target not in self.positions:
|
||
|
|
raise ValueError("no arm position named '%s'. Known: %s"
|
||
|
|
% (target, ", ".join(self.positions)))
|
||
|
|
return self.positions[target]
|
||
|
|
return target
|
||
|
|
|
||
|
|
# ------------------------------------------------------------ moves
|
||
|
|
async def move_to(self, target, speed=None):
|
||
|
|
"""Go to a named position ("up") or an exact angle (135), then hold there."""
|
||
|
|
await self.motor.run_target(self._deg_s(speed), self._target(target))
|
||
|
|
|
||
|
|
async def move_by(self, degrees, speed=None):
|
||
|
|
"""Move this many degrees from where it is now (negative = other way)."""
|
||
|
|
await self.motor.run_angle(self._deg_s(speed), degrees)
|
||
|
|
|
||
|
|
async def move_for_seconds(self, seconds, speed=None, backward=False):
|
||
|
|
"""Run for a set time, then hold."""
|
||
|
|
deg_s = self._deg_s(speed)
|
||
|
|
await self.motor.run_time(-deg_s if backward else deg_s, int(seconds * 1000))
|
||
|
|
|
||
|
|
async def move_until_stalled(self, speed=None, backward=False, power=None, max_seconds=3):
|
||
|
|
"""Run until something blocks the arm, then let it relax.
|
||
|
|
|
||
|
|
power: % power limit (1-100), so it pushes softly. None = full power.
|
||
|
|
max_seconds: give up after this long.
|
||
|
|
Returns True if it got blocked, False if it gave up.
|
||
|
|
"""
|
||
|
|
deg_s = self._deg_s(speed)
|
||
|
|
results = await multitask(
|
||
|
|
self.motor.run_until_stalled(-deg_s if backward else deg_s,
|
||
|
|
then=Stop.COAST, duty_limit=power),
|
||
|
|
wait(int(max_seconds * 1000)),
|
||
|
|
race=True,
|
||
|
|
)
|
||
|
|
return results[0] is not None # the stall finished first
|
||
|
|
|
||
|
|
async def home(self):
|
||
|
|
"""Push gently against the hard stop and call that spot home_angle (usually 0).
|
||
|
|
|
||
|
|
Do this at the start, so "up" and "down" mean the same thing every run.
|
||
|
|
Returns True if it found the stop.
|
||
|
|
"""
|
||
|
|
found = await self.move_until_stalled(speed=self.home_speed,
|
||
|
|
backward=self.home_backward,
|
||
|
|
power=self.home_power)
|
||
|
|
self.reset(self.home_angle)
|
||
|
|
if not found:
|
||
|
|
print("Arm never reached its hard stop. Check home_backward in robot.py.")
|
||
|
|
return found
|
||
|
|
|
||
|
|
# ------------------------------------------------------------ info and stopping
|
||
|
|
def reset(self, angle=0):
|
||
|
|
"""Say "the arm is at this angle right now" (default 0).
|
||
|
|
|
||
|
|
Always give a number: Pybricks' own reset_angle() with no number
|
||
|
|
does something different (it uses the motor's built-in marker).
|
||
|
|
"""
|
||
|
|
self.motor.reset_angle(angle)
|
||
|
|
|
||
|
|
def angle(self):
|
||
|
|
"""Where the arm is now, in degrees."""
|
||
|
|
return self.motor.angle()
|
||
|
|
|
||
|
|
def stop(self):
|
||
|
|
"""Let the arm go limp."""
|
||
|
|
self.motor.stop()
|
||
|
|
|
||
|
|
def hold(self):
|
||
|
|
"""Lock the arm where it is."""
|
||
|
|
self.motor.hold()
|