├── .gitignore ├── tools ├── ansi.py ├── gateway_test.py ├── spikejsonrpc.py └── gateway.py ├── .vscode ├── settings.json └── launch.json ├── programs ├── beep.py ├── balance.py ├── drive.py ├── crazy.py ├── house.py └── b2.py ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Python 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # VS Code 6 | .vscode/* 7 | !.vscode/settings.json 8 | !.vscode/tasks.json 9 | !.vscode/launch.json 10 | !.vscode/extensions.json 11 | *.code-workspace 12 | 13 | # Project specifiv 14 | *.log -------------------------------------------------------------------------------- /tools/ansi.py: -------------------------------------------------------------------------------- 1 | 2 | class AnsiEscapeCode: 3 | def __init__(self, pattern): 4 | self.pattern = pattern 5 | 6 | def __format__(self, spec): 7 | return self.pattern.format(spec) 8 | 9 | 10 | esc = AnsiEscapeCode("\033[{0}") 11 | color = AnsiEscapeCode("\033[{0}m") 12 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.formatting.autopep8Args": [ 3 | "--max-line-length", 4 | "119" 5 | ], 6 | "python.testing.unittestArgs": [ 7 | "-v", 8 | "-s", 9 | "./tools", 10 | "-p", 11 | "*_test.py" 12 | ], 13 | "python.testing.pytestEnabled": false, 14 | "python.testing.nosetestsEnabled": false, 15 | "python.testing.unittestEnabled": true 16 | } -------------------------------------------------------------------------------- /programs/beep.py: -------------------------------------------------------------------------------- 1 | from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App 2 | from mindstorms.control import wait_for_seconds, wait_until, Timer 3 | from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to 4 | import math 5 | 6 | 7 | hub = MSHub() 8 | 9 | hub.speaker.beep() 10 | 11 | print('This text will be displayed in the console.') 12 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "Python: Run Gateway", 9 | "type": "python", 10 | "request": "launch", 11 | "program": "${file}", 12 | "args": [ 13 | "--file", 14 | "data/hub-trace.bin", 15 | "--nolog" 16 | ], 17 | "console": "integratedTerminal" 18 | } 19 | ] 20 | } -------------------------------------------------------------------------------- /programs/balance.py: -------------------------------------------------------------------------------- 1 | from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App 2 | from mindstorms.control import wait_for_seconds, wait_until, Timer 3 | from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to 4 | import math 5 | import time 6 | 7 | # First experiments with a balancing robot... no yet finished. 8 | # See https://medium.com/@janislavjankov/self-balancing-robot-with-lego-spike-prime-ac156af5c2b2 for a more 9 | # complete example. 10 | 11 | wheel_radius = 2.8 12 | 13 | # Create your objects here. 14 | hub = MSHub() 15 | 16 | motor_pair = MotorPair('B', 'F') 17 | motor_pair.set_default_speed(50) 18 | motor_pair.set_motor_rotation(wheel_radius * 2 * math.pi, 'cm') 19 | 20 | while True: 21 | r = hub.motion_sensor.get_roll_angle() + 90 22 | motor_pair.start(speed = - r*40) 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /tools/gateway_test.py: -------------------------------------------------------------------------------- 1 | 2 | import unittest 3 | import gateway 4 | import tempfile 5 | 6 | 7 | class NoopLoggerTestCase(unittest.TestCase): 8 | def setUp(self): 9 | self.log = gateway.NoopLogger() 10 | 11 | def test_input(self): 12 | # just ensure method can be called silently 13 | self.log.input("an input line") 14 | 15 | def test_output(self): 16 | # just ensure method can be called silently 17 | self.log.output("an output line") 18 | 19 | class FileLoggerTestCase(unittest.TestCase): 20 | def setUp(self): 21 | self.file = tempfile.NamedTemporaryFile(delete=True) 22 | self.log = gateway.FileLogger(self.file.name) 23 | 24 | def test_input(self): 25 | self.log.input(b"an input line") 26 | 27 | line = self.file.read(1024) 28 | self.assertEqual(line, b"< an input line\n") 29 | 30 | def test_output(self): 31 | self.log.output(b"an output line") 32 | 33 | line = self.file.read(1024) 34 | self.assertEqual(line, b"> an output line\n") 35 | 36 | 37 | if __name__ == '__main__': 38 | unittest.main() 39 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Christian Kumpe 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /programs/drive.py: -------------------------------------------------------------------------------- 1 | from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App 2 | from mindstorms.control import wait_for_seconds, wait_until, Timer 3 | from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to 4 | import math 5 | 6 | # driving with Tricky 7 | 8 | wheel_radius = 2.8 9 | pen_radius = 8.6 10 | axis_radius = 5.6 11 | 12 | line_length = 5 13 | 14 | # Create your objects here. 15 | hub = MSHub() 16 | 17 | 18 | # Write your program here. 19 | hub.speaker.beep() 20 | 21 | motor = Motor('C') 22 | motor.run_to_position(165) 23 | 24 | 25 | def pen_up(): 26 | motor.run_for_degrees(-90) 27 | 28 | 29 | def pen_down(): 30 | motor.run_for_degrees(90) 31 | 32 | 33 | motor_pair = MotorPair('B', 'A') 34 | motor_pair.set_default_speed(20) 35 | motor_pair.set_motor_rotation(wheel_radius * 2 * math.pi, 'cm') 36 | 37 | pen_down() 38 | 39 | motor_pair.move_tank(axis_radius*math.pi * 2.05, 'cm', 40 | left_speed=-25, right_speed=25) 41 | 42 | #pen_up() 43 | #pen_down() 44 | 45 | motor_pair.move(-pen_radius * 2, 'cm') 46 | pen_up() 47 | 48 | motor_pair.move(pen_radius * 2, 'cm') 49 | 50 | motor_pair.move_tank(axis_radius*math.pi * - 0.5, 'cm', 51 | left_speed=-25, right_speed=25) 52 | 53 | pen_down() 54 | motor_pair.move(-pen_radius * 2, 'cm') 55 | 56 | pen_up() 57 | 58 | hub.speaker.beep() 59 | hub.light_matrix.write('done') 60 | -------------------------------------------------------------------------------- /programs/crazy.py: -------------------------------------------------------------------------------- 1 | from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App 2 | from mindstorms.control import wait_for_seconds, wait_until, Timer 3 | from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to 4 | import math 5 | import random 6 | 7 | # crazy drawings with Tricky 8 | 9 | wheel_radius = 2.8 10 | pen_radius = 8.6 11 | axis_radius = 5.6 12 | 13 | line_length = 5 14 | 15 | # Create your objects here. 16 | hub = MSHub() 17 | 18 | 19 | # Write your program here. 20 | hub.speaker.beep() 21 | 22 | motor = Motor('C') 23 | motor.run_to_position(165) 24 | 25 | 26 | def pen_up(): 27 | motor.run_for_degrees(-90) 28 | 29 | 30 | def pen_down(): 31 | motor.run_for_degrees(90) 32 | 33 | 34 | motor_pair = MotorPair('B', 'A') 35 | motor_pair.set_default_speed(50) 36 | motor_pair.set_motor_rotation(wheel_radius * 2 * math.pi, 'cm') 37 | 38 | for i in range(30): 39 | pen_down() 40 | motor_pair.move(-random.randrange(1,3), 'cm', speed = 20) 41 | pen_up() 42 | r = random.randrange(2) 43 | if r==0: 44 | motor_pair.move(pen_radius, 'cm') 45 | motor_pair.move_tank(axis_radius*math.pi/2, 'cm', 46 | left_speed=-50, right_speed=50) 47 | motor_pair.move(-pen_radius, 'cm') 48 | if r==1: 49 | motor_pair.move(pen_radius, 'cm') 50 | motor_pair.move_tank(-axis_radius*math.pi/2, 'cm', 51 | left_speed=-50, right_speed=50) 52 | motor_pair.move(-pen_radius, 'cm') 53 | if r==2: 54 | pass 55 | 56 | hub.light_matrix.write('done') 57 | -------------------------------------------------------------------------------- /programs/house.py: -------------------------------------------------------------------------------- 1 | from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App 2 | from mindstorms.control import wait_for_seconds, wait_until, Timer 3 | from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to 4 | import math 5 | 6 | # drawing a simple house with Tricky 7 | 8 | wheel_radius = 2.8 9 | pen_radius = 8.6 10 | axis_radius = 5.6 11 | 12 | line_length = 5 13 | 14 | # Create your objects here. 15 | hub = MSHub() 16 | 17 | 18 | # Write your program here. 19 | hub.speaker.beep() 20 | 21 | motor = Motor('C') 22 | motor.run_to_position(165) 23 | 24 | 25 | def pen_up(): 26 | motor.run_for_degrees(-90) 27 | 28 | 29 | def pen_down(): 30 | motor.run_for_degrees(90) 31 | 32 | 33 | motor_pair = MotorPair('B', 'A') 34 | motor_pair.set_default_speed(20) 35 | motor_pair.set_motor_rotation(wheel_radius * 2 * math.pi, 'cm') 36 | 37 | for i in range(4): 38 | 39 | if i > 0: 40 | motor_pair.move(pen_radius, 'cm') 41 | motor_pair.move_tank(axis_radius*math.pi/2, 'cm', 42 | left_speed=-20, right_speed=20) 43 | motor_pair.move(-pen_radius, 'cm') 44 | 45 | pen_down() 46 | 47 | motor_pair.move(-line_length, 'cm') 48 | 49 | pen_up() 50 | 51 | motor_pair.move(pen_radius, 'cm') 52 | motor_pair.move_tank(axis_radius*math.pi*0.2, 'cm', 53 | left_speed=-20, right_speed=20) 54 | motor_pair.move(-pen_radius + line_length/2, 'cm') 55 | 56 | #motor_pair.move(line_length / 2, 'cm') 57 | 58 | pen_down() 59 | motor_pair.move(-line_length / 2 - line_length, 'cm') 60 | pen_up() 61 | 62 | motor_pair.move(pen_radius, 'cm') 63 | motor_pair.move_tank(axis_radius*math.pi * 0.65, 'cm', 64 | left_speed=-20, right_speed=20) 65 | motor_pair.move(-pen_radius, 'cm') 66 | 67 | pen_down() 68 | motor_pair.move(-line_length / 2 - line_length, 'cm') 69 | pen_up() 70 | 71 | 72 | hub.speaker.beep() 73 | hub.light_matrix.write('done') 74 | -------------------------------------------------------------------------------- /programs/b2.py: -------------------------------------------------------------------------------- 1 | from mindstorms import MSHub, LightMatrix, Button, StatusLight, ForceSensor, MotionSensor, Speaker, ColorSensor, App, DistanceSensor, Motor, MotorPair 2 | from mindstorms.control import wait_for_seconds, wait_until, Timer 3 | import time 4 | 5 | motor_ports = ["B", "F"] 6 | SET_POINT = -90.0 7 | KP, KI, KD = 10, 120, 0.1 8 | STOP_ANGLE = 20 9 | DT = 0.02 10 | 11 | assert len(motor_ports) == 2 12 | 13 | hub = MSHub() 14 | motors = MotorPair(*motor_ports) 15 | 16 | 17 | class PID(object): 18 | def __init__(self, KP, KI, KD, max_integral: float = 100): 19 | self._KP = KP 20 | self._KI = KI 21 | self._KD = KD 22 | 23 | self._integral = 0.0 24 | self._max_integral = max_integral if KI == 0.0 else max_integral / KI 25 | self._start_time = self._now_ms() 26 | self._prev_error = None 27 | self._prev_response = None 28 | 29 | def act(self, set_point: float, state: float, dt: float = None): 30 | """ 31 | :param set_point: 32 | :param state: 33 | :param dt: time since last interaction in seconds 34 | :return: 35 | """ 36 | current_time = self._now_ms() 37 | if dt is None: 38 | dt = time.ticks_diff(current_time, self._start_time) / 1000 39 | if dt == 0 and self._prev_response is not None: 40 | return self._prev_response 41 | error = set_point - state 42 | if self._prev_error is None: 43 | self._prev_error = error 44 | self._integral += error * dt 45 | if self._max_integral and (abs(self._integral) > self._max_integral): 46 | self._integral = self._max_integral if self._integral > 0 else -self._max_integral 47 | d_error = (error - self._prev_error) / dt if dt > 0 else 0.0 48 | self._prev_error = error 49 | 50 | response = self._KP * error + self._KI * self._integral + self._KD * d_error 51 | self._prev_response = response 52 | self._start_time = current_time 53 | return response 54 | 55 | def _now_ms(self): 56 | return time.ticks_ms() 57 | 58 | 59 | def balance(angle: float, pid: PID, dt): 60 | global motors, hub, STOP_ANGLE 61 | while True: 62 | state = hub.motion_sensor.get_roll_angle() 63 | if abs(state-SET_POINT) >= STOP_ANGLE: 64 | motors.stop() 65 | hub.speaker.beep() 66 | break 67 | response = pid.act(angle, state) 68 | motors.start_at_power(int(round(response))) 69 | wait_for_seconds(dt) 70 | 71 | 72 | hub.light_matrix.show_image('YES') 73 | wait_for_seconds(0.2) 74 | pid = PID(KP=KP, KI=KI, KD=KD) 75 | balance(SET_POINT, pid=pid, dt=DT) 76 | hub.light_matrix.show_image('NO') 77 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Tools for the LEGO® MINDSTORMS® Robot Inventor (51515) 2 | 3 | This repo contains some tools and experiments with the LEGO® MINDSTORMS® Robot Inventor (51515) and its Hub. 4 | 5 | Im using Linux (Ubuntu 20.04) on my system but hopefully the Python scripts should work on Mac an Windows, too. 6 | Feel free to share your experience with diffrent plattforms. 7 | 8 | This project is in a very early state of development, is not well tested and will probably will contain many bugs. 9 | 10 | # The Tools 11 | 12 | ## The Gateway Tool 13 | 14 | This tool is meant to maintain and monitor the connection to the Lego Hub. It currently has the following features: 15 | * Connect the Lego Hub via 16 | * Bluetooth e.g. `-d AA:BB:CC:DD:EE:FF` 17 | * Serial Port (over USB) e.g. `-t /dev/ttyACM0` 18 | * Listen on `localhost` for clients to connect for forwarding the connection e.g. `-p 8888` 19 | * Listen as Bluetooth server to connect the "real" Robot Inventor App and sniff the communication (see below for more information) 20 | * Write all communication to a trace log file. 21 | * Simulate a Hub by reading data from a file. Mainly for developing and testen the Gateway itself. 22 | 23 | ``` 24 | tools$ ./gateway.py --help 25 | usage: gateway.py [-h] [--debug] [-p ] [-b] [-l | -n] (-t | -d | -f ) 26 | 27 | Tool for Monitoring Lego Mindstorms Roboter Inventor Hub and multiplexing connections. 28 | 29 | optional arguments: 30 | -h, --help show this help message and exit 31 | --debug Enable debug 32 | -p , --port 33 | port to listen on localhost for replication (default: 8888) 34 | -b, --bluetooth start blueooth server 35 | -l , --log 36 | log file (default: trace-Ymd-HMS.log 37 | -n, --nolog don't create log file 38 | -t , --tty 39 | device path 40 | -d , --device 41 | bluetooth device address 42 | -f , --file 43 | test data file 44 | ``` 45 | 46 | ## Sniff the communication from the Robot Inventor App with the Hub 47 | 48 | At first you should pair your computer with the real Hub: 49 | ``` 50 | toosl$ bluetoothctl 51 | ... 52 | # scan on 53 | ... 54 | # pair AA:BB:CC:DD:EE:FF 55 | ... 56 | # trust AA:BB:CC:DD:EE:FF 57 | ... 58 | ``` 59 | 60 | To find your computer as a Hub in the Robot Inventor App your system's name in Bluetooth has to start with `LEGO Hub` (its case sensitive). You can set a name like this: 61 | ``` 62 | tools$ bluetoothctl system-alias "LEGO Hub@gateway" 63 | ``` 64 | 65 | Start the Gateway with Bluetooth server enabled, where `AA:BB:CC:DD:EE:FF` is the Bluetooth device address of the 66 | real Hub: 67 | ``` 68 | tools$ ./gateway.py -b -d AA:BB:CC:DD:EE:FF -l 69 | ``` 70 | To made the bluetooth server work, it may have to be startet as root. 71 | 72 | Now you should discover the new `LEGO Hub@gateway` in your Robot Inventor App (e.g. on your tablet or phone). 73 | Try to connect to it and watch the communication. 74 | 75 | # Useful references 76 | Other useful projects, mainly focused on LEGO® Education SPIKE™ Prime. But the Hub is mostly the same: 77 | * https://github.com/sanjayseshan/spikeprime-tools 78 | * https://github.com/sanjayseshan/spikeprime-vscode 79 | * https://github.com/gpdaniels/spike-prime 80 | * https://github.com/robmosca/robotinventor-vscode 81 | * https://github.com/dwalton76/spikedev 82 | 83 | Scripts for the Hub itself: 84 | * A self balancing robot: https://medium.com/@janislavjankov/self-balancing-robot-with-lego-spike-prime-ac156af5c2b2 -------------------------------------------------------------------------------- /tools/spikejsonrpc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ## 4 | # This code is copied from https://github.com/sanjayseshan/spikeprime-tools/blob/master/spiketools/spikejsonrpcapispike.py 5 | # and adopted to connect to the gateway. 6 | # Thanks to sanjayseshan for this tool. 7 | ### 8 | 9 | import socket 10 | import base64 11 | import os 12 | # import sys 13 | import argparse 14 | from tqdm import tqdm 15 | import time 16 | import json 17 | import random 18 | import string 19 | import logging 20 | from datetime import datetime 21 | 22 | letters = string.ascii_letters + string.digits + '_' 23 | def random_id(len = 4): 24 | return ''.join(random.choice(letters) for _ in range(4)) 25 | 26 | class RPC: 27 | def __init__(self): 28 | self.socket = socket.create_connection(('localhost', 8888)) 29 | self.recv_buf = bytearray() 30 | 31 | def recv_message(self, timeout = 100): 32 | self.socket.settimeout(timeout) 33 | while True: 34 | try: 35 | data = self.socket.recv(1) 36 | except socket.timeout: 37 | print("Timeout") 38 | break 39 | except BlockingIOError: 40 | break 41 | if data == b'\r': 42 | try: 43 | return json.loads(self.recv_buf.decode('utf-8')) 44 | except json.JSONDecodeError: 45 | logging.debug("Cannot parse JSON: %s" % self.recv_buf) 46 | finally: 47 | self.recv_buf.clear() 48 | else: 49 | self.recv_buf += data 50 | return None 51 | 52 | def send_message(self, name, params = {}): 53 | while True: 54 | if not self.recv_message(timeout=0): 55 | break 56 | id = random_id() 57 | msg = {'m':name, 'p': params, 'i': id} 58 | msg_string = json.dumps(msg) 59 | logging.debug('sending: %s' % msg_string) 60 | self.socket.send(msg_string.encode('utf-8')) 61 | self.socket.send(b'\r') 62 | return self.recv_response(id) 63 | 64 | def recv_response(self, id): 65 | while True: 66 | m = self.recv_message() 67 | if 'i' in m and m['i'] == id: 68 | logging.debug('response: %s' % m) 69 | if 'e' in m: 70 | error = json.loads(base64.b64decode(m['e']).decode('utf-8')) 71 | raise ConnectionError(error) 72 | return m['r'] 73 | logging.debug('while waiting for response: %s' % m) 74 | 75 | # Program Methods 76 | def program_execute(self, n): 77 | return self.send_message('program_execute', {'slotid': n}) 78 | 79 | def program_terminate(self): 80 | return self.send_message('program_terminate') 81 | 82 | def get_storage_information(self): 83 | return self.send_message('get_storage_status') 84 | 85 | def start_write_program(self, name, size, slot, created, modified): 86 | meta = {'created': created, 'modified': modified, 'name': name, 'type': 'python', 'project_id': '50uN1ZaRpHj2'} 87 | return self.send_message('start_write_program', {'slotid':slot, 'size': size, 'meta': meta}) 88 | 89 | def write_package(self, data, transferid): 90 | return self.send_message('write_package', {'data': str(base64.b64encode(data), 'utf-8'), 'transferid': transferid}) 91 | 92 | def move_project(self, from_slot, to_slot): 93 | return self.send_message('move_project', {'old_slotid': from_slot, 'new_slotid': to_slot}) 94 | 95 | def remove_project(self, from_slot): 96 | return self.send_message('remove_project', {'slotid': from_slot }) 97 | 98 | # Light Methods 99 | def display_set_pixel(self, x, y, brightness = 9): 100 | return self.send_message('scratch.display_set_pixel', { 'x':x, 'y': y, 'brightness': brightness}) 101 | 102 | def display_clear(self): 103 | return self.send_message('scratch.display_clear') 104 | 105 | def display_image(self, image): 106 | return self.send_message('scratch.display_image', { 'image':image }) 107 | 108 | def display_image_for(self, image, duration_ms): 109 | return self.send_message('scratch.display_image_for', { 'image':image, 'duration': duration_ms }) 110 | 111 | def display_text(self, text): 112 | return self.send_message('scratch.display_text', {'text':text}) 113 | 114 | def get_time(self): 115 | return self.send_message('storage_status') 116 | 117 | # def get_time(self): 118 | # return self.send_message('get_hub_info'), trigger_current_sttae 119 | 120 | # Hub Methods 121 | def get_firmware_info(self): 122 | return self.send_message('get_hub_info') 123 | 124 | 125 | if __name__ == "__main__": 126 | def handle_list(): 127 | info = rpc.get_storage_information() 128 | storage = info['storage'] 129 | slots = info['slots'] 130 | print("%4s %-40s %6s %-20s %-12s %-10s" % ("Slot", "Decoded Name", "Size", "Last Modified", "Project_id", "Type")) 131 | for i in range(20): 132 | if str(i) in slots: 133 | sl = slots[str(i)] 134 | modified = datetime.utcfromtimestamp(sl['modified']/1000).strftime('%Y-%m-%d %H:%M:%S') 135 | try: 136 | decoded_name = base64.b64decode(sl['name']).decode('utf-8') 137 | except: 138 | decoded_name = sl['name'] 139 | try: 140 | project = sl['project_id'] 141 | except: 142 | project = " " 143 | try: 144 | type = sl['type'] 145 | except: 146 | type = " " 147 | # print("%2s %-40s %-40s %5db %6s %-20s %-20s %-10s" % (i, sl['name'], decoded_name, sl['size'], sl['id'], modified, project, type)) 148 | print("%4s %-40s %5db %-20s %-12s %-10s" % (i, decoded_name, sl['size'], modified, project, type)) 149 | print(("Storage free %s%s of total %s%s" % (storage['free'], storage['unit'], storage['total'], storage['unit']))) 150 | def handle_fwinfo(): 151 | info = rpc.get_firmware_info() 152 | fw = '.'.join(str(x) for x in info['version']) 153 | rt = '.'.join(str(x) for x in info['runtime']) 154 | print("Firmware version: %s; Runtime version: %s" % (fw, rt)) 155 | def handle_upload(): 156 | with open(args.file, "rb") as f: 157 | size = os.path.getsize(args.file) 158 | name = args.name if args.name else args.file 159 | now = int(time.time() * 1000) 160 | start = rpc.start_write_program(name, size, args.to_slot, now, now) 161 | bs = start['blocksize'] 162 | id = start['transferid'] 163 | with tqdm(total=size, unit='B', unit_scale=True) as pbar: 164 | b = f.read(bs) 165 | while b: 166 | rpc.write_package(b, id) 167 | pbar.update(len(b)) 168 | b = f.read(bs) 169 | if args.start: 170 | rpc.program_execute(args.to_slot) 171 | def handle_get_time(): 172 | result = rpc.get_time() 173 | 174 | parser = argparse.ArgumentParser(description='Tools for Spike Hub RPC protocol') 175 | parser.add_argument('-t', '--tty', help='Spike Hub device path', default='/dev/ttyACM0') 176 | parser.add_argument('--debug', help='Enable debug', action='store_true') 177 | parser.set_defaults(func=lambda: parser.print_help()) 178 | sub_parsers = parser.add_subparsers() 179 | 180 | list_parser = sub_parsers.add_parser('list', aliases=['ls'], help='List stored programs') 181 | list_parser.set_defaults(func=handle_list) 182 | 183 | fwinfo_parser = sub_parsers.add_parser('fwinfo', help='Show firmware version') 184 | fwinfo_parser.set_defaults(func=handle_fwinfo) 185 | 186 | get_time_parser = sub_parsers.add_parser('time', help='Get time') 187 | get_time_parser.set_defaults(func=handle_get_time) 188 | 189 | mvprogram_parser = sub_parsers.add_parser('mv', help='Changes program slot') 190 | mvprogram_parser.add_argument('from_slot', type=int) 191 | mvprogram_parser.add_argument('to_slot', type=int) 192 | mvprogram_parser.set_defaults(func=lambda: rpc.move_project(args.from_slot, args.to_slot)) 193 | 194 | cpprogram_parser = sub_parsers.add_parser('upload', aliases=['cp'], help='Uploads a program') 195 | cpprogram_parser.add_argument('file') 196 | cpprogram_parser.add_argument('to_slot', type=int) 197 | cpprogram_parser.add_argument('name', nargs='?') 198 | cpprogram_parser.add_argument('--start', '-s', help='Start after upload', action='store_true') 199 | cpprogram_parser.set_defaults(func=handle_upload) 200 | 201 | rmprogram_parser = sub_parsers.add_parser('rm', help='Removes the program at a given slot') 202 | rmprogram_parser.add_argument('from_slot', type=int) 203 | rmprogram_parser.set_defaults(func=lambda: rpc.remove_project(args.from_slot)) 204 | 205 | startprogram_parser = sub_parsers.add_parser('start', help='Starts a program') 206 | startprogram_parser.add_argument('slot', type=int) 207 | startprogram_parser.set_defaults(func=lambda: rpc.program_execute(args.slot)) 208 | 209 | stopprogram_parser = sub_parsers.add_parser('stop', help='Stop program execution') 210 | stopprogram_parser.set_defaults(func=lambda: rpc.program_terminate()) 211 | 212 | display_parser = sub_parsers.add_parser('display', help='Controls 5x5 LED matrix display') 213 | display_parser.set_defaults(func=lambda: display_parser.print_help()) 214 | display_parsers = display_parser.add_subparsers() 215 | 216 | display_image_parser = display_parsers.add_parser('image', help='Displays image on the LED matrix') 217 | display_image_parser.add_argument('image', help='format xxxxx:xxxxx:xxxxx:xxxxx:xxxx, where x is the pixel brigthness in range 0-9') 218 | display_image_parser.set_defaults(func=lambda: rpc.display_image(args.image)) 219 | 220 | display_text_parser = display_parsers.add_parser('text', help='Displays scrolling text on the LED matrix') 221 | display_text_parser.add_argument('text') 222 | display_text_parser.set_defaults(func=lambda: rpc.display_text(args.text)) 223 | 224 | display_clear_parser = display_parsers.add_parser('clear', help='Clears display') 225 | display_clear_parser.set_defaults(func=lambda: rpc.display_clear()) 226 | 227 | display_pixel_parser = display_parsers.add_parser('setpixel', help='Sets individual LED brightness') 228 | display_pixel_parser.add_argument('x', type=int) 229 | display_pixel_parser.add_argument('y', type=int) 230 | display_pixel_parser.add_argument('brightness', nargs='?', type=int, default=9, help='pixel brightness 0-9') 231 | display_pixel_parser.set_defaults(func=lambda: rpc.display_set_pixel(args.x, args.y, args.brightness)) 232 | 233 | args = parser.parse_args() 234 | if args.debug: 235 | logging.basicConfig(level=logging.DEBUG) 236 | rpc = RPC() 237 | args.func() 238 | -------------------------------------------------------------------------------- /tools/gateway.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | 5 | import argparse 6 | import base64 7 | import json 8 | import os 9 | import socket 10 | import sys 11 | import traceback 12 | from time import sleep 13 | 14 | import bluetooth 15 | import serial 16 | 17 | from ansi import esc, color 18 | import select 19 | 20 | # for testing you can use a PTY: 21 | # > socat -d -d pty,raw,echo=0 pty,raw,echo=0 22 | 23 | 24 | class LineReader: 25 | def __init__(self, name): 26 | print(f"Creating {name}{esc:K}") 27 | self.buffer = bytes() 28 | self.name = name 29 | 30 | def data_ready(self): 31 | # appending incoming data 32 | data = self.read() 33 | self.buffer += data 34 | 35 | # extract all complete lines in buffer 36 | while len(self.buffer) > 0: 37 | pos_ln = self.buffer.find(b'\n') 38 | pos_cr = self.buffer.find(b'\r') 39 | if pos_ln == -1 and pos_cr == -1: 40 | return 41 | if pos_ln == -1 or pos_cr != -1 and pos_cr < pos_ln: 42 | pos = pos_cr 43 | else: 44 | pos = pos_ln 45 | 46 | # combine all subsequent line terminators 47 | size = 1 48 | while pos + size < len(self.buffer) and self.buffer[pos + size] in b'\n\r': 49 | size += 1 50 | 51 | # take out line and line terminators 52 | line = self.buffer[:pos] 53 | line_terminators = self.buffer[pos:pos + size] 54 | self.buffer = self.buffer[pos + size:] 55 | 56 | # forward extracted line 57 | self.read_line(line, line_terminators) 58 | 59 | def write_line(self, line, line_terminators): 60 | self.write(line + line_terminators) 61 | 62 | def __str__(self): 63 | return self.name 64 | 65 | def close(self): 66 | print(f"Closing {self}{esc:K}") 67 | 68 | 69 | class HubConnection(LineReader): 70 | def __init__(self, name): 71 | super().__init__(name) 72 | self.charging = False 73 | self.charged = 0 74 | 75 | def read(self): 76 | pass 77 | 78 | def write(self, data): 79 | pass 80 | 81 | def close(self): 82 | pass 83 | 84 | def read_line(self, line, line_terminators): 85 | log.input(line) 86 | self.parse_line(line.decode('utf-8', 'ignore')) 87 | closed_clients = [] 88 | for client in clients: 89 | try: 90 | client.write_line(line, line_terminators) 91 | except: 92 | closed_clients.append(client) 93 | client.close() 94 | for client in closed_clients: 95 | clients.remove(client) 96 | 97 | def parse_line(self, line): 98 | try: 99 | message = json.loads(line) 100 | if 'i' in message and 'm' in message and 'p' in message: 101 | self.handle_request(message) 102 | elif 'i' not in message and 'm' in message and 'p' in message: 103 | self.handle_notification(message) 104 | elif 'i' in message and 'r' in message: 105 | self.handle_response(message) 106 | elif 'e' in message and 'i' in message: 107 | self.handle_error(message) 108 | elif 'i' in message and 'm' in message and 'p' in message: 109 | self.handle_user_program_print(message) 110 | else: 111 | self.print(line, f"{color:34}UNKOWN:") 112 | except json.JSONDecodeError: 113 | self.print(line, f"{color:31}JSON ERROR:", wrap=True) 114 | except Exception as e: 115 | traceback.print_exc() 116 | self.print(f"{color:2}{e}{color:0}: {line}", f"{color:31}FAILED:") 117 | 118 | def decode_base64(self, value): 119 | return base64.b64decode(value).decode('utf-8', 'ignore') 120 | 121 | def handle_request(self, message): 122 | i = message['i'] 123 | m = message['m'] 124 | p = message['p'] 125 | self.print(f"{m}: {json.dumps(p)}", f"{color:33}REQUEST:", id=i) 126 | 127 | def handle_response(self, message): 128 | i = message['i'] 129 | r = message['r'] 130 | self.print(r, f"{color:33}RESPONSE:", id=i) 131 | 132 | def handle_user_program_print(self, message): 133 | i = message['i'] 134 | m = message['m'] 135 | p = message['p'] 136 | if m != 'userProgram.print': 137 | raise AssertionError(f"m={m} but expected to be userProgram.print") 138 | self.print(self.decode_base64(p['value']), "{color:32}OUTPUT:", id=i, wrap=True) 139 | 140 | def handle_error(self, message): 141 | i = message['i'] 142 | e = message['e'] 143 | self.print(self.decode_base64(e), f'{color:31}ERROR:', id=i, wrap=True) 144 | 145 | def handle_notification(self, message): 146 | m = message['m'] 147 | p = message['p'] 148 | if m == 0: 149 | self.handle_sensor_notification(p[0:6], p[6], p[7], p[8], p[9], p[10]) 150 | elif m == 1: 151 | self.handle_storage_notification(p) 152 | elif m == 2: 153 | self.handle_battery_notification(*p) 154 | elif m == 3: 155 | self.handle_button_notification(*p) 156 | elif m == 4: 157 | self.handle_gesture_notification(p) 158 | elif m == 5: 159 | self.handle_display_notification(p) 160 | elif m == 6: 161 | self.handle_firmware_notification(p) 162 | # 7 stack start 163 | # 8 stack top 164 | # 9 info status 165 | # 10 error 166 | # 11 vm state 167 | elif m == 12: 168 | self.handle_program_notification(p) 169 | # 13 linegraph timer reset 170 | # 14 orientation status 171 | elif m == 'runtime_error': 172 | self.handle_runtime_error(p) 173 | else: 174 | self.handle_unknown_notification(m, p) 175 | 176 | def handle_sensor_notification(self, ports, accelerometer, gyroscope, position, display, time): 177 | buf = f"{color:1} " 178 | for i in range(6): 179 | gadget = ports[i][0] 180 | buf += "ABCDEF"[i] + ":" 181 | if gadget == 0: # Not connected 182 | buf += "-" 183 | # Stone gray motor medium [Speed, Diff, Pos, ?] 184 | elif gadget == 75: 185 | if len(ports[i][1]) == 4: 186 | buf += f"{ports[i][1][2]:4}°{ports[i][1][0]:3}%" 187 | else: 188 | buf += "?" 189 | elif gadget == 61: # Color sensor 190 | # none: 255, red: 9, blue: 3, green: 5, yellow: 7: white: 10, black 0 191 | buf += f"C{ports[i][1][0]}" 192 | elif gadget == 62: # Distance sensor 193 | buf += f"{ports[i][1][0]:3}cm " if ports[i][1][0] else " cm " 194 | else: 195 | buf += f"{ports[i][1]}" 196 | buf += f"{color:0;2}| {color:0;1}" 197 | 198 | buf += f"a=({accelerometer[0]:5}{accelerometer[1]:5}{accelerometer[2]:5}) " 199 | buf += f"v=({gyroscope[0]:5}{gyroscope[1]:5}{gyroscope[2]:5}) " 200 | buf += f"p=({position[0]:5}{position[1]:5}{position[2]:5}) " 201 | buf += f"Bat:{self.charged:3}%{color:0;2}| {color:0;1}" 202 | buf += f"Display:{display}{color:0;2}| {color:0;1}" 203 | buf += f"Time:{time}" 204 | self.print(buf, end="\r") 205 | 206 | def handle_storage_notification(self, p): 207 | self.print(p, f"{color:34}STORAGE:") 208 | 209 | def handle_battery_notification(self, voltage, charge, charging): 210 | self.charged = charge 211 | # 0: not charging, 1: charging, 2: unknown 212 | self.charging = charging 213 | 214 | def handle_button_notification(self, button, duration): 215 | self.print(f"Button pressed: {button} {duration:4}", f"{color:34}INFO:") 216 | 217 | def handle_gesture_notification(self, action): 218 | self.print(f"Interaction: {action}", f"{color:34}INFO:") 219 | 220 | def handle_display_notification(self, p): 221 | self.print(p, f"{color:34}DISPLAY:") 222 | 223 | def handle_firmware_notification(self, p): 224 | self.print(p, f"{color:34}FIRMWARE:") 225 | 226 | def handle_program_notification(self, p): 227 | self.print(p, f"{color:34}PROGRAM:") 228 | 229 | def handle_runtime_error(self, p): 230 | def tryDecode(value): 231 | try: 232 | return self.decode_base64(value) 233 | except: 234 | return value 235 | p = list(map(tryDecode, p)) 236 | self.print(p, f"{color:31}RUNTIME:", wrap=True) 237 | 238 | def handle_unknown_notification(self, m, p): 239 | self.print(p, f"{color:2}{m}") 240 | 241 | def print(self, data, prefix=None, wrap=False, end="\n", id=None): 242 | if not isinstance(data, str): 243 | data = json.dumps(data) 244 | if not wrap: 245 | data = f"{esc:?7l}{data}{esc:?7h}" 246 | if id: 247 | data = f"{color:2}{id}{color:0} {data}" 248 | if prefix: 249 | data = f"{prefix:17}{color:0}{data}" 250 | data = f"{data}{esc:K}{color:0}{end}" 251 | sys.stdout.write(data) 252 | 253 | 254 | class SerialHubConnection(HubConnection): 255 | def __init__(self, port): 256 | super().__init__(f"SerialHubConnection ({port})") 257 | self.port = serial.Serial(port) 258 | 259 | def read(self): 260 | return self.port.read(1024) 261 | 262 | def write(self, data): 263 | self.port.write(data) 264 | 265 | def close(self): 266 | self.port.close() 267 | 268 | def fileno(self): 269 | return self.port.fileno() 270 | 271 | def __str__(self): 272 | return self.port.name 273 | 274 | 275 | class BluetoothHubConnection(HubConnection): 276 | def __init__(self, device): 277 | super().__init__(f"BluetoothClientConnection ({device})") 278 | self.socket = bluetooth.BluetoothSocket(bluetooth.RFCOMM) 279 | self.socket.connect((device, 1)) 280 | 281 | def read(self): 282 | return self.socket.recv(1024) 283 | 284 | def write(self, data): 285 | self.socket.sendall(data) 286 | 287 | def close(self): 288 | self.socket.close() 289 | 290 | def fileno(self): 291 | return self.socket.fileno() 292 | 293 | def __str__(self): 294 | return self.socket.getpeername() 295 | 296 | 297 | class FileHubConnection(HubConnection): 298 | def __init__(self, path): 299 | super().__init__(f"FileHubConnection ({path})") 300 | self.file = open(path, 'rb') 301 | 302 | def read(self): 303 | data = b'' 304 | while not data.startswith(b'< '): 305 | data = self.file.readline(1024*1024) 306 | if len(data) < 1: 307 | print("\nEOF") 308 | os._exit(1) 309 | data = data[2:] 310 | data = data.replace(b'\n', b'\r') 311 | sleep(0.001) 312 | return data 313 | 314 | def write(self, data): 315 | pass 316 | 317 | def close(self): 318 | pass 319 | 320 | def fileno(self): 321 | return self.file.fileno() 322 | 323 | def __str__(self): 324 | return self.file.name 325 | 326 | 327 | class ClientConnection(LineReader): 328 | def __init__(self, name): 329 | super().__init__(name) 330 | self.name = name 331 | clients.append(self) 332 | 333 | def read_line(self, line, line_terminators): 334 | print(f"{color:33}REQUEST:{color:0} ", line.decode('utf-8', 'ignore'), end=f"{esc:K}\n") 335 | log.output(line) 336 | hub.write_line(line, line_terminators) 337 | 338 | def read(self): 339 | pass 340 | 341 | def write(self, data): 342 | pass 343 | 344 | 345 | class SocketClientConnection(ClientConnection): 346 | def __init__(self, client_socket): 347 | super().__init__(f"SocketClientConnection {client_socket.getpeername()}") 348 | self.client_socket = client_socket 349 | 350 | def read(self): 351 | return self.client_socket.recv(1024) 352 | 353 | def write(self, data): 354 | self.client_socket.sendall(data) 355 | 356 | def fileno(self): 357 | return self.client_socket.fileno() 358 | 359 | def close(self): 360 | super().close() 361 | self.client_socket.close() 362 | 363 | 364 | class BluetoothClientConnection(SocketClientConnection): 365 | def __init__(self): 366 | self.server_socket = bluetooth.BluetoothSocket(bluetooth.RFCOMM) 367 | self.server_socket.bind(('', bluetooth.PORT_ANY)) 368 | self.server_socket.listen(1) 369 | 370 | uuid = "94f39d29-7d6d-437d-973b-fba39e49d4ee" 371 | 372 | bluetooth.advertise_service(self.server_socket, "SampleServer", service_id=uuid, service_classes=[ 373 | uuid, bluetooth.SERIAL_PORT_CLASS], profiles=[bluetooth.SERIAL_PORT_PROFILE]) 374 | 375 | print("Waiting for connection on RFCOMM channel 1") 376 | 377 | client_socket, client_info = self.server_socket.accept() 378 | print("Accepted connection from", client_info) 379 | 380 | super().__init__(client_socket) 381 | 382 | def close(self): 383 | super().close() 384 | self.server_socket.close() 385 | 386 | def write(self,data): 387 | super().write(data) 388 | 389 | 390 | class NoopLogger: 391 | def __init__(self): 392 | print("No Logging") 393 | 394 | def output(self, line): 395 | pass 396 | 397 | def input(self, line): 398 | pass 399 | 400 | 401 | class FileLogger: 402 | def __init__(self, path): 403 | print(f"Logging to {path}") 404 | self.file = open(path, mode='wb', buffering=0) 405 | 406 | def output(self, line): 407 | self.file.write(b'> ' + line + b'\n') 408 | 409 | def input(self, line): 410 | self.file.write(b'< ' + line + b'\n') 411 | 412 | 413 | class ServerSocket: 414 | def __init__(self, port): 415 | print(f"Listing on port localhost:{port}") 416 | self.server_socket = socket.create_server(('localhost', port)) 417 | 418 | def fileno(self): 419 | return self.server_socket.fileno() 420 | 421 | def data_ready(self): 422 | client_socket, client_address = self.server_socket.accept() 423 | client = SocketClientConnection(client_socket) 424 | 425 | def close(self): 426 | self.server_socket.close() 427 | 428 | 429 | clients = [] 430 | log = NoopLogger() 431 | hub = HubConnection("NoOpHubConnetion") 432 | 433 | def start(): 434 | parser = argparse.ArgumentParser( 435 | description="Tool for Monitoring Lego Mindstorms Roboter Inventor Hub and multiplexing connections.") 436 | parser.add_argument("--debug", help="Enable debug", action="store_true") 437 | parser.add_argument("-p", "--port", help="port to listen on localhost for replication (default: 8888)", 438 | metavar="", default=8888, type=int) 439 | parser.add_argument("-b", "--bluetooth", help="start blueooth server", action="store_true") 440 | 441 | log_group = parser.add_mutually_exclusive_group() 442 | log_group.add_argument("-l", "--log", help="log file (default: trace-Ymd-HMS.log", metavar="") 443 | log_group.add_argument("-n", "--nolog", help="don't create log file", action="store_true") 444 | 445 | device_group = parser.add_mutually_exclusive_group(required=True) 446 | device_group.add_argument("-t", "--tty", help="device path", metavar="") 447 | device_group.add_argument("-d", "--device", help="bluetooth device address", metavar="") 448 | device_group.add_argument("-f", "--file", help="test data file", metavar="") 449 | 450 | args = parser.parse_args() 451 | 452 | if not args.nolog: 453 | log = FileLogger(args.log) 454 | 455 | if args.tty: 456 | hub = SerialHubConnection(args.tty) 457 | elif args.device: 458 | hub = BluetoothHubConnection(args.device) 459 | elif args.file: 460 | hub = FileHubConnection(args.file) 461 | 462 | if args.bluetooth: 463 | bluetooth_client = BluetoothClientConnection() 464 | 465 | server = ServerSocket(args.port) 466 | 467 | try: 468 | while True: 469 | ready_inputs, _, _ = select.select(clients + [hub, server], [], []) 470 | for input in ready_inputs: 471 | input.data_ready() 472 | finally: 473 | for input in clients + [hub, server]: 474 | input.close() 475 | 476 | 477 | if __name__ == "__main__": 478 | start() 479 | --------------------------------------------------------------------------------