def __init__(self):
super(Joystick, self).__init__()
self._on_joystick = set()
self.button_pressed = False
self._angle_A = 0
self.angle_B = 0
self._angle_C = 0
print("Starting search for Joystick...")
self._hub = MoveHub()
self._reset_sensors()
self._hub.button.subscribe(self._on_btn)
self._on_motor_a(self._on_a)
self.on_rotation(self._on_b)
self._on_motor_c(self._on_c)
print("Joystick is ready")
class Boost():
def __init__(self):
self.grip = None
self.hub = MoveHub(get_connection_bleak(hub_mac = "D8EED5BD-D9DA-43C5-97E1-4273F0368182"))
def rotate(self, direction: int, overshoot: bool = False):
overshoot_value = 0
if overshoot:
if direction > 0:
overshoot_value = RTURN_OVERSHOOT
else:
overshoot_value = LTURN_OVERSHOOT
print(direction*QUARTER_TURN + overshoot_value)
res = self.hub.motor_external.angled(direction*QUARTER_TURN + overshoot_value, 0.1)
if overshoot:
res &= self.hub.motor_external.angled(-overshoot_value, 1)
return res
def grip_up(self):
if self.grip == True:
return True
res = self.hub.motor_B.angled(-GRIP_TURN, 0.2)
self.grip = True
return res
def grip_down(self):
if self.grip == False:
return
res = self.hub.motor_B.angled(GRIP_TURN, 0.2)
self.grip = False
return res
def tilt(self):
self.grip_down()
res = self.hub.motor_A.angled(-ROD_TURN, 0.2)
res &= self.hub.motor_A.angled(ROD_TURN, 0.5)
self.grip_up()
return res
def off(self):
self.hub.switch_off()
def dbr_run(frame_queue, key_queue, cond, num, result_queue):
conn = GattConnection()
try:
conn.connect()
hub = MoveHub(conn)
print('Robot connected')
speed = 0.5
dbr.initLicense('t0126lQMAAJKX0RvMyzlh6PuQjcJyenARHjo4+sFqhwweCXfp3hAVHYasqSvCLpym3urmWpADdzSI19PIjSv4RBLR1HkSjR7O0lsOF8wumF0wu2B2wRyCOQRzCOYQzCGYKZgpmCmYKZjzW+sncrPQMG8MWRNv9W/aWNJhfgMslLDp')
while num.value == 1:
print('wait for event')
key = key_queue.get()
if key == ord('q'):
break
try:
if key == ord('c'):
inputframe = frame_queue.get()
results = dbr.decodeBuffer(inputframe, 0x4000000)
if (len(results) > 0):
print(get_time())
print("Total count: " + str(len(results)))
for result in results:
print("Type: " + result[0])
print("Value: " + result[1] + "\n")
result_queue.put(Result(inputframe, results))
elif key == ord('a'):
# left
print('left')
hub.motor_AB.angled(90, speed * -1, speed)
elif key == ord('d'):
# right
print('right')
hub.motor_AB.angled(90, speed, speed * -1)
elif key == ord('w'):
# up
print('up')
hub.motor_AB.start_speed(speed)
elif key == ord('s'):
# down
print('down')
hub.motor_AB.start_speed(speed * -1)
elif key == ord('p'):
print('pause')
hub.motor_AB.stop()
except:
pass
dbr.destroy()
print("Detection is done.")
finally:
conn.disconnect()
def __init__(self):
super(Joystick, self).__init__()
self._on_joystick = set()
self._hub = MoveHub()
self._reset_sensors()
self.button_pressed = False
self._hub.button.subscribe(self._on_btn)
self._angle_A = 0
self._on_motor_a(self._on_a)
self.angle_B = 0
self.on_rotation(self._on_b)
self._angle_C = 0
self._on_motor_c(self._on_c)
logging.info("Done initializing")
def __init__(self, hub_mac=None):
log.info("Searching for LEGO Move Hub (using MAC %s)...", hub_mac)
self.conn = BlueGigaConnection()
if hub_mac == None:
self.conn.connect(hub_name="LEGO Move Hub")
else:
self.conn.connect(hub_mac)
self.hub = MoveHub(self.conn)
log.info("Connected to LEGO Move Hub!")
self._detect_motor()
self.motor_running = None
self.motorA_running = None
def main():
logging.basicConfig(level=logging.INFO)
global motor_state
try:
motor_state = 0
hub = MoveHub()
hub.button.subscribe(button_callback)
#hub.vision_sensor.subscribe(callback, mode=VisionSensor.COLOR_DISTANCE_FLOAT)
while True:
moveslow(hub)
#if motor_state == 1:
# hub.button.unsubscribe()
# hub.disconnect()
#if not hub.connection.is_alive():
# connect()
# hub.vision_sensor.subscribe(callback, mode=VisionSensor.COLOR_DISTANCE_FLOAT)
# print("Reconnection!")
#else:
# print("Connected!")
sleep(3)
except:
print("ex!")
sleep(5)
main()
finally:
#hub.vision_sensor.unsubscribe(callback)
#hub.disconnect()
hub.disconnect()
def __init__(cls):
"""Create the connection with the car"""
cls.connection = get_connection_gatt(hub_mac=cls.MY_MOVEHUB_ADD)
try:
# The motors
cls.movehub = MoveHub(cls.connection)
cls.front_motor = Motor(cls.movehub, cls.movehub.PORT_A)
cls.back_motor = Motor(cls.movehub, cls.movehub.PORT_B)
cls.directionnal_motor = EncodedMotor(cls.movehub,
cls.movehub.PORT_C)
cls.old_angle = cls.DEFAULT_ANGLE
except:
cls.movehub = None
cls.front_motor = None
cls.back_motor = None
cls.directionnal_motor = None
cls.instance = None
cls.connection = None
cls.old_angle = None
def test_capabilities(self):
conn = ConnectionMock()
conn.notifications.append('0f00 04 01 0125000000001000000010')
conn.notifications.append('0f00 04 02 0126000000001000000010')
conn.notifications.append('0f00 04 37 0127000100000001000000')
conn.notifications.append('0f00 04 38 0127000100000001000000')
conn.notifications.append('0900 04 39 0227003738')
conn.notifications.append('0f00 04 32 0117000100000001000000')
conn.notifications.append('0f00 04 3a 0128000000000100000001')
conn.notifications.append('0f00 04 3b 0115000200000002000000')
conn.notifications.append('0f00 04 3c 0114000200000002000000')
conn.notification_delayed('12000101064c45474f204d6f766520487562', 0.1)
conn.notification_delayed('0b00010d06001653a0d1d4', 0.3)
conn.notification_delayed('060001060600', 0.5)
conn.notification_delayed('0600030104ff', 0.7)
MoveHub(conn.connect())
time.sleep(1)
conn.wait_notifications_handled()
self.assertEqual(b"0500010105", conn.writes[1][1])
self.assertEqual(b"0500010d05", conn.writes[2][1])
self.assertEqual(b"0500010605", conn.writes[3][1])
self.assertEqual(b"0500030103", conn.writes[4][1])
def __init__(self):
self.grip = None
self.hub = MoveHub(get_connection_bleak(hub_mac = "D8EED5BD-D9DA-43C5-97E1-4273F0368182"))
if known_face_name != nameOld:
shake_head()
say_text('Hello, ' + known_face_name)
nameOld = known_face_name
else:
# If an unknown face has been detected for a few times, add it to the list
unknown_face_count += 1
if unknown_face_count > unknown_face_limit:
unknown_face_count = 0
add_face(face_encoding)
process_this_frame = not process_this_frame
if __name__ == '__main__':
logging.basicConfig(
level=logging.INFO,
format='%(relativeCreated)d\t%(levelname)s\t%(name)s\t%(message)s')
try:
train_faces()
say_text('Hi, I\'m Verny! Please push on the green button')
movehub = MoveHub()
movehub.vision_sensor.subscribe(wave_callback)
dramatic_turn()
do_image_recognition()
finally:
# Release handle to the webcam
video_capture.release()
say_text('Okay, goodbye!')
movehub.disconnect()
def __init__(self):
super(Automata, self).__init__()
self.__hub = MoveHub()
self.__hub.vision_sensor.subscribe(self.__on_sensor)
self._sensor = []
log.info("Voltage: %s", value)
movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=0)
movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=1)
movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=0)
movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=1)
time.sleep(5)
movehub.current.unsubscribe(callback1)
movehub.voltage.unsubscribe(callback2)
def demo_all(movehub):
demo_voltage(movehub)
demo_led_colors(movehub)
demo_motors_timed(movehub)
demo_motors_angled(movehub)
demo_port_cd_motor(movehub)
demo_tilt_sensor_simple(movehub)
demo_tilt_sensor_precise(movehub)
demo_color_sensor(movehub)
demo_motor_sensors(movehub)
if __name__ == '__main__':
logging.basicConfig(level=logging.INFO)
hub = MoveHub()
demo_all(hub)
hub.disconnect()
import pylgbst
from pylgbst.hub import MoveHub
hub = MoveHub()
for device in hub.peripherals:
print(device)
params['hub_mac'] = parsed.netloc
for key, value in parse_qs(parsed.query).items():
if len(value) == 1:
params[key] = value[0]
else:
params[key] = value
return factory(**params)
if __name__ == '__main__':
logging.basicConfig(
level=logging.INFO,
format='%(relativeCreated)d\t%(levelname)s\t%(name)s\t%(message)s')
parser = get_options()
options = parser.parse_args()
parameters = {}
try:
connection = connection_from_url(
options.connection
) # get_connection_bleak(hub_name=MoveHub.DEFAULT_NAME)
parameters['connection'] = connection
except ValueError as err:
parser.error(err.args[0])
hub = MoveHub(**parameters)
try:
demo = DEMO_CHOICES[options.demo]
demo(hub)
finally:
hub.disconnect()
from pylgbst.hub import MoveHub, Voltage, VisionSensor
from pylgbst import get_connection_gatt
color = 0
def callback(clr, distance):
global color
color = clr
print(color)
conn = get_connection_gatt(hub_mac="00:16:53:A6:60:CC")
hub = MoveHub(conn)
hub.vision_sensor.unsubscribe(callback)
hub.vision_sensor.subscribe(callback, mode=VisionSensor.COLOR_DISTANCE_FLOAT)
p = 0
y = 0
x = 0
mult = 0.6
def penup():
global p
if p != 0:
hub.motor_A.angled(40, 0.8)
p = 0
plotter._transfer_to(0, scale)
elif c == u'1':
plotter._tool_down()
elif c == u'0':
plotter._tool_up()
elif c == u' ':
pass
else:
logging.warning(u"Неизвестная команда: %s", c)
if __name__ == '__main__':
logging.basicConfig(level=logging.DEBUG)
logging.getLogger('').setLevel(logging.DEBUG)
hub = MoveHub() if 1 else get_hub_mock()
plotter = Plotter(hub, 0.75)
FIELD_WIDTH = 0.9
try:
"""
while True:
cmd = six.moves.input("программа> ").decode('utf8')
if not cmd.strip():
continue
plotter.initialize()
interpret_command(cmd, plotter)
plotter.finalize()
"""
class Joystick(object):
RANGE_A = 40
RANGE_C = 30
def __init__(self):
super(Joystick, self).__init__()
self._on_joystick = set()
self.button_pressed = False
self._angle_A = 0
self.angle_B = 0
self._angle_C = 0
print("Starting search for Joystick...")
self._hub = MoveHub()
self._reset_sensors()
self._hub.button.subscribe(self._on_btn)
self._on_motor_a(self._on_a)
self.on_rotation(self._on_b)
self._on_motor_c(self._on_c)
print("Joystick is ready")
def disconnect(self):
print("Joystick disconnects")
self._hub.disconnect()
def _reset_sensors(self):
logging.info("Resetting motor encoders")
self._hub.motor_A.preset_encoder()
self._hub.motor_B.preset_encoder()
self._hub.motor_external.preset_encoder()
def on_button(self, callback):
"""
Notifies about button state change. ``callback(state)`` gets single bool parameter
"""
def wrapper(state):
if state in (0, 1):
callback(bool(state))
self._hub.button.subscribe(wrapper)
def _on_motor_a(self, callback):
def wrapper(angle):
logging.debug("Raw angle: %s", angle)
angle = _clamp(-self.RANGE_A, angle, self.RANGE_A)
callback(angle)
self._hub.motor_A.subscribe(wrapper)
def on_rotation(self, callback):
"""
Notifies about B motor rotation. ``callback(state)`` gets single int parameter from 0 to 359
"""
def wrapper(angle):
logging.debug("Raw angle: %s", angle)
val = angle % 360
val = val if val >= 0 else 360 - val - 1
val = 359 - val
callback(val)
self._hub.motor_B.subscribe(wrapper)
def _on_motor_c(self, callback):
def wrapper(angle):
logging.debug("Raw angle: %s", angle)
angle = _clamp(-self.RANGE_C, angle, self.RANGE_C)
callback(angle)
self._hub.motor_external.subscribe(wrapper)
def _on_btn(self, state):
self.button_pressed = bool(state)
def _on_a(self, angle):
logging.debug("A rotated: %s", angle)
self._angle_A = angle
self._calc_joystick()
def _on_b(self, angle):
logging.debug("B rotated: %s", angle)
self.angle_B = angle
def _on_c(self, angle):
logging.debug("C rotated: %s", angle)
self._angle_C = angle
self._calc_joystick()
def on_joystick(self, callback):
"""
Notifies about joystick change. ``callback(speed, direction)`` gets parameters:
- ``speed`` - int value from 0 to 10
- ``direction`` - int value from 0 to 359
"""
self._on_joystick.add(callback)
def _calc_joystick(self):
norm_a = self._angle_A / self.RANGE_A
norm_b = self._angle_C / self.RANGE_C
logging.debug("%s / %s", self._angle_A, self._angle_C)
logging.debug("%s / %s", norm_a, norm_b)
speed = math.sqrt(norm_a**2 + norm_b**2) # / math.sqrt(2)
speed = _clamp(-1.0, speed, 1.0)
maxsize = sys.maxsize if norm_a >= 0 else -sys.maxsize
direction = math.atan(norm_a / norm_b if norm_b else maxsize)
direction *= 180 / math.pi
if norm_a >= 0 and norm_b >= 0:
direction = 90 - direction
elif norm_a < 0 and norm_b >= 0:
direction = 90 - direction
elif norm_a < 0 and norm_b < 0:
direction = 270 - direction
else:
direction = 270 - direction
for callback in self._on_joystick:
callback(int(round(10 * speed)), int(direction))
class Joystick(object):
def __init__(self):
super(Joystick, self).__init__()
self._on_joystick = set()
self._hub = MoveHub()
self._reset_sensors()
self.button_pressed = False
self._hub.button.subscribe(self._on_btn)
self._angle_A = 0
self._on_motor_a(self._on_a)
self.angle_B = 0
self.on_rotation(self._on_b)
self._angle_C = 0
self._on_motor_c(self._on_c)
logging.info("Done initializing")
def disconnect(self):
self._hub.disconnect()
def _reset_sensors(self):
logging.info("Resetting motor encoders")
self._hub.motor_A.preset_encoder()
self._hub.motor_B.preset_encoder()
self._hub.motor_external.preset_encoder()
def on_button(self, callback):
"""
Notifies about button state change. ``callback(state)`` gets single bool parameter
"""
def wrapper(state):
if state in (0, 1):
callback(bool(state))
self._hub.button.subscribe(wrapper)
def _on_motor_a(self, callback):
def wrapper(angle):
logging.debug("Raw angle: %s", angle)
spread = 25
angle = _clamp(-spread, angle, spread)
callback(angle)
self._hub.motor_A.subscribe(wrapper)
def on_rotation(self, callback):
"""
Notifies about B motor rotation. ``callback(state)`` gets single int parameter from 0 to 359
"""
def wrapper(angle):
logging.debug("Raw angle: %s", angle)
val = angle % 360
callback(val if val >= 0 else 360 - val)
self._hub.motor_B.subscribe(wrapper)
def _on_motor_c(self, callback):
def wrapper(angle):
logging.debug("Raw angle: %s", angle)
spread = 25
angle = _clamp(-spread, angle, spread)
callback(angle)
self._hub.motor_external.subscribe(wrapper)
def _on_btn(self, state):
self.button_pressed = bool(state)
def _on_a(self, angle):
logging.debug("A rotated: %s", angle)
self._angle_A = angle
self._calc_joystick()
def _on_b(self, angle):
logging.debug("B rotated: %s", angle)
self.angle_B = angle
def _on_c(self, angle):
logging.debug("C rotated: %s", angle)
self._angle_C = angle
self._calc_joystick()
def on_joystick(self, callback):
"""
Notifies about joystick change. ``callback(speed, direction)`` gets parameters:
- ``speed`` - float value from 0.0 to 1.0
- ``direction`` - int value from 0 to 359
"""
self._on_joystick.add(callback)
def _calc_joystick(self):
norm_a = self._angle_A / 25
norm_b = self._angle_C / 25
logging.info("%s / %s", self._angle_A, self._angle_C)
logging.info("%s / %s", norm_a, norm_b)
speed = math.sqrt(norm_a**2 + norm_b**2) / math.sqrt(2)
speed = _clamp(-1.0, speed, 1.0)
maxsize = sys.maxsize if norm_a >= 0 else -sys.maxsize
direction = math.atan(norm_a / norm_b if norm_b else maxsize)
direction *= 180 / math.pi
if norm_a >= 0 and norm_b >= 0:
logging.info("Sector 1")
direction = 90 - direction
elif norm_a < 0 and norm_b >= 0:
logging.info("Sector 2")
direction = 90 - direction
elif norm_a < 0 and norm_b < 0:
logging.info("Sector 3")
direction = 270 - direction
else:
logging.info("Sector 4")
direction = 270 - direction
for callback in self._on_joystick:
callback(speed, 359 - int(direction))
