# # uses ev3dev2-based code # # Date Author Change Notes # 4/15/2020 Ron King initial dev # ref. https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py import time, tty, sys from ev3dev2 import list_devices from ev3dev2.port import LegoPort from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import Sensor p1 = LegoPort(INPUT_1) # 'serial0-0:S1' # spi0.1:S1:i2c1 # spi0.1:S1:i2c17 # spi0.1:S1:i2c34 # p1 = LegoPort(address='serial0-0:S1:i2c22') # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes p1.mode = 'nxt-i2c' # allow for some time for mode to setup time.sleep(0.5) # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors p1.set_device = 'nxt-i2c-sensor'
#
# uses ev3dev2-based code
#
# Date Author Change Notes
# 5/10/2021 Ron King initial dev
# ref. https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py
import time, tty, sys
from ev3dev2 import list_devices
from ev3dev2.port import LegoPort
from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import Sensor
p1 = LegoPort(INPUT_1)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes
p1.mode = 'ev3-uart'
# allow for some time for mode to setup
time.sleep(0.5)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors
""" https://github.com/ev3dev/ev3dev/issues/640
dlech commented on May 15, 2016
You are forgetting the I2C address for set_device
$ echo mx-pps58-nx 0x0C >/sys/class/lego-port/port0/set_device
# uses ev3dev2-based code
#
# Date Author Change Notes
# 4/16/2020 Ron King initial dev
# ref. https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py
import time, tty, sys
from ev3dev2 import list_devices
from ev3dev2.port import LegoPort
from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import Sensor
# import ev3dev2.sensor
p1 = LegoPort(INPUT_1)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes
p1.mode = 'nxt-i2c'
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors
""" https://github.com/ev3dev/ev3dev/issues/640
dlech commented on May 15, 2016
You are forgetting the I2C address for set_device
$ echo mx-pps58-nx 0x0C >/sys/class/lego-port/port0/set_device
dmesg -w
"""
Motors (if needed) motor A = Left motor motor B = Right motor """ import time, tty, sys, threading from ev3dev2 import list_devices from ev3dev2.port import LegoPort from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, SpeedDPS, MoveTank from ev3dev2.sensor import INPUT_1, INPUT_2 from ev3dev2.sensor.lego import InfraredSensor from ev3dev2.sensor.lego import Sensor p1 = LegoPort(INPUT_1) # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes p1.mode = 'ev3-uart' # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors p1.set_device = 'lego-ev3-ir' # Connect infrared to any sensor port ir = InfraredSensor(INPUT_1) # allow for some time to load the new drivers time.sleep(0.5) irProxVal = 0 prev_irProxVal = 0 p2 = LegoPort(INPUT_2)
# # uses ev3dev2-based code # # Date Author Change Notes # 3/22/2020 Ron King initial dev # 5/3/2020 Ron King modify to use Proximity and Beacon/Seek (distance, heading) mode simultaneously # discovered that the IR beacon signal interfers with the proximity mode readings import time, tty, sys from ev3dev2 import list_devices from ev3dev2.port import LegoPort from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent from ev3dev2.sensor import INPUT_3 from ev3dev2.sensor.lego import InfraredSensor p1 = LegoPort(INPUT_3) # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes p1.mode = 'ev3-uart' # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors p1.set_device = 'lego-ev3-ir' # Connect infrared to any sensor port ir = InfraredSensor(INPUT_3) # allow for some time to load the new drivers time.sleep(0.5) ir.mode = 'IR-SEEK' irProxVal = 0 prev_irProxVal = 0
def __init__(self,
robot_config_file,
initRobotConfig,
timeStep,
odometryTimer=0.1,
defaultLogging=30):
"""
Creates the main bot class.
"""
if not os.path.exists(robot_config_file):
raise ValueError(f'Cannot find configuration file '
f'"{robot_config_file}"')
with open(robot_config_file, 'r') as f:
self.robotConfiguration = json.load(f)
# Adjust JSON just read
self.__adjustConfigDict(self.robotConfiguration)
# Exception Queue for inter threads exception communications
self.exceptionQueue = queue.Queue()
self.mqttMoveQueue = queue.Queue()
self.baseMovementLock = asyncio.locks.Lock()
self.timeStep = timeStep
self.powerSupply = PowerSupply()
# region sensor Init
self.sensors = {}
for sensor in self.robotConfiguration["sensors"]:
sensorDict = self.robotConfiguration["sensors"][sensor]
legoPort = LegoPort(sensorDict["input"])
# Applies to both I2C and SDK controled sensors
if legoPort.mode != sensorDict["mode"]:
legoPort.mode = sensorDict["mode"]
# Only applies to sensors controled by the python SDK
if "set_device" in sensorDict:
legoPort.set_device = sensorDict["set_device"]
time.sleep(1)
address = sensorDict["input"]
self.sensors[sensor] = sensorDict["sensorType"](address=address)
# endregion
# base Init
self.base = DiffDriveBase(self.robotConfiguration["base"],
initRobotConfig, odometryTimer)
# arm/endeffector Init
self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])
# Other matrix init
self.M0e = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"]) # noqa F501
self.Toe_grip = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"]) # noqa F501
self.Tbc = mr.RpToTrans(
R=np.array(
self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
self.Tcb = mr.TransInv(self.Tbc)
# Reset all motors at init
self.urgentStop = False
log.info(LOGGER_DDR_MAIN, 'Done main robot init.')
# Data for requesting block
GET_OBJECTS_COMMAND = [174, 193, 32, 2, BALL_SIGNATURE, 1]
# The y position of the ball when it reaches the goal - I found this value by opening PixyMon and hovering
# my mouse over the goalie line on the board. PixyMon shows the coordinates of the mouse, so I used
# the y-position of the mouse as Ye.
Ye = 190
# Sets the lego port of the large motor to port C
lm = LargeMotor(OUTPUT_C)
#Where the robot starts
BALL_START = 170
# Set LEGO port for Pixy2 on input port 1
in1 = LegoPort(INPUT_1)
in1.mode = 'other-i2c'
# Short wait for the port to get ready
time.sleep(0.5)
# Settings for I2C (SMBus(3) for INPUT_1)
bus = SMBus(3)
# Function that reads the signature, x-position and y-position of the ball and checks that they are
# valid values. It checks this by making sure that the signature read is 1, the signature we asked for
def readValues():
sig = 0
while sig != BALL_SIGNATURE:
# Request block
def __init__(self,
sensorPort: str,
linkID: int,
i2cBus: str,
i2cAddress=0x11,
gyroFilterValue=GYRO_FILTER_SENSITIVITY_VALUE,
gyroSensitivity='2G',
odometryLoopTimeStep=GYRO_LOOP_TIMER):
'''
Description:constructor for an angle sensor
param:sensorPort:desc:input used on the lego robot (INPUT_1, INPUT_2, etc.)
param:sensorPort:type:string
param:linkID:linkid of the robot section containing the gyroscope
param:linkID:int
param:i2cBus:desc:device no (/dev/i2c-x)... for brickpi3, it's INPUT_N, it's N+2
param:i2cBus:type:int
param:i2cAddress:desc:channel on the device - should be 0x11
param:i2cAddress:type:int or hex
param:gyroFitlerValue:value between 0 and 7 for n-order filtering. 7 = slows down reading by 10ms 0 = 0ms
param:gyroFitlerValue:int
param:gyroSensitivity:desc:sensitivity, 2g, 4g, 8g or 16g - see documentation for the sensor
param:gyroSensitivity:type:str
'''
LOGGER_GS_MAIN_INSTANCE = LOGGER_GS_MAIN.format(linkID)
# Place the port in I2C mode
LegoPort(sensorPort).mode = GYRO_I2C_MODE
self.__i2cBus = i2cBus
self.__bus = SMBus(i2cBus)
self.__i2cAddress = i2cAddress
self.odometryLoopTimeStep = GYRO_LOOP_TIMER
self.__linkID = linkID
# Se gyro filter sensitivity
log.debug(LOGGER_GS_MAIN_INSTANCE, 'Setting gyro filter sensitivity')
# Set acceleration sensitivity to XG
if gyroSensitivity == '2G':
self.__GYRO_ACCEL_SENSITIVITY = GYRO_ACCEL_SENSITIVITY_2G_COMMAND
self.__GYRO_DPS_SCALE = GYRO_DPS_SCALE_2G
elif gyroSensitivity == '4G':
self.__GYRO_ACCEL_SENSITIVITY == GYRO_ACCEL_SENSITIVITY_4G_COMMAND
self.__GYRO_DPS_SCALE = GYRO_DPS_SCALE_4G
elif gyroSensitivity == '8G':
self.__GYRO_ACCEL_SENSITIVITY == GYRO_ACCEL_SENSITIVITY_8G_COMMAND
self.__GYRO_DPS_SCALE = GYRO_DPS_SCALE_8G
elif gyroSensitivity == '16G':
self.__GYRO_ACCEL_SENSITIVITY == GYRO_ACCEL_SENSITIVITY_16G_COMMAND
self.__GYRO_DPS_SCALE = GYRO_DPS_SCALE_16G
# Configure device sensitivity
self.__bus.write_byte_data(self.__i2cAddress,
GYRO_FILTER_SENSITIVITY_ADDRESS,
gyroFilterValue)
self.__bus.write_byte_data(self.__i2cAddress,
GYRO_IMU_COMMAND_REGISTER,
self.__GYRO_ACCEL_SENSITIVITY)
time.sleep(0.05) # sleep after changing sensor sensitivity
# Calculate components of acceleration (ignore gyr data at initialization)
accData, _ = self.__getSensorData()
pitch = np.float(arctan2(accData[0], accData[2]) * 180 / pi)
roll = np.float(arctan2(accData[1], accData[2]) * 180 / pi)
yaw = 0
self.__angles = np.array([pitch, roll, yaw])
# Launch odometry thread # TODO : put this in a run function?
self.killThread = False
log.info(LOGGER_GS_MAIN_INSTANCE, 'Launching gyro thread')
self.threadGyroLoop = threading.Thread(
target=self.threadImplGyroLoop,
args=([
self.odometryLoopTimeStep,
ODOMETRY_GYRO_LOGGING_LOOP_AGGREGATED,
GYRO_COMPLEMENTARY_FILTER_TIME_CONSTANT
]),
name="threadImplGyroLoop_{}".format(self.__linkID))
self.threadGyroLoop.start()
log.info(LOGGER_GS_MAIN_INSTANCE, 'Started gyro thread.')
# CONSTANTS
# Make sure the same address is set in Pixy2
PIXY2_I2C_ADDRESS = 0x54
# Data for requesting block
GET_OBJECTS_COMMAND = [174, 193, 32, 2, 255, 255]
# Defines all of the LEGO Motor Ports
lmA = LargeMotor(OUTPUT_A)
lmB = LargeMotor(OUTPUT_B)
lmC = LargeMotor(OUTPUT_C)
mmD = MediumMotor(OUTPUT_D)
# Set LEGO port for Pixy2 on input port 1
in4 = LegoPort(INPUT_4)
in4.mode = 'other-i2c'
# List of valid three-in-a-rows
isValidThreeInARow = [[0, 1, 2], [3, 4, 5], [6, 7, 8], [0, 3, 6], [0, 4, 8],
[1, 4, 7], [2, 5, 8], [2, 4, 6]]
#Speed of the wheels
speed = 10
# number of rotations to pick up a coin
pickingUpCoin = -0.16
# lower the claw this number of rotations + extra for each coin to pick up
lowerToReachCoin = 0.12
# - PiCamera
# - LEGO LargeMotor connected to port BAM1
# - LEGO LargeMotor connected to port BBM1
# - LEGO LargeMotor connected to port BBM2
# - LEGO MediumMotor connected to port BAM2
# - LEGO TouchSensor connected to port BAS2
# - LEGO TouchSensor connected to port BBS2
#
from time import sleep
from ev3dev2.port import LegoPort
from ev3dev2.motor import Motor, LargeMotor, OUTPUT_D
from ev3dev2.sensor.lego import TouchSensor
# Configure ports for sensors
port_BAS2 = LegoPort('pistorms:BAS2')
port_BAS2.mode = 'ev3-analog'
port_BBS2 = LegoPort('pistorms:BBS2')
port_BBS2.mode = 'ev3-analog'
sleep(2)
class ExceededRetries(Exception):
''' Custom exception for exceeding retries at Remote I/O error.'''
pass
def try_except(func):
''' Retry function when Remote I/O error occurs.'''
def wrapper(*args, **kwargs):
for i in range(10):
from pybricks.robotics import DriveBase
import utime, array
from pybricks.ev3devio import Ev3devSensor
class EV3Sensor(Ev3devSensor):
_ev3dev_driver_name = 'ev3-analog-01'
def read(self):
self._mode('ANALOG')
return self._value(0)
# this is a hack to set the mode properly
from ev3dev2.port import LegoPort
s2 = LegoPort(address='ev3-ports:in2')
s2.mode = 'ev3-analog'
utime.sleep(1)
s3 = LegoPort(address='ev3-ports:in3')
s3.mode = 'ev3-analog'
utime.sleep(1)
# Write your program here
brick.sound.beep()
# testing
# sensor=EV3Sensor(Port.S1) # same port as above
# while True:
# print(sensor.read())
# wait(2000)
#!/usr/bin/env python3 # # uses ev3dev2-based code # # Date Author Change Notes # 4/12/2020 Ron King initial dev import time, tty, sys from ev3dev2 import list_devices from ev3dev2.port import LegoPort from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import SoundSensor p1 = LegoPort(INPUT_1) # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes p1.mode = 'nxt-analog' # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html p1.set_device = 'lego-nxt-sound' # Connect infrared to any sensor port ss = SoundSensor(INPUT_1) # allow for some time to load the new drivers time.sleep(0.5) ssDbVal = 0 prev_ssDbVal = 0 ssDbaVal = 0
# Date Author Change Notes
# 5/10/2021 Ron King initial dev
#
# 5/31/2021 Ron King - work-around for bogus intermittent readings of '0' (possibly a f/w bug in BrickPi3)
#
# ref. https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py
import time, tty, sys
from ev3dev2 import list_devices
from ev3dev2.port import LegoPort
from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent
from ev3dev2.sensor import INPUT_3
from ev3dev2.sensor.lego import GyroSensor
p3 = LegoPort(INPUT_3)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes
p3.mode = 'ev3-uart'
# allow for some time for mode to setup
time.sleep(0.5)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors
""" https://github.com/ev3dev/ev3dev/issues/640
dlech commented on May 15, 2016
You are forgetting the I2C address for set_device
$ echo mx-pps58-nx 0x0C >/sys/class/lego-port/port0/set_device
# port B = Large Motor
# port C = Large Motor
# port D = Medium Motor (Lift)
tank_pair = MoveTank(OUTPUT_B,OUTPUT_C)
steer_pair = MoveSteering(OUTPUT_B,OUTPUT_C)
MediumMotor_Claw = MediumMotor(OUTPUT_A)
MediumMotor_lift = MediumMotor(OUTPUT_D)
sound = Sound()
leds = Leds()
btn = Button()
# Mindstorms EV3-Sensor-Mux connected to EV3 Ultrasonic Sensor, Infrared Sensor and Gyro Sensor --------------------
us_port = LegoPort(address='ev3-ports:in2:i2c80:mux1')
ir_port = LegoPort(address='ev3-ports:in2:i2c81:mux2')
gyro_port = LegoPort(address='ev3-ports:in2:i2c82:mux3')
us_port.mode = 'uart'
ir_port.mode = 'uart'
gyro_port.mode = 'uart'
us_port.set_device = 'lego-ev3-us'
ir_port.set_device = 'lego-ev3-ir'
gyro_port.set_device = 'lego-ev3-gyro'
sleep(2)
us = UltrasonicSensor('ev3-ports:in2:i2c80:mux1')
ir = InfraredSensor('ev3-ports:in2:i2c81:mux2')
# uses ev3dev2-based code # # Date Author Change Notes # 4/15/2020 Ron King initial dev # ref. https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py import time, tty, sys from ev3dev2 import list_devices from ev3dev2.port import LegoPort from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import Sensor p1 = LegoPort(INPUT_1) # 'serial0-0:S1' # spi0.1:S1:i2c1 # spi0.1:S1:i2c17 # spi0.1:S1:i2c34 # p1 = LegoPort(address='serial0-0:S1:i2c22') # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes p1.mode = 'nxt-i2c' # allow for some time for mode to setup time.sleep(0.5) # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors """ https://github.com/ev3dev/ev3dev/issues/640
# uses ev3dev2-based code
#
# Date Author Change Notes
# 4/16/2020 Ron King initial dev
# ref. https://github.com/ev3dev/ev3dev-lang-python-demo/blob/stretch/platform/brickpi3-motor-and-sensor.py
import time, tty, sys
from ev3dev2 import list_devices
from ev3dev2.port import LegoPort
from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent
from ev3dev2.sensor import INPUT_2
from ev3dev2.sensor.lego import Sensor
# import ev3dev2.sensor
p2 = LegoPort(INPUT_2)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes
p2.mode = 'nxt-i2c'
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors
""" https://github.com/ev3dev/ev3dev/issues/640
dlech commented on May 15, 2016
You are forgetting the I2C address for set_device
$ echo mx-pps58-nx 0x0C >/sys/class/lego-port/port0/set_device
dmesg -w
"""
# - DON'T Forget to start xming and export DISPLAY=10.0.0.9:0.0 (change IP addr as req'd) import time, tty, sys, threading import matplotlib.pyplot as plt import matplotlib.animation as animation from matplotlib import style import numpy as np from math import sin, cos, radians from ev3dev2 import list_devices from ev3dev2.port import LegoPort from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, SpeedDPS, MoveTank from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3 from ev3dev2.sensor.lego import InfraredSensor, UltrasonicSensor from ev3dev2.sensor.lego import Sensor p1 = LegoPort(INPUT_1) # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes p1.mode = 'ev3-uart' # http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html#supported-sensors p1.set_device = 'lego-ev3-us' # Connect lego-ev3-us to any sensor port us = UltrasonicSensor(INPUT_1) # allow for some time to load the new drivers time.sleep(0.5) usDistCmVal = 0 prev_usDistCmVal = 0 p2 = LegoPort(INPUT_2)
def temp_C(self, ADC: [int, float]):
self.Rt = self.Ro / ((self._maxADC / ADC - 1))
return self._steinhart_temperature_C_(self.Rt)
#Definerer NTC parametere:
a = ntc()
a.beta = 3984
a.maxADC(5000)
#
#tempS = TemperatureSensor(Port.S4)
#Definerer port 4 til å være analog inngang:
p4 = LegoPort(INPUT_4)
p4.mode = 'nxt-analog'
p4.set_device = 'nxt-analog'
#Definerer at temperatursensoren er koblet til port 4:
ntc_sensor = Sensor(INPUT_4)
while True:
#Leser av sensorverdi i volt:
msr = ntc_sensor.value()
#Konverterer til temperatur:
t = a.temp_C(msr)
# Printer temperaturen til EV3 LCD skjerm