mirror of
https://github.com/pybricks/pybricks-projects.git
synced 2026-07-28 04:06:07 +00:00
sets/mindstorms-robot-inventor/main-models/gelo: new programs
This adds new programs for Gelo. Inspired by the official LEGO programs.
This commit is contained in:
committed by
laurensvalk
parent
b60589abf4
commit
6543d63b00
@@ -0,0 +1,22 @@
|
||||
---
|
||||
title: "A Simple Gelo Program"
|
||||
maintainer:
|
||||
user: "TheVinhLuong102"
|
||||
name: "The Lương-Phạm Family"
|
||||
image:
|
||||
local: "../gelo.jpg"
|
||||
credit: "LEGO"
|
||||
code: "#program"
|
||||
---
|
||||
|
||||
|
||||
## Program
|
||||
|
||||
The playing instructions for each robot variant are in the docstrings of the corresponding file.
|
||||
|
||||
The code for Gelo's basic walk is in `gelo_basic.py` as follows:
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo_basic.py %}
|
||||
```
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 44 KiB |
@@ -0,0 +1,510 @@
|
||||
from pybricks.hubs import InventorHub
|
||||
from pybricks.geometry import Axis
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
|
||||
from pybricks.parameters import Color, Direction, Port, Side
|
||||
from pybricks.tools import wait, StopWatch
|
||||
|
||||
#######################################################
|
||||
# Constants
|
||||
#######################################################
|
||||
|
||||
_CW = Direction.CLOCKWISE
|
||||
_CCW = Direction.COUNTERCLOCKWISE
|
||||
|
||||
# index to limits() tuple
|
||||
_TORQUE = const(2)
|
||||
|
||||
_BEEP_START = const(1)
|
||||
_BEEP_END = const(2)
|
||||
_BEEP_ERROR = const(3)
|
||||
|
||||
DEFAULT_COLORS = [
|
||||
Color.RED,
|
||||
Color.GREEN,
|
||||
Color.BLUE,
|
||||
Color.YELLOW,
|
||||
Color.NONE,
|
||||
]
|
||||
|
||||
#######################################################
|
||||
# Helper context managers
|
||||
#######################################################
|
||||
|
||||
|
||||
class Acceleration:
|
||||
"""
|
||||
Context manager for temporarily changing acceleration
|
||||
of the legs.
|
||||
|
||||
Args:
|
||||
legs:
|
||||
A list of legs.
|
||||
accel:
|
||||
The acceleration in mm/s/s.
|
||||
"""
|
||||
def __init__(self, legs: list[Motor], accel: int):
|
||||
self._legs = legs
|
||||
self._accel = accel
|
||||
self._limits = {}
|
||||
|
||||
def __enter__(self):
|
||||
for leg in self._legs:
|
||||
self._limits[leg] = leg.control.limits()
|
||||
leg.control.limits(acceleration=self._accel)
|
||||
|
||||
def __exit__(self, exc, value, trace):
|
||||
for leg in self._legs:
|
||||
leg.control.limits(*self._limits[leg])
|
||||
|
||||
|
||||
class IgnoreException:
|
||||
"""
|
||||
A context manager that supresses exceptions.
|
||||
"""
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exc, value, trace):
|
||||
# Don't supress BaseException like SystemExit
|
||||
# or KeyboardInterrupt.
|
||||
if exc:
|
||||
return issubclass(exc, Exception)
|
||||
|
||||
|
||||
#######################################################
|
||||
# Helper functions
|
||||
#######################################################
|
||||
|
||||
|
||||
def copy_sign(x, y):
|
||||
"""
|
||||
Like math.copysign(), but for integers.
|
||||
"""
|
||||
if y < 0:
|
||||
return -x
|
||||
|
||||
return x
|
||||
|
||||
|
||||
def wait_gen(time):
|
||||
"""
|
||||
Yields until time has elapsed.
|
||||
|
||||
Args:
|
||||
time:
|
||||
The length of time to wait in milliseconds.
|
||||
"""
|
||||
timer = StopWatch()
|
||||
|
||||
while timer.time() < time:
|
||||
yield
|
||||
|
||||
|
||||
def wait_until_gen(condition):
|
||||
"""
|
||||
Yields until condition is met.
|
||||
|
||||
Args:
|
||||
condition:
|
||||
A function that returns ``False`` to keep waiting
|
||||
and ``True`` when the condition is met.
|
||||
"""
|
||||
while not condition():
|
||||
yield
|
||||
|
||||
#######################################################
|
||||
# Main class
|
||||
#######################################################
|
||||
|
||||
|
||||
class Gelo:
|
||||
"""
|
||||
Object used to control the Gelo model from the Robot Inventor set.
|
||||
"""
|
||||
def __init__(self):
|
||||
self.hub = InventorHub(front_side=-Axis.X)
|
||||
self.hub.display.orientation(Side.BOTTOM)
|
||||
|
||||
self.back_right = Motor(Port.A, _CCW)
|
||||
self.back_left = Motor(Port.B, _CW)
|
||||
self.front_right = Motor(Port.C, _CCW)
|
||||
self.front_left = Motor(Port.D, _CW)
|
||||
self.color = ColorSensor(Port.F)
|
||||
self.ultrasonic = UltrasonicSensor(Port.E)
|
||||
|
||||
self._all_legs = [
|
||||
self.back_right,
|
||||
self.back_left,
|
||||
self.front_right,
|
||||
self.front_left,
|
||||
]
|
||||
|
||||
self._back_legs = [
|
||||
self.back_right,
|
||||
self.back_left,
|
||||
]
|
||||
|
||||
self._front_legs = [
|
||||
self.front_right,
|
||||
self.front_left,
|
||||
]
|
||||
|
||||
self._leg_map = {
|
||||
"all": self._all_legs,
|
||||
"front": self._front_legs,
|
||||
"back": self._back_legs,
|
||||
}
|
||||
|
||||
# 90% of full load for all motors
|
||||
self._near_max_load = 90 * sum([
|
||||
l.control.limits()[_TORQUE] for l in self._all_legs
|
||||
]) // 100
|
||||
|
||||
self._zero = {
|
||||
self.back_right: 0,
|
||||
self.back_left: 0,
|
||||
self.front_right: 0,
|
||||
self.front_left: 0,
|
||||
}
|
||||
|
||||
def __enter__(self):
|
||||
self.color.lights.off()
|
||||
self.ultrasonic.lights.on(10)
|
||||
self._beep(_BEEP_START)
|
||||
|
||||
return self
|
||||
|
||||
def __exit__(self, exc, value, trace):
|
||||
# if there was an exception we will indicate
|
||||
# indicate that the program stopped because
|
||||
# of an error
|
||||
error = exc and issubclass(exc, Exception)
|
||||
|
||||
if error:
|
||||
self.hub.light.on(Color.RED)
|
||||
|
||||
for leg in self._all_legs:
|
||||
with IgnoreException():
|
||||
leg.stop()
|
||||
|
||||
with IgnoreException():
|
||||
self.ultrasonic.lights.off()
|
||||
|
||||
with IgnoreException():
|
||||
self.color.lights.off()
|
||||
|
||||
wait(500)
|
||||
|
||||
if error:
|
||||
self._beep(_BEEP_ERROR)
|
||||
else:
|
||||
self._beep(_BEEP_END)
|
||||
|
||||
def _beep(self, kind):
|
||||
"""
|
||||
Beep indications for program start and end.
|
||||
|
||||
Args:
|
||||
kind:
|
||||
The kind of beep.
|
||||
"""
|
||||
if kind == _BEEP_ERROR:
|
||||
for b in [100, 50]:
|
||||
self.hub.speaker.beep(b, 300)
|
||||
else:
|
||||
beeps = [100, 200, 300]
|
||||
|
||||
if kind == _BEEP_END:
|
||||
beeps = reversed(beeps)
|
||||
|
||||
for b in beeps:
|
||||
self.hub.speaker.beep(b)
|
||||
|
||||
def _steer(self, steer: int) -> dict[int, int]:
|
||||
"""
|
||||
Creates a steering offset map.
|
||||
|
||||
Args:
|
||||
steer:
|
||||
The amount steer. 90 for left,
|
||||
0 for straight and -90 for right.
|
||||
"""
|
||||
steer = max(-90, min(steer, 90))
|
||||
|
||||
# A: 0, B: 180, C: 180, D: 0 will walk straight.
|
||||
# A: 90, B 180, C: 90, D: 0 will turn in place to the right
|
||||
# A: 0, B 270, C: 180, D: 270 will turn in place to the left
|
||||
|
||||
return {
|
||||
self.back_right: -steer if steer < 0 else 0,
|
||||
self.back_left: (180 + steer) if steer > 0 else 180,
|
||||
self.front_right: (180 + steer) if steer < 0 else 180,
|
||||
self.front_left: (360 - steer) if steer > 0 else 0,
|
||||
}
|
||||
|
||||
def _reset_legs(self, offset: dict[int, int], absolute: bool):
|
||||
"""
|
||||
Resets the angle measurement of all of the legs to zero.
|
||||
|
||||
Args:
|
||||
offset:
|
||||
The offset mapping that determines the
|
||||
phase of each leg.
|
||||
absolute:
|
||||
If true, use the absolute position of
|
||||
the motor instead of zero.
|
||||
"""
|
||||
|
||||
for leg in self._all_legs:
|
||||
# Reset to position read by absolute encoder.
|
||||
leg.reset_angle()
|
||||
|
||||
# Ensure legs travel shortest distance to reach opposition
|
||||
# after init. Often, the legs will coast past 180 when
|
||||
# stopping so when the program restarts, they come up
|
||||
# with and angle of -176 degrees, for example, and would
|
||||
# have to rotate nearly 360 degrees to reach opposition.
|
||||
if offset[leg] - leg.angle() > 180:
|
||||
leg.reset_angle(leg.angle() + 360)
|
||||
|
||||
if not absolute:
|
||||
# Adjust the angle of each so that the average of all
|
||||
# legs is 0 while keeping the relative position of each
|
||||
# motor.
|
||||
avg = sum([
|
||||
l.angle() - offset[l] for leg in self._all_legs
|
||||
]) // len(self._all_legs)
|
||||
|
||||
for leg in self._all_legs:
|
||||
leg.reset_angle(leg.angle() - avg)
|
||||
|
||||
def _track_target(self, offset: dict[int, int], target: int):
|
||||
"""
|
||||
Runs ``Motor.track_target`` for all legs at the same time.
|
||||
|
||||
Args:
|
||||
offset:
|
||||
The offset map containing the phase for each leg in
|
||||
degrees.
|
||||
target:
|
||||
The target angle for the legs in degrees.
|
||||
"""
|
||||
for leg in self._all_legs:
|
||||
leg.track_target(target + offset[leg])
|
||||
|
||||
def oppose_legs_gen(self, speed=400):
|
||||
"""
|
||||
Yields until legs are in opposing (walking) positions.
|
||||
"""
|
||||
offset = self._steer(0)
|
||||
|
||||
self._reset_legs(offset, absolute=False)
|
||||
|
||||
for leg in self._all_legs:
|
||||
leg.run_target(speed, offset[leg], wait=False)
|
||||
|
||||
while not all(map(Motor.done, self._all_legs)):
|
||||
yield
|
||||
|
||||
def oppose_legs(self, speed=400):
|
||||
"""
|
||||
Moves legs to opposing (walking) positions.
|
||||
"""
|
||||
for _ in self.oppose_legs_gen(speed):
|
||||
wait(10)
|
||||
|
||||
def stand_gen(self, speed=400):
|
||||
"""
|
||||
Yields until all legs have moved to the standing position.
|
||||
|
||||
Args:
|
||||
The speed to turn the motors in degrees per second.
|
||||
"""
|
||||
self._reset_legs(self._zero, absolute=True)
|
||||
|
||||
for leg in self._front_legs:
|
||||
leg.run_target(speed, -45, wait=False)
|
||||
|
||||
for leg in self._back_legs:
|
||||
leg.run_target(speed, -90, wait=False)
|
||||
|
||||
while not all(map(Motor.done, self._all_legs)):
|
||||
yield
|
||||
|
||||
def stand(self, speed=400):
|
||||
"""
|
||||
Moves legs to the standing position.
|
||||
|
||||
Args:
|
||||
The speed to turn the motors in degrees per second.
|
||||
"""
|
||||
for _ in self.stand_gen(speed):
|
||||
wait(10)
|
||||
|
||||
def _move_legs_gen(self, legs, angle, speed):
|
||||
for leg in legs:
|
||||
leg.run_angle(speed, angle, wait=False)
|
||||
|
||||
while not all(map(Motor.done, legs)):
|
||||
yield
|
||||
|
||||
def walk_gen(self, speed=800, steer=0):
|
||||
"""
|
||||
Yields forever while moving the legs in a walking motion.
|
||||
"""
|
||||
offset = self._steer(steer)
|
||||
|
||||
self._reset_legs(offset, absolute=False)
|
||||
|
||||
# The actual fastest rate the motors can turn on
|
||||
# Gelo is somewhere between 700 and 900 deg/sec
|
||||
# depedning on battery voltage and terrain.
|
||||
# We will adjust the rate down if needed.
|
||||
target_rate = max(-900, min(speed, 900))
|
||||
|
||||
timer = StopWatch()
|
||||
|
||||
while True:
|
||||
angle = target_rate * timer.time() // 1000
|
||||
self._track_target(offset, angle)
|
||||
|
||||
load = sum(map(Motor.load, self._all_legs))
|
||||
|
||||
# If we are trying to go faster than the motors
|
||||
# can actually turn, we need reduce the rate so
|
||||
# that they can keep up.
|
||||
if abs(load) > self._near_max_load:
|
||||
target_rate -= copy_sign(10, target_rate)
|
||||
|
||||
yield
|
||||
|
||||
def walk(self, time=5000, speed=800, steer=0):
|
||||
"""
|
||||
Moves legs in a walking motion for a certain amount of time.
|
||||
|
||||
Args:
|
||||
time:
|
||||
The length of time to walk in milliseconds.
|
||||
speed:
|
||||
The speed to walk at. Values between 100 and 800 work
|
||||
best. Making the speed negative will walk backwards.
|
||||
steer:
|
||||
How much the robot should veer to the left or right.
|
||||
Allowable values are -90 to +90. Positive values will
|
||||
turn left and negative values will turn right.
|
||||
"""
|
||||
for _ in zip(self.walk_gen(speed, steer), wait_gen(time)):
|
||||
wait(10)
|
||||
|
||||
def walk_until(self, condition, speed=800, steer=0):
|
||||
"""
|
||||
Moves legs in a walking motion until condition is met.
|
||||
|
||||
Args:
|
||||
condition:
|
||||
A function that returns ``False`` when walking
|
||||
should continue and ``True`` when walking should stop.
|
||||
speed:
|
||||
The speed to walk at. Values between 100 and 800 work
|
||||
best. Making the speed negative will walk backwards.
|
||||
steer:
|
||||
How much the robot should veer to the left or right.
|
||||
Allowable values are -90 to +90. Positive values will
|
||||
turn left and negative values will turn right.
|
||||
"""
|
||||
for _ in zip(
|
||||
self.walk_gen(speed, steer),
|
||||
wait_until_gen(condition),
|
||||
):
|
||||
wait(10)
|
||||
|
||||
def kick_gen(self, angle, speed=1000, legs="all"):
|
||||
"""
|
||||
Yields until the legs have kicked (moved with high
|
||||
acceleration).
|
||||
|
||||
Args:
|
||||
angle:
|
||||
The relative angle to move the legs in degrees.
|
||||
speed:
|
||||
The speed to move the legs in degrees per second.
|
||||
legs:
|
||||
The legs to kick. Can be "all", "front" or "back".
|
||||
Default is "all" legs.
|
||||
"""
|
||||
legs = self._leg_map[legs]
|
||||
with Acceleration(legs, 10_000):
|
||||
for _ in self._move_legs_gen(legs, angle, speed):
|
||||
yield
|
||||
|
||||
def kick(self, angle, speed=1000, legs="all"):
|
||||
"""
|
||||
Kicks the legs (moves with high acceleration).
|
||||
|
||||
Args:
|
||||
angle:
|
||||
The relative angle to move the legs in degrees.
|
||||
speed:
|
||||
The speed to move the legs in degrees per second.
|
||||
legs:
|
||||
The legs to kick. Can be "all", "front" or "back".
|
||||
Default is "all" legs.
|
||||
"""
|
||||
for _ in self.kick_gen(angle, speed, legs):
|
||||
wait(10)
|
||||
|
||||
def color_gen(self, colors=DEFAULT_COLORS):
|
||||
"""
|
||||
Yields currently detected color and sets light to match.
|
||||
|
||||
Args:
|
||||
color:
|
||||
Optional list of colors to detect.
|
||||
Default is red, green, blue, yellow.
|
||||
|
||||
Yields:
|
||||
The detected color.
|
||||
"""
|
||||
if Color.NONE not in colors:
|
||||
raise ValueError(
|
||||
"must include Color.NONE in list of colors"
|
||||
)
|
||||
|
||||
self.color.detectable_colors(colors)
|
||||
|
||||
while True:
|
||||
color = self.color.color()
|
||||
self.hub.light.on(color)
|
||||
|
||||
yield color
|
||||
|
||||
def wait_color(self, colors=DEFAULT_COLORS):
|
||||
"""
|
||||
Waits for a color to be detected.
|
||||
|
||||
Args:
|
||||
color:
|
||||
Optional list of colors to detect.
|
||||
Default is red, green, blue, yellow.
|
||||
|
||||
Returns:
|
||||
The detected color.
|
||||
"""
|
||||
for color in self.color_gen(colors):
|
||||
if color != Color.NONE:
|
||||
return color
|
||||
|
||||
wait(10)
|
||||
|
||||
|
||||
#######################################################
|
||||
# Simple program
|
||||
#######################################################
|
||||
|
||||
# This file is mainly used as an import by other files
|
||||
# but if we run this file, it will still do something.
|
||||
|
||||
if __name__ == "__main__":
|
||||
with Gelo() as gelo:
|
||||
gelo.walk()
|
||||
@@ -0,0 +1,204 @@
|
||||
from pybricks.hubs import InventorHub
|
||||
from pybricks.pupdevices import Remote
|
||||
from pybricks.parameters import Button, Color
|
||||
from pybricks.tools import wait, StopWatch
|
||||
|
||||
from gelo import Gelo, copy_sign
|
||||
|
||||
|
||||
#######################################################
|
||||
# State management
|
||||
#######################################################
|
||||
|
||||
_SPEED_MAP = {
|
||||
0: 0,
|
||||
1: 100,
|
||||
2: 300,
|
||||
3: 500,
|
||||
4: 800,
|
||||
}
|
||||
|
||||
_STEER_MAP = {
|
||||
0: 0,
|
||||
1: 23,
|
||||
2: 45,
|
||||
3: 68,
|
||||
4: 90,
|
||||
}
|
||||
|
||||
|
||||
class State:
|
||||
def __init__(self):
|
||||
self.speed_index = 0
|
||||
self.steer_index = 0
|
||||
self.timer = StopWatch()
|
||||
|
||||
def speed(self):
|
||||
return copy_sign(_SPEED_MAP[abs(self.speed_index)], self.speed_index)
|
||||
|
||||
def steer(self):
|
||||
return copy_sign(_STEER_MAP[abs(self.steer_index)], self.steer_index)
|
||||
|
||||
def inc_speed(self):
|
||||
if self.speed_index < 4:
|
||||
self.speed_index += 1
|
||||
|
||||
def dec_speed(self):
|
||||
if self.speed_index > -4:
|
||||
self.speed_index -= 1
|
||||
|
||||
def inc_steer(self):
|
||||
if self.steer_index < 4:
|
||||
self.steer_index += 1
|
||||
|
||||
def dec_steer(self):
|
||||
if self.steer_index > -4:
|
||||
self.steer_index -= 1
|
||||
|
||||
|
||||
#######################################################
|
||||
# Helper functions
|
||||
#######################################################
|
||||
|
||||
def pressed_oneshot_gen(remote: Remote):
|
||||
"""
|
||||
Yields a list of buttons that were pressed
|
||||
since the last iteration.
|
||||
"""
|
||||
previous = ()
|
||||
|
||||
while True:
|
||||
pressed = remote.buttons.pressed()
|
||||
oneshot = []
|
||||
|
||||
for b in pressed:
|
||||
if b not in previous:
|
||||
oneshot.append(b)
|
||||
|
||||
yield oneshot
|
||||
|
||||
previous = pressed
|
||||
|
||||
|
||||
def handle_pressed(buttons: list[Button], state: State) -> bool:
|
||||
"""
|
||||
Updates the state based on any new button presses.
|
||||
|
||||
Returns: True if the state changed, otherwise false.
|
||||
"""
|
||||
# Prefer stopping if multiple buttons are pressed
|
||||
# at the same time.
|
||||
if Button.LEFT in buttons or Button.RIGHT in buttons:
|
||||
state.speed_index = 0
|
||||
state.steer_index = 0
|
||||
return True
|
||||
|
||||
if Button.LEFT_PLUS in buttons:
|
||||
state.inc_speed()
|
||||
return True
|
||||
|
||||
if Button.LEFT_MINUS in buttons:
|
||||
state.dec_speed()
|
||||
return True
|
||||
|
||||
if Button.RIGHT_PLUS in buttons:
|
||||
state.inc_steer()
|
||||
return True
|
||||
|
||||
if Button.RIGHT_MINUS in buttons:
|
||||
state.dec_steer()
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def update_display(hub: InventorHub, state: State):
|
||||
"""
|
||||
Updates the display on the hub based on the current state.
|
||||
"""
|
||||
i = state.speed_index
|
||||
j = state.steer_index
|
||||
|
||||
if i == 0 and j == 0:
|
||||
pulse = state.timer.time() // 10 % 200
|
||||
|
||||
if pulse > 100:
|
||||
pulse = 200 - pulse
|
||||
|
||||
half_pulse = pulse // 2
|
||||
|
||||
hub.display.pixel(0, 2, 0)
|
||||
hub.display.pixel(1, 2, half_pulse)
|
||||
hub.display.pixel(3, 2, half_pulse)
|
||||
hub.display.pixel(4, 2, 0)
|
||||
|
||||
hub.display.pixel(2, 2, pulse)
|
||||
|
||||
hub.display.pixel(2, 0, 0)
|
||||
hub.display.pixel(2, 1, half_pulse)
|
||||
hub.display.pixel(2, 3, half_pulse)
|
||||
hub.display.pixel(2, 4, 0)
|
||||
else:
|
||||
# speed indication
|
||||
hub.display.pixel(0, 2, 100 if i > 3 else (50 if i > 2 else 0))
|
||||
hub.display.pixel(1, 2, 100 if i > 1 else (50 if i > 0 else 0))
|
||||
hub.display.pixel(3, 2, 100 if i < -1 else (50 if i < 0 else 0))
|
||||
hub.display.pixel(4, 2, 100 if i < -3 else (50 if i < -2 else 0))
|
||||
|
||||
# center pixel
|
||||
hub.display.pixel(2, 2, 100)
|
||||
|
||||
# steering indication
|
||||
hub.display.pixel(2, 0, 100 if j > 3 else (50 if j > 2 else 0))
|
||||
hub.display.pixel(2, 1, 100 if j > 1 else (50 if j > 0 else 0))
|
||||
hub.display.pixel(2, 3, 100 if j < -1 else (50 if j < 0 else 0))
|
||||
hub.display.pixel(2, 4, 100 if j < -3 else (50 if j < -2 else 0))
|
||||
|
||||
|
||||
def idle_gen():
|
||||
"""
|
||||
Yields forever (doesn't do anything).
|
||||
"""
|
||||
while True:
|
||||
yield
|
||||
|
||||
|
||||
def get_action(gelo: Gelo, state: State):
|
||||
"""
|
||||
Gets an action based on the current state.
|
||||
"""
|
||||
if state.speed_index == 0:
|
||||
return idle_gen()
|
||||
|
||||
return gelo.walk_gen(state.speed(), state.steer())
|
||||
|
||||
|
||||
#######################################################
|
||||
# Main program
|
||||
#######################################################
|
||||
|
||||
|
||||
with Gelo() as gelo:
|
||||
# yellow indicates we are waiting for remote
|
||||
gelo.hub.light.on(Color.YELLOW)
|
||||
|
||||
remote = Remote()
|
||||
|
||||
# green indicates that remote is connected
|
||||
gelo.hub.light.on(Color.GREEN)
|
||||
|
||||
# inital state
|
||||
state = State()
|
||||
action_iter = idle_gen()
|
||||
pressed_iter = pressed_oneshot_gen(remote)
|
||||
|
||||
while True:
|
||||
# when a button is pressed, select a new action
|
||||
if handle_pressed(next(pressed_iter), state):
|
||||
action_iter = get_action(gelo, state)
|
||||
|
||||
# update the outputs based on the current state
|
||||
update_display(gelo.hub, state)
|
||||
next(action_iter)
|
||||
|
||||
wait(10)
|
||||
@@ -0,0 +1,15 @@
|
||||
from gelo import Gelo
|
||||
|
||||
gelo = Gelo()
|
||||
|
||||
# Normally, we don't call methods with
|
||||
# double-underscores directly, but this
|
||||
# is an exceptional case!
|
||||
gelo.__enter__()
|
||||
|
||||
# A KeyboardInterrupt will stop the
|
||||
# program and start the interactive
|
||||
# prompt. You can also trigger this
|
||||
# in any running program by pressing
|
||||
# CTRL+C in the terminal.
|
||||
raise KeyboardInterrupt
|
||||
@@ -0,0 +1,24 @@
|
||||
from pybricks.parameters import Icon
|
||||
from urandom import choice
|
||||
from gelo import Gelo
|
||||
|
||||
with Gelo() as gelo:
|
||||
# If the ultrasonic sensor measures less than
|
||||
# 30 cm (1 ft), then we are too close!
|
||||
def too_close():
|
||||
return gelo.ultrasonic.distance() < 300
|
||||
|
||||
while True:
|
||||
# walk until the ultrasonic sensor detects an obstruction
|
||||
gelo.hub.display.icon(Icon.ARROW_UP)
|
||||
gelo.walk_until(too_close)
|
||||
|
||||
# randomly turn left or right to avoid the obstruction
|
||||
direction = choice(["left", "right"])
|
||||
|
||||
if choice == "left":
|
||||
gelo.hub.display.icon(Icon.ARROW_LEFT)
|
||||
gelo.walk(steer=90, time=4000)
|
||||
else:
|
||||
gelo.hub.display.icon(Icon.ARROW_RIGHT)
|
||||
gelo.walk(steer=-90, time=4000)
|
||||
@@ -0,0 +1,9 @@
|
||||
from pybricks.parameters import Button, Color, Icon
|
||||
from pybricks.tools import wait, StopWatch
|
||||
|
||||
from gelo import Gelo
|
||||
|
||||
|
||||
with Gelo() as gelo:
|
||||
# Add your code here
|
||||
gelo.walk()
|
||||
@@ -0,0 +1,137 @@
|
||||
"""
|
||||
Use the color sensor to command Gelo to do tricks!
|
||||
|
||||
When the program runs, hold one of the colors on
|
||||
the color "bone" (red, green, blue or yellow) in
|
||||
front of the color sensor to see Gelo perform a
|
||||
trick for you.
|
||||
|
||||
This works best when Gelo is on carpet.
|
||||
"""
|
||||
|
||||
from pybricks.parameters import Color, Side
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.geometry import Axis
|
||||
|
||||
from gelo import Gelo
|
||||
|
||||
|
||||
##################################################
|
||||
# Fancy Python stuff!
|
||||
##################################################
|
||||
|
||||
# map of colors to trick functions
|
||||
tricks = {}
|
||||
|
||||
|
||||
def trick(color):
|
||||
"""
|
||||
Decorator to assign colors to a trick.
|
||||
"""
|
||||
def decorator(func):
|
||||
tricks[color] = func
|
||||
|
||||
return func
|
||||
|
||||
return decorator
|
||||
|
||||
|
||||
##################################################
|
||||
# Define one trick for each color!
|
||||
##################################################
|
||||
|
||||
|
||||
@trick(Color.BLUE)
|
||||
def buck(gelo: Gelo):
|
||||
"""
|
||||
Tells Gelo to kick up its back legs.
|
||||
"""
|
||||
# tip over
|
||||
gelo.kick(130)
|
||||
|
||||
# kick back legs
|
||||
gelo.kick(470, legs="back")
|
||||
|
||||
# stand back up
|
||||
gelo.kick(-120, legs="front")
|
||||
|
||||
|
||||
@trick(Color.GREEN)
|
||||
def headstand(gelo: Gelo):
|
||||
"""
|
||||
Tells Gelo to stand on its head.
|
||||
"""
|
||||
# tip over
|
||||
gelo.kick(220)
|
||||
|
||||
# hold the position for a bit
|
||||
wait(1500)
|
||||
|
||||
# go back down
|
||||
gelo.kick(-20)
|
||||
wait(200)
|
||||
gelo.kick(-250)
|
||||
wait(500)
|
||||
|
||||
|
||||
@trick(Color.RED)
|
||||
def flip(gelo: Gelo):
|
||||
"""
|
||||
Tells Gelo to flip all the way over on
|
||||
its back.
|
||||
"""
|
||||
# big kick to flip over
|
||||
gelo.kick(300)
|
||||
|
||||
|
||||
@trick(Color.YELLOW)
|
||||
def spin(gelo: Gelo):
|
||||
"""
|
||||
Tells Gelo to spin around in a circle.
|
||||
"""
|
||||
timer = StopWatch()
|
||||
rate = 0
|
||||
|
||||
# TODO: replace this when we get a proper
|
||||
# imu.heading() method. This is not very
|
||||
# accurate due to the low sample rate and
|
||||
# wild movements.
|
||||
def full_circle():
|
||||
nonlocal rate
|
||||
|
||||
# integrate average rate over time to get angle
|
||||
rate = (
|
||||
99 * rate + gelo.hub.imu.angular_velocity(Axis.Z)
|
||||
) / 100
|
||||
angle = rate * timer.time() / 1000
|
||||
|
||||
return angle >= 360
|
||||
|
||||
gelo.walk_until(full_circle, steer=90)
|
||||
|
||||
|
||||
##################################################
|
||||
# The main program!
|
||||
##################################################
|
||||
|
||||
|
||||
with Gelo() as gelo:
|
||||
while True:
|
||||
# make sure Gelo is right-side up
|
||||
# before continuing
|
||||
while gelo.hub.imu.up() != Side.TOP:
|
||||
gelo.hub.speaker.beep(50)
|
||||
wait(2000)
|
||||
|
||||
# get in "ready" position
|
||||
gelo.stand()
|
||||
|
||||
# wait until a color is detected
|
||||
color = gelo.wait_color()
|
||||
gelo.hub.speaker.beep()
|
||||
|
||||
# look up the trick to perform
|
||||
do_trick = tricks[color]
|
||||
|
||||
# perform the trick
|
||||
do_trick(gelo)
|
||||
@@ -1,28 +1,42 @@
|
||||
---
|
||||
title: "Gelo"
|
||||
maintainer:
|
||||
user: "TheVinhLuong102"
|
||||
name: "The Lương-Phạm Family"
|
||||
user: "pybricks"
|
||||
name: "The Pybricks Team"
|
||||
image:
|
||||
local: "gelo.jpg"
|
||||
credit: "LEGO"
|
||||
video:
|
||||
youtube: "5Fa4m1XzlCA"
|
||||
description:
|
||||
"A real life four-legged robot. Its unique mechanism means it can walk, avoid obstacles, and even perform tricks."
|
||||
"Gelo is a 4-legged robot that operates autonomously or via remote control. It can even do tricks!"
|
||||
building_instructions:
|
||||
external: https://www.lego.com/cdn/product-assets/product.bi.additional.main.pdf/51515_Gelo.pdf
|
||||
code: "#program"
|
||||
code: "#gelo-module"
|
||||
---
|
||||
|
||||
## Activities
|
||||
|
||||
## Program
|
||||
These activities are similar to the starter programs available in the official
|
||||
LEGO app.
|
||||
|
||||
The playing instructions for each robot variant are in the docstrings of the corresponding file.
|
||||
First copy the [Gelo module](#gelo-module) below and save it in Pybricks Code.
|
||||
It is used by all of the activities. Then follow one of the links below.
|
||||
|
||||
The code for Gelo's basic walk is in `gelo-basic.py` as follows:
|
||||
- [Roam around](./roam): Gelo walks around uses the ultrasonic sensor to avoid obstacles.
|
||||
- [Tricks](./tricks): Use the color sensor to tell Gelo to do a trick for you.
|
||||
- [Remote control](./remote): Use the LEGO Powered Up remote to control Gelo.
|
||||
- [Command prompt](./repl): Use the interactive command prompt in Pybricks Code to control Gelo.
|
||||
- [Make your own program](./template): A starter template for writing your own program.
|
||||
|
||||
## Gelo module
|
||||
|
||||
Save this program as `gelo.py` in Pybricks Code. It is used in the
|
||||
[activities](#activities) above.
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo-basic.py %}
|
||||
{% include_relative gelo.py %}
|
||||
```
|
||||
|
||||
## Basic program
|
||||
|
||||
If you are looking for something less complex, try out this [basic](./basic) program.
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
---
|
||||
title: "Remote control Gelo"
|
||||
maintainer:
|
||||
user: "pybricks"
|
||||
name: "The Pybricks Team"
|
||||
image:
|
||||
local: "../gelo.jpg"
|
||||
credit: "LEGO"
|
||||
building_instructions:
|
||||
external: https://www.lego.com/cdn/product-assets/product.bi.additional.main.pdf/51515_Gelo.pdf
|
||||
code: "#program"
|
||||
---
|
||||
|
||||
## Instructions
|
||||
|
||||
This program requires the LEGO Powered Up remote control. The left <kbd>+</kbd>
|
||||
and <kbd>-</kbd> buttons control the speed and the right <kbd>+</kbd> and
|
||||
<kbd>-</kbd> buttons control the steering. Either red button will make Gelo stop.
|
||||
|
||||

