def __init__(self,
port_motor_rear=OUTPUT_A,
port_motor_steer=OUTPUT_B,
port_sensor_gyro='in1',
port_sensor_us_front='in2',
port_sensor_us_rear='in3'):
# initialization of devices
self.__button_halt = Button()
self.__motor_rear = LargeMotor(port_motor_rear)
self.__motor_steer = LargeMotor(port_motor_steer)
self.__sensor_gyro = GyroSensor(port_sensor_gyro)
self.__sensor_us_rear = UltrasonicSensor(port_sensor_us_rear)
self.__sensor_us_front = UltrasonicSensor(port_sensor_us_front)
self.__velocity_controller = VelocityController(self, 0, 0)
# NOTE: possible using other controllers. Change only here!
self.__trajectory_controller = ControllerWithLinearization()
self.__localization = Localization(self)
self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega
self.reset()
Sound.speak('Initialization complete!').wait()
def __init__(self, left_motor_port, right_motor_port, front_us_port,
right_us_port, left_us_port):
self.leftMotor = LargeMotor('out' + left_motor_port)
self.rightMotor = LargeMotor('out' + right_motor_port)
self.FRONT_US_SENSOR = UltrasonicSensor('in' + front_us_port)
self.RIGHT_US_SENSOR = UltrasonicSensor('in' + right_us_port)
self.LEFT_US_SENSOR = UltrasonicSensor('in' + left_us_port)
self.TOUCH_SENSOR = TouchSensor()
assert self.leftMotor.connected, "Connect left Motor to port" + \
str(left_motor_port)
assert self.rightMotor.connected, "Connect right Motor to port" + \
str(right_motor_port)
assert self.TOUCH_SENSOR.connected, "Connect a touch sensor"
assert self.FRONT_US_SENSOR.connected, "Connect the ultrasound sensor in the front"
assert self.RIGHT_US_SENSOR.connected, "Connect the ultrasound sensor on the right"
assert self.LEFT_US_SENSOR.connected, "Connect the ultrasound sensor on the left"
#set sensor mode to cm
self.FRONT_US_SENSOR.mode = 'US-DIST-CM'
self.RIGHT_US_SENSOR.mode = 'US-DIST-CM'
self.LEFT_US_SENSOR.mode = 'US-DIST-CM'
#!/usr/bin/env python3
from ev3dev.ev3 import ColorSensor, INPUT_1, INPUT_2, OUTPUT_A, OUTPUT_B, LargeMotor, Button, GyroSensor, Sound, UltrasonicSensor
from PID import PID
from os import system
from time import sleep
from json import dump, load
# Instanciando sensores
cl_left = ColorSensor(address=INPUT_1)
cl_right = ColorSensor(address=INPUT_2)
gyro = GyroSensor('in3')
sonic = UltrasonicSensor('in4')
# Instanciando motores
m_left = LargeMotor(address=OUTPUT_A)
m_right = LargeMotor(address=OUTPUT_B)
# Verificando se os sensores/motores estão conectados
assert cl_left.connected
assert cl_right.connected
assert gyro.connected
assert sonic.connected
assert m_left.connected
assert m_right.connected
# Definindo modo reflectância
cl_left.mode = 'COL-REFLECT'
cl_right.mode = 'COL-REFLECT'
gyro.mode = 'GYRO-ANG'
from ev3dev.ev3 import LargeMotor, ColorSensor, UltrasonicSensor, Sound, GyroSensor
from enum import Enum, IntEnum
from pickled import pickled
from ring import RingBuf
from util import SubinstructionError
# -- MOTORS --
mLeft = LargeMotor('outD')
mRight = LargeMotor('outC')
mRight.polarity = 'inversed'
mLeft.polarity = 'inversed'
# -- SENSORS --
sSonic = UltrasonicSensor('in1')
sSonic.mode = UltrasonicSensor.MODE_US_DIST_CM
sGyro = GyroSensor('in2')
sGyro.mode = GyroSensor.MODE_GYRO_ANG
cLeft = ColorSensor('in3')
cRight = ColorSensor('in4')
""" Marks which side of the robot the line is on. """
class MoveDir(IntEnum):
LINE_LEFT = -1
LINE_RIGHT = 1
""" Minimum and maximum reflectance the line sensor sees. """
#!/usr/bin/env python3
from time import sleep
from ev3dev.ev3 import LargeMotor, GyroSensor, UltrasonicSensor, ColorSensor, Sound
from os import system
system('setfont Lat15-TerminusBold14')
cl_left = ColorSensor('in1')
cl_right = ColorSensor('in4')
l = LargeMotor('outA')
r = LargeMotor('outC')
gyro = GyroSensor('in2')
sonic = UltrasonicSensor('in3')
gradient = []
count = 0
while 1:
l.run_forever(speed_sp=-300)
r.run_forever(speed_sp=-300)
count += 1
if(count % 25 == 0):
count = 1
gradient.append((cl_left.value(), cl_right.value()))
if(len(gradient) > 10):
class Robot(object):
BASE = 12.3 #base of the tire
RADUIS = 3 #radius of the tire
CIRCUMFERENCE = 17.2 #circumference of the tires
'''
left_motor_port :: left motor port
right_motor_port :: right motor port
front_us_port :: front ultrasonic sensor port
right_us_port ::right ultrasonic sensor port
left_us_port ::left ultrasonic sensor port
'''
def __init__(self, left_motor_port, right_motor_port, front_us_port,
right_us_port, left_us_port):
self.leftMotor = LargeMotor('out' + left_motor_port)
self.rightMotor = LargeMotor('out' + right_motor_port)
self.FRONT_US_SENSOR = UltrasonicSensor('in' + front_us_port)
self.RIGHT_US_SENSOR = UltrasonicSensor('in' + right_us_port)
self.LEFT_US_SENSOR = UltrasonicSensor('in' + left_us_port)
self.TOUCH_SENSOR = TouchSensor()
assert self.leftMotor.connected, "Connect left Motor to port" + \
str(left_motor_port)
assert self.rightMotor.connected, "Connect right Motor to port" + \
str(right_motor_port)
assert self.TOUCH_SENSOR.connected, "Connect a touch sensor"
assert self.FRONT_US_SENSOR.connected, "Connect the ultrasound sensor in the front"
assert self.RIGHT_US_SENSOR.connected, "Connect the ultrasound sensor on the right"
assert self.LEFT_US_SENSOR.connected, "Connect the ultrasound sensor on the left"
#set sensor mode to cm
self.FRONT_US_SENSOR.mode = 'US-DIST-CM'
self.RIGHT_US_SENSOR.mode = 'US-DIST-CM'
self.LEFT_US_SENSOR.mode = 'US-DIST-CM'
#move straight
def moveStraight(self, distance, speed):
n = (360 * distance) / self.CIRCUMFERENCE
self.rightMotor.run_to_rel_pos(position_sp=n,
speed_sp=speed,
stop_action="brake")
self.leftMotor.run_to_rel_pos(position_sp=n,
speed_sp=speed,
stop_action="brake")
self.rightMotor.wait_while('running')
self.leftMotor.wait_while('running')
#move backward
def moveBackward(self, distance, speed):
n = (360 * distance) / self.CIRCUMFERENCE
n = (-1 * n)
self.rightMotor.run_to_rel_pos(position_sp=n,
speed_sp=speed,
stop_action="brake")
self.leftMotor.run_to_rel_pos(position_sp=n,
speed_sp=speed,
stop_action="brake")
self.rightMotor.wait_while('running')
self.leftMotor.wait_while('running')
#left left
def turnLeft(self, distance, speed):
n = (360 * distance) / self.CIRCUMFERENCE
m = (-1 * n)
self.rightMotor.run_to_rel_pos(position_sp=n,
speed_sp=speed,
stop_action="brake")
self.leftMotor.run_to_rel_pos(position_sp=m,
speed_sp=speed,
stop_action="brake")
self.rightMotor.wait_while('running')
self.leftMotor.wait_while('running')
#turn right
def turnRight(self, distance, speed):
n = (360 * distance) / self.CIRCUMFERENCE
m = (-1 * n)
self.rightMotor.run_to_rel_pos(position_sp=m,
speed_sp=speed,
stop_action="brake")
self.leftMotor.run_to_rel_pos(position_sp=n,
speed_sp=speed,
stop_action="brake")
self.rightMotor.wait_while('running')
self.leftMotor.wait_while('running')
#stop robot movement
def stopMotor(self):
self.rightMotor.stop()
self.leftMotor.stop()
#get ultrasonic sensor reading
def getSensorReading(self, sensor):
if sensor == 'front':
reading = self.FRONT_US_SENSOR.value() / 10
elif sensor == 'right':
reading = self.RIGHT_US_SENSOR.value() / 10
elif sensor == 'left':
reading = self.LEFT_US_SENSOR.value() / 10
return reading
from ev3dev.ev3 import ColorSensor, Motor, UltrasonicSensor
from .driver import Driver
from .linesensorarray import LineSensorArray
from .colorsensor import ColorSensor
driver = Driver('outA', 'outB')
arm = Motor('outC')
claws = Motor('outD')
line_sensor_array = LineSensorArray('in1')
color_sensors = (ColorSensor('in2'), ColorSensor('in3'))
ultrasonic_sensor = UltrasonicSensor('in4')
# central_line_sensor = ColorSensor('in4')
#!/usr/bin/env python3
from ev3dev.ev3 import UltrasonicSensor, ColorSensor, Button
from os import system
import paho.mqtt.client as mqtt
# Sensores ultrasonicos
ULTRA1 = UltrasonicSensor("in1")
ULTRA2 = UltrasonicSensor("in2")
ULTRA1.mode = "US-DIST-CM"
ULTRA2.mode = "US-DIST-CM"
# Sensores de Cor
CL1 = ColorSensor("in3")
CL2 = ColorSensor("in4")
CL1.mode = "COL-COLOR"
CL2.mode = "COL-COLOR"
DISTANCIA_BONECOS = 20
cor_anterior1 = ""
cor_anterior2 = ""
client = mqtt.Client()
botao = Button()
client.connect("localhost", 1883, 60)
client.loop_start()
def on_disconnect(client, userdata, rc=0):
client.loop_stop()
#!/usr/bin/env python3
from ev3dev.ev3 import LargeMotor, ColorSensor, UltrasonicSensor, InfraredSensor, MediumMotor, INPUT_1, INPUT_2, INPUT_3, INPUT_4
# MOTORES
esq = LargeMotor('outA')
dir = LargeMotor('outB')
motor_garra = LargeMotor('outC')
#motor_sensor = MediumMotor('outD')
# SENSORES
sensor_esq = ColorSensor(address=INPUT_1)
sensor_dir = ColorSensor(address=INPUT_2)
sensor_frente = UltrasonicSensor(address=INPUT_3)
sensor_lado = UltrasonicSensor(address=INPUT_4)
sensor_esq.mode = 'COL-REFLECT'
sensor_dir.mode = 'COL-REFLECT'
from time import sleep
from ev3dev.ev3 import MediumMotor, UltrasonicSensor
us = UltrasonicSensor()
SM = MediumMotor("outC")
def sonicValue(tolerance=10):
cache = [1, 100]
while abs(cache[0] - cache[1]) > tolerance and not (cache[0] > 21.5
and cache[1] > 21.5):
cache[0] = us.value() / 10
sleep(0.025)
cache[1] = us.value() / 10
sleep(0.025)
return sum(cache) / len(cache)
data = [0, 0, 0]
while True:
data[1] = sonicValue()
SM.run_to_rel_pos(position_sp=90, speed_sp=1550, stop_action="hold")
sleep(0.16)
data[0] = sonicValue()
SM.run_to_rel_pos(position_sp=-180, speed_sp=1550, stop_action="hold")
sleep(0.35)
data[2] = sonicValue()
SM.run_to_rel_pos(position_sp=90, speed_sp=1550, stop_action="hold")
sleep(0.2)
#!/usr/bin/env python3
from time import sleep
from ev3dev.ev3 import LargeMotor, GyroSensor, UltrasonicSensor, ColorSensor
from os import system
system('setfont Lat15-TerminusBold14')
cl_left = ColorSensor('in1')
cl_right = ColorSensor('in2')
l = LargeMotor('outB')
r = LargeMotor('outA')
gyro = GyroSensor('in3')
sonic = UltrasonicSensor('in4')
def girar(graus):
pos0 = gyro.value()
if (graus > 0):
while gyro.value() < pos0 + graus:
l.run_forever(speed_sp=-500)
r.run_forever(speed_sp=250)
else:
while gyro.value() > pos0 + graus:
l.run_forever(speed_sp=250)
r.run_forever(speed_sp=-500)
l.stop()
r.stop()
#!/usr/bin/env python3
from ev3dev.ev3 import UltrasonicSensor
from os import system
system('setfont Lat15-TerminusBold14')
sonic = UltrasonicSensor('in3')
while True:
print(sonic.value())
system("clear")