Files
bioglow_solutions/regionals/arm.py

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