|
||||
|
||||
This script makes use of the [gelo.py](../#gelo-module) module, so make
|
||||
sure to save that program in Pybricks Code first.
|
||||
|
||||
Then save the script below as `gelo_remote.py` and run it.
|
||||
|
||||
When the program starts, the light will turn yellow. This means Gelo is waiting
|
||||
to connect to the remote control. Turn on the remote and it will connect
|
||||
automatically and the light on Gelo will turn green. Then you can press the
|
||||
buttons on the remote to control Gelo.
|
||||
|
||||
## Program
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo_remote.py %}
|
||||
```
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 76 KiB |
@@ -0,0 +1,50 @@
|
||||
---
|
||||
title: Command prompt
|
||||
description: Control Gelo with the interactive command prompt
|
||||
maintainer:
|
||||
user: "pybricks"
|
||||
name: "The Pybricks Team"
|
||||
image:
|
||||
local: ../gelo.jpg
|
||||
credit: LEGO
|
||||
building_instructions:
|
||||
external: https://www.lego.com/cdn/product-assets/product.bi.additional.main.pdf/51515_Gelo.pdf
|
||||
code: "#program"
|
||||
---
|
||||
|
||||
## Using the command prompt
|
||||
|
||||
Python has an interactive command prompt. This is also sometimes called the
|
||||
REPL (Read Evaluate Print Loop). It can be used to run Python code as you
|
||||
type it in.
|
||||
|
||||
Save the program below as `gelo_repl.py` in Pybricks Code. And make sure you
|
||||
have the [gelo.py](../#main-program) saved there too.
|
||||
|
||||
Then connect to Gelo and run the program. In the terminal window in Pybricks
|
||||
Code, you will see a prompt like this:
|
||||
|
||||
```
|
||||
>>>
|
||||
```
|
||||
|
||||
Type in a command like this and press enter:
|
||||
|
||||
```
|
||||
>>> gelo.walk()
|
||||
```
|
||||
|
||||
### Tips
|
||||
|
||||
* To save some typing, after typing `gelo.`, you can press the <kbd>tab</kbd>
|
||||
key to provide a list of available methods. Then type the first few letters
|
||||
and press <kbd>tab</kbd> again to complete the name.
|
||||
* To cancel a running command, you can press <kbd>ctrl</kbd>+<kbd>c</kbd>.
|
||||
|
||||
|
||||
## Program
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo_repl.py %}
|
||||
```
|
||||
@@ -0,0 +1,35 @@
|
||||
---
|
||||
title: Obstacle avoidance with Gelo
|
||||
description: Gelo can autonomously roam around and avoid obstacles by using the ultrasonic sensor.
|
||||
maintainer:
|
||||
user: "pybricks"
|
||||
name: "The Pybricks Team"
|
||||
image:
|
||||
local: ../gelo.jpg
|
||||
credit: LEGO
|
||||
code: "#program"
|
||||
---
|
||||
|
||||
## Quick start
|
||||
|
||||
This script makes use of the [gelo.py](../#gelo-module) module, so make
|
||||
sure to save that program in Pybricks Code first.
|
||||
|
||||
Then save the script below as `gelo_roam.py` and run it.
|
||||
|
||||
Gelo will walk forward until it "sees" an obstacle. Then it will randomly turn
|
||||
left or right then start walking again until it sees the next obstacle.
|
||||
|
||||
## Program
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo_roam.py %}
|
||||
```
|
||||
|
||||
## Change it up
|
||||
|
||||
Try changing the program to attack obstacles instead avoiding them.
|
||||
|
||||
* Change the main loop to turn until an obstacle is detected.
|
||||
* Then walk towards the obstacle until Gelo crashes into it!
|
||||
@@ -0,0 +1,31 @@
|
||||
---
|
||||
title: Make your own Gelo program
|
||||
description: A basic template for your own Gelo program.
|
||||
maintainer:
|
||||
user: "pybricks"
|
||||
name: "The Pybricks Team"
|
||||
image:
|
||||
local: ../gelo.jpg
|
||||
credit: LEGO
|
||||
building_instructions:
|
||||
external: https://www.lego.com/cdn/product-assets/product.bi.additional.main.pdf/51515_Gelo.pdf
|
||||
code: "#program"
|
||||
---
|
||||
|
||||
|
||||
## Program
|
||||
|
||||
Use this script as a starting point for your own Gelo program.
|
||||
|
||||
This script makes use of the [gelo.py](../#gelo-module) module, so make
|
||||
sure to save that program in Pybricks Code first.
|
||||
|
||||
### Tip
|
||||
|
||||
In Pybricks Code, type `gelo` and then `.` to see what Gelo can do and get help
|
||||
on what parameters you can pass to the methods.
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo_template.py %}
|
||||
```
|
||||
@@ -0,0 +1,35 @@
|
||||
---
|
||||
title: Make Gelo Do Tricks
|
||||
description: Use the color sensor to make Gelo perform a trick.
|
||||
maintainer:
|
||||
user: "pybricks"
|
||||
name: "The Pybricks Team"
|
||||
image:
|
||||
local: ../gelo.jpg
|
||||
credit: LEGO
|
||||
building_instructions:
|
||||
external: https://www.lego.com/cdn/product-assets/product.bi.additional.main.pdf/51515_Gelo.pdf
|
||||
code: "#program"
|
||||
---
|
||||
|
||||
## Instructions
|
||||
|
||||
This script makes use of the [gelo.py](../#gelo-module) module, so make
|
||||
sure to save that program in Pybricks Code first.
|
||||
|
||||
Then save the script below as `gelo_tricks.py` and run it.
|
||||
|
||||

|
||||
|
||||
|
||||
Wave one of the colors on the bone in front of the color sensor on Gelo and
|
||||
watch which trick he does.
|
||||
|
||||
The tricks work best when Gelo is on carpet where he can get a good grip.
|
||||
|
||||
## Program
|
||||
|
||||
{% include copy-code.html %}
|
||||
```python
|
||||
{% include_relative gelo_tricks.py %}
|
||||
```
|
||||
Reference in New Issue
Block a user