Skip to content

Instantly share code, notes, and snippets.

@antonvh

antonvh/connection.py

Last active Jun 17, 2020
Embed
What would you like to do?
Remote control a MINDSTORMS EV3 robot with a SPIKE Prime Hub
# 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]
#!/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
You can’t perform that action at this time.