Last active
June 15, 2021 08:43
-
-
Save antonvh/4ef10ad19611d82cb743af264b6f5c0b to your computer and use it in GitHub Desktop.
Remote control a MINDSTORMS EV3 robot with a SPIKE Prime Hub
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# This code was originally written by Pybricks | |
# Paste all of it into a file named 'connection.py' and place it next to your 'main.py' file | |
# Nothing below is written by Anton's Mindstorms | |
# I found it here. https://github.com/pybricks/pybricks-projects | |
from uctypes import addressof, sizeof, struct | |
from usocket import socket, SOCK_STREAM | |
from _thread import start_new_thread | |
from pybricks.bluetooth import ( | |
str2ba, | |
sockaddr_rc, | |
AF_BLUETOOTH, | |
BTPROTO_RFCOMM | |
) | |
from pybricks.tools import wait, StopWatch | |
def get_bluetooth_rfcomm_socket(address, channel): | |
addr_data = bytearray(sizeof(sockaddr_rc)) | |
addr = struct(addressof(addr_data), sockaddr_rc) | |
addr.rc_family = AF_BLUETOOTH | |
str2ba(address, addr.rc_bdaddr) | |
addr.rc_channel = channel | |
sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM) | |
sock.connect(addr_data) | |
return sock | |
class SpikePrimeStreamReader(): | |
def __init__(self, address): | |
try: | |
self.sock = get_bluetooth_rfcomm_socket(address, 1) | |
except OSError as e: | |
print("Turn on Bluetooth on the EV3 and on SPIKE.") | |
raise e | |
self._values = None | |
start_new_thread(self.reader, ()) | |
watch = StopWatch() | |
while watch.time() < 2000: | |
if self.values() is not None: | |
return | |
wait(100) | |
raise IOError("No data received") | |
def disconnect(self): | |
self.sock.close() | |
def reader(self): | |
while True: | |
try: | |
raw = self.sock.recv(1024) | |
except OSError: | |
break | |
try: | |
data = eval(raw) | |
if data['m'] == 0: | |
self._values = data['p'] | |
except (SyntaxError, KeyError): | |
pass | |
def values(self): | |
return self._values | |
def device(self, port): | |
if 'A' <= port <= 'F': | |
return self.values()[ord(port)-ord('A')][1] | |
else: | |
raise ValueError | |
def acceleration(self): | |
return self.values()[6] | |
def gyro(self): | |
return self.values()[7] | |
def orientation(self): | |
return self.values()[8] |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env pybricks-micropython | |
# Replace the contents of your 'main.py' file with this script. | |
# Author: Anton's Mindstorms | |
# Full tutorial on https://antonsmindstorms.com | |
# Connection class by https://pybricks.com | |
from pybricks.hubs import EV3Brick | |
from pybricks.tools import wait | |
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, | |
InfraredSensor, UltrasonicSensor, GyroSensor) | |
from pybricks.parameters import Port, Stop, Direction, Button, Color | |
from math import sin, radians, pi, atan2, degrees | |
from connection import SpikePrimeStreamReader | |
# Create the connection. Replace this with the MAC address for your SPIKE hub !! | |
spike = SpikePrimeStreamReader('38:0B:3C:A3:45:0D') | |
MAX_SPEED = 800 | |
DEADZONE = MAX_SPEED * 0.1 | |
three_o_clock = Motor(Port.A) | |
seven_o_clock = Motor(Port.C) | |
eleven_o_clock = Motor(Port.B) | |
gun = Motor(Port.D) | |
def deadzone(value): | |
if abs(value) < DEADZONE: | |
return 0 | |
else: | |
return value | |
def drive(speed, direction, turn): | |
three_o_clock.run( sin(radians(90 - direction)) * speed + turn) | |
seven_o_clock.run( sin(radians(210 - direction)) * speed + turn) | |
eleven_o_clock.run( sin(radians(330 - direction)) * speed + turn) | |
direction = 0 | |
speed = 0 | |
turn = 0 | |
# Now you can simply read values! | |
while True: | |
down, backward, left = spike.acceleration() | |
if spike.device("E")[0] >= 5: | |
# Touch sensor is pressed | |
gun.run_angle(400, -90) | |
turn = spike.device("C")[0] * 20 | |
forward = backward * -1 | |
right = left * -1 | |
direction = degrees( atan2(right, forward) ) | |
speed = (right ** 2 + forward ** 2) ** 0.5 | |
drive(speed, direction, turn) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Nothing needed on the hub.