#!/usr/bin/env python3
# Constants are tricky to do in Python, as they are not
# supported by the language. # There are several
# recognised hacks, including the use of namedtuple
import ev3dev.ev3 as ev3
import collections
import time
ev3.Sound.beep().wait()
Constants = collections.namedtuple('Constants', ['PortLSLeft', 'PortLSRight'])
GConstants = Constants('in1', 'in4')
GLSLeft = ev3.ColorSensor(GConstants.PortLSLeft)
GLSLeft.mode = 'COL-REFLECT'
GLSRight = ev3.ColorSensor(GConstants.PortLSRight)
GLSRight.mode = 'COL-REFLECT'
print(str(GLSLeft.value()))
print(str(GLSLeft.value()))
print("----------")
time.sleep(5)
a connection to the remote control that sends the arm up if the ir remote control up button
is pressed. That's a specific input --> output task. Maybe some other task would want to use
the IR remote up button for something different. Instead just make a method called arm_up that
could be called. That way it's a generic action that could be used in any task.
"""
import ev3dev.ev3 as ev3
import math
import time
import mqtt_remote_method_calls as com
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
arm_motor = ev3.MediumMotor(ev3.OUTPUT_A)
touch_sensor = ev3.TouchSensor()
color_sensor = ev3.ColorSensor()
btn = ev3.Button()
class Snatch3r(object):
"""Commands for the Snatch3r robot that might be useful in many different programs."""
def __init__(self):
self.color_sensor = ev3.ColorSensor()
assert self.color_sensor
self.ir_sensor = ev3.InfraredSensor()
assert self.ir_sensor
self.pixy = ev3.Sensor(driver_name="pixy-lego")
assert self.pixy
def drive_inches(self, lenth, velocity):
""" """
def __init__(self, port): # port must be 3
self._color_sensor = ev3.ColorSensor('in' + str(port))
def __init__(self, in3):
self.sd = ev3.ColorSensor(in3)
assert self.sd.connected
def run(self):
# sensors
cs = ev3.ColorSensor(); assert cs.connected # measures light intensity
us = ev3.UltrasonicSensor(); assert us.connected # measures distance
cs.mode = 'COL-REFLECT' # measure light intensity
us.mode = 'US-DIST-CM' # measure distance in cm
# motors
lm = ev3.LargeMotor('outB'); assert lm.connected # left motor
rm = ev3.LargeMotor('outC'); assert rm.connected # right motor
# mm = ev3.MediumMotor('outD'); assert mm.connected # medium motor
speed = 360/4 # deg/sec, [-1000, 1000]
dt = 500 # milliseconds
stop_action = "coast"
# PID tuning
Kp = 1 # proportional gain
Ki = 0 # integral gain
Kd = 0 # derivative gain
integral = 0
previous_error = 0
# initial measurment
target_value = cs.value()
# Start the main loop
while not self.shut_down:
# deal with obstacles
distance = us.value() // 10 # convert mm to cm
if distance <= 5: # sweep away the obstacle
# mm.run_timed(time_sp=600, speed_sp=+150, stop_action="hold").wait()
# mm.run_timed(time_sp=600, speed_sp=-150, stop_action="hold").wait()
# Calculate steering using PID algorithm
aerror = target_value - cs.value()
integral += (aerror * dt)
derivative = (aerror - previous_error) / dt
# u zero: on target, drive forward
# u positive: too bright, turn right
# u negative: too dark, turn left
u = (Kp * aerror) + (Ki * integral) + (Kd * derivative)
# limit u to safe values: [-1000, 1000] deg/sec
if speed + abs(u) > 1000:
if u >= 0:
u = 1000 - speed
else:
u = speed - 1000
# run motors
if u >= 0:
lm.run_timed(time_sp=dt, speed_sp=speed + u, stop_action=stop_action)
rm.run_timed(time_sp=dt, speed_sp=speed - u, stop_action=stop_action)
sleep(dt / 1000)
else:
lm.run_timed(time_sp=dt, speed_sp=speed - u, stop_action=stop_action)
rm.run_timed(time_sp=dt, speed_sp=speed + u, stop_action=stop_action)
sleep(dt / 1000)
previous_error = aerror
# Check if buttons pressed (for pause or _stop)
if not self.btn.down: # Stop
print("Exit program... ")
self.shut_down = True
elif not self.btn.left: # Pause
print("[Pause]")
self.pause()
# 'Pause' method
def pause(self, pct=0.0, adj=0.01):
while self.btn.right or self.btn.left: # ...wait 'right' button to unpause
ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.AMBER, pct)
ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.AMBER, pct)
if (pct + adj) < 0.0 or (pct + adj) > 1.0:
adj = adj * -1.0
pct = pct + adj
print("[Continue]")
ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN)
ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN)
def __init__(self, mediumMotorOut='outA'):
self.mediumMotorOut = mediumMotorOut
self.mediumMotor = ev3.Motor(mediumMotorOut)
self.touchSensor = ev3.TouchSensor()
self.colorSensor = ev3.ColorSensor()
def stop(self):
self.mc.stop()
def start(self):
self.lineFollowing()
if __name__ == "__main__":
done = False
rm = ev3.LargeMotor('outC')
lm = ev3.LargeMotor('outB')
lf = ev3.MediumMotor('outA')
gs = ev3.GyroSensor()
cs = ev3.ColorSensor()
mc = Movement.MovementController(lm, rm)
ac = Arm.ArmController(lf)
rc = Route.RoutingController('', '')
# action = ["pickup", "putdown", "pickup", "putdown"]
assert cs.connected
assert gs.connected
cs.mode = "COL-COLOR"
gs.mode = "GYRO-ANG"
robot = Robot("Start", [], [], gs, cs, mc, ac, rc)
while not done:
try:
def aroundObstacle():
# connecting motors and sensors
motorL = ev3.LargeMotor('outA')
motorL.connected
motorR = ev3.LargeMotor('outD')
motorR.connected
motorM = ev3.MediumMotor('outB')
motorM.connected
c = ev3.ColorSensor(ev3.INPUT_3)
c.connected
c.mode = 'COL-REFLECT'
sonar = ev3.UltrasonicSensor(ev3.INPUT_4)
sonar.connected
sonar.mode = 'US-DIST-CM'
# adaptation of TaskA followline_PID function
def moving():
# Constants for PID
offset = 45
Tp = 20
Kp = 26
# move forward until sonar detects object
while (sonar.value() > 80):
color = c.value()
error = color - offset
turn = Kp * error
turn = turn / 100
powerL = Tp - turn
powerR = Tp + turn
motorR.run_timed(duty_cycle_sp=powerL, time_sp=150)
motorL.run_timed(duty_cycle_sp=powerR, time_sp=150)
time.sleep(.1)
# then switch to circumvent function
ev3.Sound.speak('Obstacle Detected').wait
time.sleep(1)
circumvent()
# function to handles moving around obstacle
def circumvent():
# initial turn when obstacle is detected
motorR.run_timed(duty_cycle_sp=30, time_sp=3000)
motorL.run_timed(duty_cycle_sp=-90, time_sp=3000)
time.sleep(.2)
motorM.run_timed(duty_cycle_sp=30, time_sp=800)
time.sleep(.2)
# move forward for a bit as a buffer between turns
motorL.run_timed(duty_cycle_sp=25, time_sp=800)
motorR.run_timed(duty_cycle_sp=25, time_sp=800)
time.sleep(.2)
# PID constants
offset = 90
Tp = 30
Kp = 12
Ki = 0
Kd = 0
lastError = 0
integral = 0
colorVal = c.value()
# while color sensor doesn't detect black line, go around obstacle
# using PID controller proportional to ultrasonic value
while (c.value() > 15):
s = sonar.value()
error = s - offset
integral = integral + error
derivative = error - lastError
turn = Kp * error + Ki * integral + Kd * derivative
turn = turn / 1000
powerL = Tp + turn
powerR = Tp - turn
motorL.run_timed(duty_cycle_sp=powerL, time_sp=150)
motorR.run_timed(duty_cycle_sp=powerR, time_sp=150)
lastError = error
time.sleep(.1)
# when color sensor detects black line, turn and follow line again
ev3.Sound.speak('Found black line again').wait()
motorR.run_timed(duty_cycle_sp=60, time_sp=3000)
motorL.run_timed(duty_cycle_sp=-60, time_sp=3000)
time.sleep(.2)
motorM.run_timed(duty_cycle_sp=-30, time_sp=800)
time.sleep(.2)
moving()
# start with moving function
moving()
import ev3dev.ev3 as ev3
from threading import Thread
from time import sleep
ir_right = ev3.InfraredSensor('in3')
ir_left = ev3.UltrasonicSensor('in1')
ir_left.mode = "US-DIST-CM"
ir_right.mode = 'IR-PROX'
color = ev3.ColorSensor() #in2
color.mode = 'COL-COLOR'
while True:
dist1 = ir_right.value()
dist2 = ir_left.value() / 10
if dist1 <= 80 or dist2 < 60:
print('True')
if color.value() != 6:
print("Drive")
else:
print("Stop")
else:
print("Far", dist1)
print("Close", dist2)
if color.value() != 6:
print("Search")
else:
print("Stop")
sleep(1)
def __init__(self):
# define sensors
self.gyroscope_sensor = ev3.GyroSensor('in3')
self.gyroscope_sensor.mode = 'GYRO-RATE'
self.gyroscope_sensor.mode = 'GYRO-ANG'
self.DEFAULT_SPEED = DEFAULT_SPEED
self.color_sensors = Duo(ev3.ColorSensor('in1'),
ev3.ColorSensor('in2'))
self.ultrasonic_sensor = ev3.UltrasonicSensor('in4')
self.ta_no_final_da_pista = False
self.time_desabilita_o_realinhamento_da_cor = datetime.now()
self.ta_na_ranpa = False
# define motors
self.motors = Duo(ev3.LargeMotor('outA'), ev3.LargeMotor('outD'),
ev3.LargeMotor('outC'))
# self.handler = ev3.LargeMotor('outC')
# self.learned_colors = {}
# self.learned_colors = {'Blue': ['left'], 'Red': ['forward'], 'Green': ['right']}
self.primeiro_bounding_box = True
# define status
self.historic = [""]
self.undefined_counter = 0
self.in_rect = False
self.rect_color = "Undefined"
self.reverse_path = None
self.dor_open = True
self.has_doll = False # OBS: LEMBRAR DE SETAR PRA FALSE
self.done_learning = False
self.voltou = False
self.tempo_para_chamar_run_action = datetime.now()
self.has_came_from_json = False
self.bounding_box = False
# self.historic = ['', 'left', 'forward', 'right', 'right', 'forward', 'left']
# self.reverse_path = True
# self.dor_open = True
# self.has_doll = True
# self.done_learning = True
self.nao_pode = False
self.realigment_counter = 0
self.starting_angle = self.sensor_data("GyroSensor")
# define network sensors
self.infrared_sensors = (0, 0)
self.white_counter = 0
# path detection variables
self.fila_para_registro_do_fim = ["White", "White"]
# contador de tempo para o identificar de fim de pista
self.kon_const = 25
self.kon = self.kon_const + 1
self.file_name = "learned_colors.json"
self.fixed_file_name = "learning_dic.json"
# tenta abrir arquivo json com cores aprendidas
try:
with open(self.file_name) as json_file:
process = json.load(json_file)
self.learned_colors = process[0]
except FileNotFoundError:
self.learned_colors = {}
# tenta abrir achar arquivo de aprendizado individual, se nao existir cria:
try:
with open(self.fixed_file_name) as json_file:
process = json.load(json_file)
for k in sorted(self.learned_colors.keys()):
self.learned_colors[k][-1] = 0
except FileNotFoundError:
with open(self.fixed_file_name, 'w') as outfile:
json.dump({}, outfile)
import ev3dev.ev3 as ev3
import time
m1 = ev3.LargeMotor('outA')
m2 = ev3.LargeMotor('outD')
m3 = ev3.LargeMotor('outC')
cs = ev3.ColorSensor('in4')
us = ev3.UltrasonicSensor('in2')
#gs = ev3.GyroSensor('in3')
us.mode = 'US-DIST-CM'
speed = 2.2
motortime = 1000 #ms
def forward():
for i in range(int(motortime / 100)):
if way_blocked():
m1.stop()
m2.stop()
time.sleep(0.2)
i = i - 1
else:
m1.run_timed(speed_sp=-speed * 100, time_sp=120)
m2.run_timed(speed_sp=-speed * 100, time_sp=120)
m3.stop()
def line_detected():
#table is about 60, white paper is about 90
return cs.reflected_light_intensity > 75
#!/usr/bin/python3
#https://sites.google.com/site/ev3python/learn_ev3_python/using-sensors/sensor-modes
#http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-jessie/sensor_data.html
import ev3dev.ev3 as ev3
sensorRight = ev3.ColorSensor('in1')
sensorLeft = ev3.ColorSensor('in4')
sensorGripper = ev3.LightSensor('in3')
assert sensorRight.connected, "sensorRight(ColorSensor) is not connected"
assert sensorLeft.connected, "sensorLeft(ColorSensor) is not conected"
assert sensorGripper.connected, "sensorGripper(LightSensor) is not conected"
AMBIENT = 0
REFLECT = 1
COLOR = 2
arrayofcolors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red', 'white',
'brown')
filterValue = [0, 0, 0, 0]
def setmodeSensorsLR(mode):
if mode == 0:
sensorLeft.mode = 'COL-AMBIENT' # measures lux
sensorRight.mode = 'COL-AMBIENT' # measures lux
sensorGripper.mode = 'AMBIENT'
print("Sensor mode set to ambient")
elif mode == 1:
sensorLeft.mode = 'COL-REFLECT' # measures light intensity
#!/usr/bin/python3
# Echo server program
import socket,time,os,struct,threading
import ev3dev.ev3 as ev3
from time import sleep
B = ev3.MediumMotor('outB')
C = ev3.MediumMotor('outC')
cs1 = ev3.ColorSensor("in4")
cs1.mode = 'COL-COLOR'
ip = socket.gethostbyname(socket.gethostname())
cc_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
cc_sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
score = 0
lastDir = "S"
print("INIT")
speed = 400
class Move(object):
def __init__(self):
self.host = '' # Symbolic name meaning all available interfaces
self.port = 50007 # Arbitrary non-privileged port
self.active = 1
print("MOVE INIT")
def drive(self):
global speed, lastDir
#!/usr/bin/env python3
# coding: utf-8
import ev3dev.ev3 as ev3
from ev3dev.ev3 import *
#from multiprocessing import Process
from time import sleep
from time import time
cor = ev3.ColorSensor('in3')
assert cor.connected
class Calibracao:
def __init__(self, color, speed, time):
self.color = color
self.speed = speed
self.time = time
self.p1 = [1021, -1]
self.p2 = [1021, -1]
self.p3 = [1021, -1]
def calibrate(self, wait_time, repeat):
for i in range(repeat):
cor_lida = cor.raw
self.p1[0] = min(cor.raw[0], self.p1[0])
self.p1[1] = max(cor.raw[0], self.p1[1])
self.p2[0] = min(cor.raw[1], self.p2[0])
self.p2[1] = max(cor.raw[1], self.p2[1])
self.p3[0] = min(cor.raw[2], self.p3[0])
self.p3[1] = max(cor.raw[2], self.p3[1])
sleep(wait_time)
#!/usr/bin/env python3
import ev3dev.ev3 as ev3
import time
ev3.Sound.beep().wait()
cl = ev3.ColorSensor('in1')
cl.mode = 'RGB-RAW'
def readAverageLightRGB(ACount):
LRSum = 0
LGSum = 0
LBSum = 0
for i in range(0, ACount - 1):
LRSum += cl.value(0)
LGSum += cl.value(1)
LBSum += cl.value(2)
time.sleep(0.1)
return LRSum / ACount, LGSum / ACount, LBSum / ACount
while True:
LR, LG, LB = readAverageLightRGB(10)
print(LR, LG, LB)
time.sleep(1)
#!/usr/bin/env python3
import ev3dev.ev3 as ev3
import time
ev3.Sound.beep().wait()
btn = ev3.Button()
GLightRight = ev3.ColorSensor('in4')
GLightRight.mode = 'COL-REFLECT'
GMotorLeft = ev3.Motor()
GMotorLeft = ev3.LargeMotor('outD')
GMotorRight = ev3.Motor()
GMotorRight = ev3.LargeMotor('outA')
while not btn.backspace:
if GLightRight.value() > 40:
GMotorLeft.run_forever(speed_sp=450) # equivalent to power=20 in EV3-G
GMotorRight.stop(stop_action='brake')
else:
GMotorRight.run_forever(
speed_sp=450) # equivalent to power=20 in EV3-G
GMotorLeft.stop(stop_action='brake')
time.sleep(0.01)
#!/usr/bin/env python3
import ev3dev.ev3 as ev3
from time import time, sleep
# Variaveis dos sensores e motores, e os modos dos sensores
M_PORTA = ev3.MediumMotor('outB')
M_DIREITO = ev3.LargeMotor("outD")
M_ESQUERDO = ev3.LargeMotor("outC")
CL = ev3.ColorSensor("in1")
CL.mode = "COL-COLOR"
PROX1 = ev3.InfraredSensor("in3") # Direita
PROX2 = ev3.InfraredSensor("in2") # Esquerda
PROX3 = ev3.UltrasonicSensor("in4")
PROX1.mode = "IR-PROX"
PROX2.mode = "IR-PROX"
PROX3.mode = "US-DIST-CM"
# Variaveis usadas durante o programa
DISTANCIA_BONECOS = 30
VELOCIDADE_CURVA = 300
VELOCIDADE_ATUAL = 400
KP = 1.7
VERDE = 3
VERMELHO = 5
AMARELO = 4
AZUL = 2
PRETO = 1
quant_bonecos = 0
lista_valores = [0, 0, 0
] # indice 0 - VERMELHO, indice 1 - VERDE, indice 2 - AMARELO
import ev3dev.ev3 as ev3
from time import sleep
import functions as FN
from random import randint
ColorS = ev3.ColorSensor() # Sensor de color
ProxS = ev3.UltrasonicSensor() # Sensor Ultrasonico (proximidad)
ProxTS = ev3.UltrasonicSensor() # Sensor Ultrasonico de arriba(proximidad)
GyroS = ev3.GyroSensor() # Sensor Giroscopio
MotorL = ev3.LargeMotor('outA') # Motor izquierdo
MotorR = ev3.LargeMotor('outD') # Motor derecho
MotorS = ev3.MediumMotor('outB') # Motor del sensor de arriba
MotorG = ev3.MediumMotor('outC') # Motor del sensor de arriba
Finish = False
MadeM = []
MadeCM = []
MadeCMR = []
AllPosibleM = [90,-90,0, 180]
CurrentPosibleM = [90,-90,0]
BaseColors = ['NoneColor', 'White', 'Brown']
StopColor = 'Black' ## Deberia ser Black
CurrentAngle = 0
def Stop():
return MotorL.stop(), MotorR.stop()
FN.LoadMotors(MotorL, MotorR, MotorS, MotorG)
FN.LoadSensors(ColorS, ProxS, ProxTS, GyroS)
#!/usr/bin/env python3
import ev3dev.ev3 as ev3
from multiprocessing import Process
from time import sleep
################################################################
##################### SENSORES E MOTORES #######################
################################################################
motorEsq = ev3.LargeMotor('outC'); assert motorEsq.connected
motorDir = ev3.LargeMotor('outB'); assert motorDir.connected
##motorGarra = ev3.MediumMotor('outA'); assert motorGarra.connected
## Sensores de cor
corEsq = ev3.ColorSensor('in1'); assert corEsq.connected
corEsq.mode = 'COL-COLOR'
corDir = ev3.ColorSensor('in4'); assert corDir.connected
corDir.mode = 'COL-COLOR'
corCheck = ev3.ColorSensor('in2'); assert corCheck.connected
corCheck.mode = 'COL-COLOR'
#################################################################
######################### VALORES ###############################
#################################################################
velocidade = -400
delta = 150 # delta de velocidade ao ajeitar caminho
v_curva = -100 # velocidade em curvas
import ev3dev.ev3 as ev3
from threading import Thread
from time import sleep
color_left = ev3.ColorSensor('in2')
color_right = ev3.ColorSensor('in4')
color_left.mode = "COL-COLOR"
color_right.mode = "COL-COLOR"
while True:
print('Left', color_left.value())
print('Right', color_right.value())
sleep(2)
#!/usr/local/bin/python3
import ev3dev.ev3 as ev3 # Import EV3 library
import time # Import time library
# Connect color sensors
colSensorL = ev3.ColorSensor('in4')
assert colSensorL.connected, "Error while connecting left ColorSensor"
colSensorR = ev3.ColorSensor('in1')
assert colSensorR.connected, "Error while connecting right ColorSensor"
ev3.Sound.speak('Sensors connected!').wait()
################################################################################
while True:
print('L: ' + str(colSensorL.reflected_light_intensity) + ' R: ' +
str(colSensorR.reflected_light_intensity))
print('Left Green: ' + str(colSensorL.green) + ' Right Green: ' +
str(colSensorR.green))
print(' ')
time.sleep(0.5)
import ev3dev.ev3 as ev3
import time
motorl = ev3.LargeMotor('outA')
motorl.connected
motorr = ev3.LargeMotor('outD')
motorr.connected
c = ev3.ColorSensor(ev3.INPUT_3)
c.connected
c.mode = 'COL-REFLECT'
#insert PID here
def taskB():
side = 1
line = 0
powerleft = 20
powerright = 20
while line < 3:
while (c.value() <= 20):
motorl.run_direct(duty_cycle_sp = powerleft)
motorr.run_direct(duty_cycle_sp = powerright)
time.sleep(.1)
print(c.value())
motorl.run_direct(duty_cycle_sp = 0)
motorr.run_direct(duty_cycle_sp = 0)
time.sleep(1)
turn(side)
side = side + 1
line = line + 1
#!/usr/bin/env python3
import json
import ev3dev.ev3 as ev3
from common.follower import Follower
from common.line_detectors import OneSensorLineDetector
from common.utils import get_json_from_file
__author__ = 'Xomak'
color_sensor = ev3.ColorSensor(address='in1')
left_motor = ev3.LargeMotor('outB')
right_motor = ev3.LargeMotor('outC')
calibration = get_json_from_file('data/calibration_one_sensor.json')
line_detector = OneSensorLineDetector(color_sensor, calibration)
follower = Follower(right_motor, left_motor, line_detector,
'data/pid_and_speed_1.json')
follower.follow()
#!/usr/local/bin/python3
import ev3dev.ev3 as ev3
import time
# Define Ports
motorL = ev3.LargeMotor('outD')
motorR = ev3.LargeMotor('outA')
cSL = ev3.ColorSensor('in4')
cSR = ev3.ColorSensor('in1')
# Define other variables
btn = ev3.Button()
direction = 1
# Calibrate White
print ("Calibrate White")
while True:
if btn.enter == True:
whiteCal = (cSL.reflected_light_intensity + cSR.reflected_light_intensity)/2 # Calculate average value
print (whiteCal)
break # Exit loop and move on
else:
time.sleep(0.01)
time.sleep(1) # Wait
# Calibrate Black
class LineFollower:
# sensors
ultrasonicSensor = ev3.UltrasonicSensor()
colorSensor = ev3.ColorSensor()
gyroSensor = ev3.GyroSensor()
assert ultrasonicSensor.connected
assert colorSensor.connected
assert gyroSensor.connected
# motors
leftMotor = ev3.LargeMotor('outB')
rightMotor = ev3.LargeMotor('outC')
assert leftMotor.connected
assert rightMotor.connected
# variables
direction = 0 # start direction always NORTH
x = 0
y = 0
offset = 170 #141
integral = 0
lastError = 0
derivative = 0
listDistances = []
listPaths = []
blocked = False
red = (135, 60, 15)
blue = (30, 150, 100)
# get listDistances
def get_distances(self):
return self.listDistances
# set & get direction
def set_direction(self, direction):
self.direction = direction
def get_direction(self):
return self.direction
def get_pathstatus(self):
return self.blocked
# calculate new offset value
def calibrate(self):
self.colorSensor.mode = 'RGB-RAW'
valueWhite = 0
valueBlack = 0
for i in range(3):
inp = input("White: ")
color = self.colorSensor.bin_data('hhh')
valueWhite += (color[0] + color[1] + color[2])/3
print(f"white: {valueWhite}")
for i in range(3):
inp = input("Black: ")
color = self.colorSensor.bin_data('hhh')
valueWhite += (color[0] + color[1] + color[2])/3
print(f"black: {valueBlack}")
valueWhite /= 3
valueBlack /= 3
self.offset = (valueBlack + valueWhite)/2
print(f"offset: {self.offset}")
def make_sound(self):
ev3.Sound.beep()
# turn
def turn(self, deg, direction):
self.gyroSensor.mode = 'GYRO-RATE'
self.gyroSensor.mode = 'GYRO-ANG'
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
if direction == "right":
while self.gyroSensor.value() < deg:
self.leftMotor.duty_cycle_sp = 20
self.rightMotor.duty_cycle_sp = -20
else:
while abs(self.gyroSensor.value()) < deg:
self.leftMotor.duty_cycle_sp = -20
self.rightMotor.duty_cycle_sp = 20
self.leftMotor.stop()
self.rightMotor.stop()
# adjustment for moving along path given by Dijkstra
def move_for_dijkstra(self):
self.gyroSensor.mode = 'GYRO-RATE'
self.gyroSensor.mode = 'GYRO-ANG'
self.rightMotor.run_to_rel_pos(position_sp=100, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=100, speed_sp=150)
time.sleep(1.5)
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
while abs(self.gyroSensor.value()) < 40:
self.rightMotor.run_to_rel_pos(position_sp=-20, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=20, speed_sp=150)
self.leftMotor.stop()
self.rightMotor.stop()
# obstacle detection
def obstacle(self):
self.ultrasonicSensor.mode = 'US-DIST-CM'
dist = self.ultrasonicSensor.value() // 10
if dist < 14:
ev3.Sound.beep()
self.blocked = True
self.lastError = 0
self.integral = 0
self.derivative = 0
self.turn(90, "right")
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
self.leftMotor.duty_cycle_sp = 20
self.rightMotor.duty_cycle_sp = 20
time.sleep(0.5)
while self.colorSensor.value() not in range(30, 44):
self.leftMotor.duty_cycle_sp = 20
self.rightMotor.duty_cycle_sp = -20
self.leftMotor.stop()
self.rightMotor.stop()
self.leftMotor.duty_cycle_sp = 0
self.rightMotor.duty_cycle_sp = 0
print("Found blocked path")
# vertex detection
def vertex(self):
self.colorSensor.mode = 'RGB-RAW'
color = self.colorSensor.bin_data('hhh')
if (color[0] in range(self.red[0]-30, self.red[0]+30)) and (color[1] in range(self.red[1]-30, self.red[1]+30)) \
and (color[2] in range(self.red[2]-30, self.red[2]+30)):
return True
elif (color[0] in range(self.blue[0]-30, self.blue[0]+30)) and (color[1] in range(self.blue[1]-30, self.blue[1]+30)) \
and (color[2] in range(self.blue[2]-30, self.blue[2]+30)):
return True
else:
return False
# vertex exploration
def explore(self, direction):
self.listPaths.clear()
self.listPaths.append((direction + 180) % 360)
self.rightMotor.run_to_rel_pos(position_sp=100, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=100, speed_sp=150)
time.sleep(1.5)
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
self.gyroSensor.mode = 'GYRO-RATE'
self.gyroSensor.mode = 'GYRO-ANG'
self.colorSensor.mode = 'RGB-RAW'
t90 = False
t270 = False
t360 = False
while abs(self.gyroSensor.value()) < 400:
self.rightMotor.run_to_rel_pos(position_sp=-10, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=10, speed_sp=150)
color = self.colorSensor.bin_data('hhh')
if self.gyroSensor.value() in range(70, 110) and int((color[0] + color[1] + color[2]) / 3) < 50 and not t90:
#print(f"path, direction: {(direction + 90) % 360}")
self.listPaths.append((direction + 90) % 360)
t90 = True
if self.gyroSensor.value() in range(250, 290) and int((color[0] + color[1] + color[2]) / 3) < 50 and not t270:
#print(f"path, direction: {(direction - 90) % 360}")
self.listPaths.append((direction - 90) % 360)
t270 = True
if self.gyroSensor.value() in range(340, 400) and int((color[0] + color[1] + color[2]) / 3) < 50 and not t360:
#print(f"path, direction: {direction}")
self.listPaths.append(direction)
t360 = True
print(f"Directions: {self.listPaths}")
self.leftMotor.stop()
self.rightMotor.stop()
self.leftMotor.stop()
self.rightMotor.stop()
#select path
def select_path(self, direction):
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
self.gyroSensor.mode = 'GYRO-RATE'
self.gyroSensor.mode = 'GYRO-ANG'
if direction == self.direction:
while abs(self.gyroSensor.value()) < 70:
self.rightMotor.run_to_rel_pos(position_sp=10, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=-10, speed_sp=150)
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
while color not in range(self.offset - 30, self.offset + 30):
self.rightMotor.duty_cycle_sp = -15
self.leftMotor.duty_cycle_sp = 15
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
self.rightMotor.stop()
self.leftMotor.stop()
elif direction == (self.direction + 90) % 360:
while abs(self.gyroSensor.value()) < 10:
self.rightMotor.run_to_rel_pos(position_sp=-10, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=10, speed_sp=150)
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
while color not in range(self.offset - 30, self.offset + 30):
self.rightMotor.duty_cycle_sp = -15
self.leftMotor.duty_cycle_sp = 15
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
self.rightMotor.stop()
self.leftMotor.stop()
elif direction == (self.direction - 90) % 360:
while abs(self.gyroSensor.value()) < 160:
self.rightMotor.run_to_rel_pos(position_sp=10, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=-10, speed_sp=150)
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
while color not in range(self.offset - 30, self.offset + 30):
self.rightMotor.duty_cycle_sp = -15
self.leftMotor.duty_cycle_sp = 15
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
self.rightMotor.stop()
self.leftMotor.stop()
elif direction == (self.direction + 180) % 360:
while abs(self.gyroSensor.value()) < 100:
self.rightMotor.run_to_rel_pos(position_sp=-10, speed_sp=150)
self.leftMotor.run_to_rel_pos(position_sp=10, speed_sp=150)
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
while color not in range(self.offset - 30, self.offset + 30):
self.rightMotor.duty_cycle_sp = -15
self.leftMotor.duty_cycle_sp = 15
color = self.colorSensor.bin_data('hhh')
color = int((color[0] + color[1] + color[2]) / 3)
self.leftMotor.stop()
self.rightMotor.stop()
# follow line
def drive(self, p=11, i=0.8, d=8, v=35):
kp = p # 8
ki = i # 1
kd = d # 4
tp = v # 20
self.blocked = False
positionLeft = self.leftMotor.position
positionRight = self.rightMotor.position
self.gyroSensor.mode = 'GYRO-RATE'
self.gyroSensor.mode = 'GYRO-ANG'
self.listDistances.clear()
while not self.vertex():
self.leftMotor.command = 'run-direct'
self.rightMotor.command = 'run-direct'
self.colorSensor.mode = 'RGB-RAW'
color = self.colorSensor.bin_data('hhh')
lightValue = int((color[0]+color[1]+color[2])/3)
error = lightValue - self.offset
self.integral += error
self.derivative = error - self.lastError
turn = (kp * error) + (ki * self.integral) + (kd * self.derivative)
turn /= 100
powerLeft = tp + turn
powerRight = tp - turn
if powerLeft > 100:
powerLeft = 100
elif powerLeft < -100:
powerLeft = -100
if powerRight > 100:
powerRight = 100
elif powerRight < -100:
powerRight = -100
self.leftMotor.duty_cycle_sp = powerLeft
self.rightMotor.duty_cycle_sp = powerRight
self.lastError = error
dl = 0.048 * (self.leftMotor.position - positionLeft)
dr = 0.048 * (self.rightMotor.position - positionRight)
positionLeft = self.leftMotor.position
positionRight = self.rightMotor.position
self.listDistances.append([dl, dr])
self.obstacle()
self.leftMotor.stop()
self.rightMotor.stop()
self.leftMotor.stop()
self.rightMotor.stop()
def waitformotor(motor):
#run more than once to ensure that motor is stopped and that it was not a false reading
while motor.state == [u'running'] or motor.state == [u'ramping']:
xxx = 0
while motor.state == [u'running'] or motor.state == [u'ramping']:
xxx = 0
while motor.state == [u'running'] or motor.state == [u'ramping']:
xxx = 0
while motor.state == [u'running'] or motor.state == [u'ramping']:
xxx = 0
# define motors and use brake mode
col = ev3.ColorSensor()
paper = ev3.MediumMotor('outA')
pen = ev3.LargeMotor('outB')
LR = ev3.MediumMotor('outC')
pen.stop_command = u"brake"
LR.stop_command = u"brake"
paper.stop_command = u"brake"
LR.ramp_up_sp = 100
LR.ramp_down_sp = 200
LR.reset()
LR.run_to_abs_pos(position_sp=-50, duty_cycle_sp=75)
waitformotor(LR)
waitformotor(LR)
LR.reset()
LR.speed_regulation_enabled = u'on'
#----------------------------------------------------------------------------------------------------------------------
#Initial setup of sensors and motors
#Attach motors mA is left, mb right
mA = ev3.LargeMotor('outA')
mB = ev3.LargeMotor('outB')
mB.run_direct()
mA.run_direct()
#Set motor direction
mA.polarity = "normal"
mB.polarity = "normal"
#Attach sensors
colorSensorLeft = ev3.ColorSensor('in1')
colorSensorRight = ev3.ColorSensor('in2')
lightSensorFront = ev3.LightSensor('in3')
#Check sensors if they are connected, will throw an error message if not
assert colorSensorLeft.connected, "ColorSensorLeft (CS) is not connected to in1"
assert colorSensorRight.connected, "ColorSensorLeft (CS) is not connected to in2"
assert lightSensorFront.connected, "LightSensoFront (LS) is not connected to in3"
#Put the color sensor into COL-REFLECT mode to measure reflected light intensity.
colorSensorLeft.mode='COL-REFLECT'
colorSensorRight.mode='COL-REFLECT'
#For shutdown, stop motors
def signal_handler(sig, frame):
print('Shutting down gracefully')
Stops()
exit(0)
import ev3dev.ev3 as ev3
import time
mr = ev3.LargeMotor('outB') # right motor
ml = ev3.LargeMotor('outC') # left motor
cs = ev3.ColorSensor('in3') # colour sensor
def move_forward():
mr.run_direct(duty_cycle_sp=100)
ml.run_direct(duty_cycle_sp=100)
def move_backward():
mr.run_direct(duty_cycle_sp=-100)
ml.run_direct(duty_cycle_sp=-100)
def turn_right():
mr.run_direct(duty_cycle_sp=-100)
ml.run_direct(duty_cycle_sp=100)
def turn_left():
mr.run_direct(duty_cycle_sp=100)
ml.run_direct(duty_cycle_sp=-100)
def stop():
mr.stop()
ml.stop()
value = maxInput if value > maxInput else value
value = minInput if value < minInput else value
inputSpan = maxInput - minInput
outputSpan = maxOutput - minOutput
scaledThrust = float(value - minInput) / float(inputSpan)
return minOutput + (scaledThrust * outputSpan)
# Create ultrasonic sensor entity,
# 'in1' is the port sensor is connected.
s = ev3.ColorSensor('in2')
# Configure sensor mode
# 'US-DIST-CM' means that Continuous measurement in centimeters.
s.mode = 'COL-REFLECT'
# Create motor entity
# 'outA' is the port Medium motor is connected.
m = ev3.MediumMotor('outD')
KP = 1
KI = 1
KD = 1
import ev3dev.ev3 as ev3
from time import sleep
import signal
mA = ev3.LargeMotor('outA')
mB = ev3.LargeMotor('outB')
THRESHOLD_LEFT = 30
THRESHOLD_RIGHT = 350
BASE_SPEED = 30
TURN_SPEED = 80
lightSensorLeft = ev3.ColorSensor('in1')
lightSensorRight = ev3.LightSensor('in2')
assert lightSensorLeft.connected, "LightSensorLeft(ColorSensor) is not connected"
assert lightSensorRight.connected, "LightSensorRight(LightSensor) is not conected"
mB.run_direct()
mA.run_direct()
mA.polarity = "inversed"
mB.polarity = "inversed"
def signal_handler(sig, frame):
print('Shutting down gracefully')
mA.duty_cycle_sp = 0