def __init__(self,
init_pose,
soc=None,
port_motor_rear=OUTPUT_A,
port_motor_steer=OUTPUT_B,
port_sensor_gyro='in1'):
# 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.__velocity_controller = VelocityController(self,
0,
0,
adaptation=False)
self.s = soc
# NOTE: possible using other controllers. Change only here!
self.__trajectory_controller = ControllerWithLinearization()
self.__path_controller = PathController()
self.__localization = Localization(self, init_pose)
self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega
self.reset()
Sound.speak('Ready').wait()
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,
motor1Port,
motor2Port,
gyroPort=None,
wheelDiameter=None): #init
if GyroSensor != None:
self.GS = GyroSensor(gyroPort)
else:
self.GS = GyroSensor()
self.M1 = LargeMotor(motor1Port)
self.M2 = LargeMotor(motor2Port)
self.motor_stop = True
self.wheelDiameter = wheelDiameter
self.time = 0
self.MDistanceRunning = True
self.distance = 0
self.pauseDistance = []
def run(fun):
# Connect two motors and two (different) light sensors
mL = LargeMotor('outC')
mR = LargeMotor('outB')
sL = ColorSensor('in1')
sR = ColorSensor('in4')
gy = GyroSensor('in3')
# Check if the sensors are connected
assert sL.connected, "Left ColorSensor is not connected"
assert sR.connected, "Right ColorSensor is not conected"
assert gy.connected, "Gyro is not connected"
gyro = Gyro()
light_sensors = LightSensors()
encoder = Encoder()
# Set the motor mode
mL.polarity = "normal" # "inversed"
mR.polarity = "normal" # "inversed"
mL.position = 0
mR.position = 0
def stop_motors():
mL.run_direct()
mR.run_direct()
mL.duty_cycle_sp = 0
mR.duty_cycle_sp = 0
stop_motors()
# The example doesn't end on its own.
# Use CTRL-C to exit it (needs command line).
# This is a generic way to be informed
# of this event and then take action.
def signal_handler(sig, frame):
stop_motors()
print('Shut down gracefully')
exit(0)
# Install the signal handler for CTRL+C
signal(SIGINT, signal_handler)
print('Press Ctrl+C to exit')
per = {
'mL': mL,
'mR': mR,
'sL': sL,
'sR': sR,
'gy': gy
}
light_sensors.init(per)
gyro.init(gy)
# Endless loop reading sensors and controlling motors.
last_log = time()
last_now = time()
max_dt = 0
dts = 0
speed_mL = None
speed_mR = None
while True:
state = {}
state = light_sensors(per, state)
state = encoder(per, state)
state = gyro(per, state)
state = fun(per, state)
max_speed = 40 * TICKS_PER_CM
_speed_mL = state.get('mL', 0)
if _speed_mL != speed_mL:
speed_mL = _speed_mL
mL.run_forever(speed_sp=speed_mL/100 * max_speed)
_speed_mR = state.get('mR', 0)
if _speed_mR != speed_mR:
speed_mR = _speed_mR
mR.run_forever(speed_sp=speed_mR/100 * max_speed)
dts += 1
now = time()
dt = now - last_now
last_now = now
if dt > max_dt: max_dt = dt
if now - last_log > 5.0:
last_log = now
print("avg fps: ", dts / 5.0)
print('min fps: ', 1 / max_dt)
max_dt = 0
dts = 0
from ev3dev.ev3 import GyroSensor
import time
gy = GyroSensor('in1')
gy.mode = 'GYRO-FAS'
time.sleep(0.1)
def intstr(val):
return (' ' * 5 + str(val))[-5:]
startvalue = gy.value()
lasttime = time.time()
angle = 0
while True:
v = gy.value() - startvalue
now = time.time()
dt = now - lasttime
lasttime = now
angle += dt * v
print("gyro: " + str(angle) + " " * 10, end='\r')
time.sleep(0.01)
from os import system
import paho.mqtt.client as mqtt
# Motores
M_PORTA = LargeMotor("outC")
M_ESQUERDO = LargeMotor("outA")
M_DIREITO = LargeMotor("outB")
# Sensores infravermelhos
PROX1 = InfraredSensor("in4")
PROX2 = InfraredSensor("in3")
PROX1.mode = "IR-PROX"
PROX2.mode = "IR-PROX"
# Giroscopio
GYRO = GyroSensor("in1")
GYRO.mode = "GYRO-ANG"
# Sensor de cor
COLOR = ColorSensor("in2")
COLOR.mode = "COL-COLOR"
# Variaveis usadas durante o programa
# Constantes
TEMPO_CURVA_90 = 1700
TEMPO_CURVA_180 = 3400
ANGULO_CURVA_180 = 180
ANGULO_CURVA_90 = 90
ANGULO_DESVIO = 9
VELOCIDADE_CURVA = 400
VELOCIDADE_ATUAL = 350
from ev3dev.ev3 import LargeMotor, Sound, \
GyroSensor, OUTPUT_A, INPUT_1
from time import sleep, time
from math import sin, pi
N = 1000
motor = LargeMotor(OUTPUT_A)
gyro = GyroSensor(INPUT_1)
Sound().beep()
fout = open("data.txt", "w")
sleep(1)
start_time = time()
for i in range(0, N):
t = (time() - start_time) * 1000
rotation = gyro.value() + 4
u_float = 100 * sin(pi * t / N)
motor.run_forever(speed_sp=u_float * 10) #should use speed_sp!!!
s = "%d\t%d\t%d" % (t, rotation, u_float)
fout.write(s)
sleep(0.004)
fout.close()
#!/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'
class TrueTurn:
def __init__(self,
motor1Port,
motor2Port,
gyroPort=None,
wheelDiameter=None): #init
if GyroSensor != None:
self.GS = GyroSensor(gyroPort)
else:
self.GS = GyroSensor()
self.M1 = LargeMotor(motor1Port)
self.M2 = LargeMotor(motor2Port)
self.motor_stop = True
self.wheelDiameter = wheelDiameter
self.time = 0
self.MDistanceRunning = True
self.distance = 0
self.pauseDistance = []
def turn(self, degrees, speed=150, tolerance=0.05):
self.resetValue()
self.stopMotors()
self.tolerance = tolerance
self.speed = speed
multiplier = -1
if degrees > 0:
multiplier = 1
self.resetValue()
angle = self.GS.value()
running = False
self.breaker = False
rightTurn = False # not actually right
leftTurn = False # not actually left
slowRightTurn = False # not actually right
slowLeftTurn = False # not actually left
if tolerance > 0:
field = range(math.ceil(degrees - self.tolerance * degrees),
math.ceil(degrees + self.tolerance * degrees),
multiplier)
advancedField = range(math.ceil(degrees - 0.1 * degrees),
math.ceil(degrees + 0.1 * degrees),
multiplier)
print(advancedField)
else:
field = [self.tolerance]
advancedField = range(math.ceil(degrees - 0.1 * degrees),
math.ceil(degrees + 0.1 * degrees),
multiplier)
print(advancedField)
while self.GS.value() - angle not in field:
print(advancedField)
print(self.GS.value() - angle)
print(abs(self.GS.value() - angle))
if self.GS.value() - angle in advancedField:
print("minor")
print(self.GS.value())
if abs(self.GS.value() - angle) < abs(
field[0]
): #we have to make them absolute because we want to turn on both sides
if not slowRightTurn:
print("slow right")
self.M1.run_forever(speed_sp=self.speed * multiplier /
2.5)
self.M2.run_forever(speed_sp=self.speed * multiplier *
-1 / 2.5)
slowRightTurn = True
slowLeftTurn = False
sleep(0.001)
if abs(self.GS.value() - angle) > abs(
field[len(field) - 1]
): #we have to make them absolute because we want to turn on both sides
if not leftTurn:
print("slow right")
self.M1.run_forever(speed_sp=self.speed * multiplier *
-1 / 2)
self.M2.run_forever(speed_sp=self.speed * multiplier /
2)
slowRightTurn = False
slowLeftTurn = True
sleep(0.001)
else:
if abs(self.GS.value() - angle) < abs(
field[0]
): #we have to make them absolute because we want to turn on both sides
if not rightTurn:
print("normal")
print(self.GS.value())
self.M1.run_forever(speed_sp=self.speed * multiplier)
self.M2.run_forever(speed_sp=self.speed * multiplier *
-1)
rightTurn = True
leftTurn = False
else:
sleep(0.0012)
if abs(self.GS.value() - angle) > abs(
field[len(field) - 1]
): #we have to make them absolute because we want to turn on both sides
if not leftTurn:
print(self.GS.value())
print("normal left")
self.M1.run_forever(speed_sp=self.speed * multiplier *
-1)
self.M2.run_forever(speed_sp=self.speed * multiplier)
rightTurn = False
leftTurn = True
else:
sleep(0.0012)
self.M1.stop()
self.M2.stop()
sleep(0.1)
print("ok it works")
leftTurn = False
rightTurn = False
slowLeftTurn = False
slowRightTurn = False
if self.GS.value() - angle not in field:
while self.GS.value() - angle not in field:
if abs(self.GS.value() - angle) < abs(
field[0]
): #we have to make them absolute because we won to turn on both sides
if not rightTurn:
print(self.GS.value() - angle)
print("micro")
self.M1.run_forever(speed_sp=self.speed * multiplier /
5)
self.M2.run_forever(speed_sp=self.speed * multiplier *
-1 / 5)
rightTurn = True
leftTurn = False
sleep(0.001)
if abs(self.GS.value() - angle) > abs(
field[len(field) - 1]
): #we have to make them absolute because we won to turn on both sides
if not leftTurn:
print(self.GS.value() - angle)
print("working")
self.M1.run_forever(speed_sp=self.speed * multiplier *
-1 / 5)
self.M2.run_forever(speed_sp=self.speed * multiplier /
5)
rightTurn = False
leftTurn = True
sleep(0.001)
self.M1.stop()
self.M2.stop()
self.resetValue()
return True
def straight(self, direction, speed, tolerance):
self.stopMotors()
self.resetValue()
angle = self.GS.value()
multiplier = 1
if angle < 0:
multiplier = -1
self.motor_stop = False
def inField(field, thing):
succes = 0
j = 0
for i in field:
if j == 0:
if i < thing:
succes = 2
break
if j == len(field) - 1:
if i > thing:
succes = 3
break
if thing == i:
succes = 1
break
j = j + 1
return succes
field = range(angle - tolerance, angle + tolerance)
while self.motor_stop == False:
self.M1.run_forever(speed_sp=speed * direction)
self.M2.run_forever(speed_sp=speed * direction)
sleep(0.2)
value = self.GS.value()
if inField(field, value) == 2:
print("compesating 2")
self.M1.run_forever(speed_sp=speed - 50 * direction)
while self.GS.value() not in field:
sleep(0.02)
print(self.GS.value())
self.M1.run_forever(speed_sp=speed * direction)
self.M2.run_forever(speed_sp=speed * direction)
elif inField(field, value) == 3:
print("compesating 3")
self.M2.run_forever(speed_sp=speed - 50 * direction)
while self.GS.value() not in field:
print(self.GS.value())
sleep(0.02)
self.M2.run_forever(speed_sp=speed * direction)
self.M1.run_forever(speed_sp=speed * direction)
if self.motor_stop is True:
self.stopMotors()
def measureDistanceStart(self):
self.distance = self.M1.position
# ~ self.MDistanceRunning = True
def measureDistance(self, wheelDiameter=5.5):
turns = (self.M1.position - self.distance) / 360
dist = turns * wheelDiameter * math.pi
return dist
def measureDistanceRunning(self):
return self.MDistanceRunning
def stopMotors(self):
self.motor_stop = True
self.M2.stop()
self.M1.stop()
self.resetValue()
def resetValue(self):
self.GS.mode = 'GYRO-RATE'
self.GS.mode = 'GYRO-ANG'
def isRunning(self):
return not self.motor_stop
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. """
MIN_REFL = 1
MAX_REFL = 60
""" Target value of reflectance for staying on line. """
#!/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):
def main():
data = None
if True:
with open('zemljevid.json') as f:
data = json.load(f)
else:
resource = urllib.request.urlopen(
'http://192.168.0.200:8080/zemljevid.json')
content = resource.read()
content = content.decode("utf-8")
data = json.loads(content)
os.system('setfont Lat15-TerminusBold14')
if os.path.exists("logs"):
import shutil
shutil.rmtree("logs")
os.mkdir("logs")
mL = LargeMotor('outB')
mL.stop_action = 'hold'
mR = LargeMotor('outC')
mR.stop_action = 'hold'
global cl
cl = ColorSensor()
cl.mode = 'COL-COLOR'
gy = GyroSensor()
gy.mode = 'GYRO-RATE'
gy.mode = 'GYRO-ANG'
# Give gyro a bit of time to start
time.sleep(3)
global start_time
start_time = time.time()
debug_print("Start time: ", start_time)
start = [0, 0]
locations = []
for key, item in data.items():
if key == "start":
start = item
else:
locations.append(item)
# Sort by distance, TODO might be better to minimize turns by prioritizing victims that are in the same line
locations = sorted(
locations,
key=lambda x: abs(start[0] - x[0]) + abs(start[1] - x[1]),
reverse=False)
current_location = start
def reset_neighbourhood_search():
global searching_neighbourhood
nonlocal neighbourhood_locations
searching_neighbourhood = False
neighbourhood_locations = []
global searching_neighbourhood
searching_neighbourhood = False
neighbourhood_locations = []
while locations:
next_location = locations.pop(0)
go_to_location(x=next_location[0],
y=next_location[1],
current_x=current_location[0],
current_y=current_location[1],
mL=mL,
mR=mR,
gyro=gy)
current_location = next_location
color = cl.value()
if color == 1: # BLACK: START, 2 second beep
debug_print("Color sensor: START")
beep(1, 2000)
reset_neighbourhood_search()
locations = sorted(
locations,
key=lambda x: abs(start[0] - x[0]) + abs(start[1] - x[1]),
reverse=False)
elif color == 2: # BLUE: good condition, 1 beep
debug_print("Color sensor: BLUE")
beep(1)
reset_neighbourhood_search()
locations.insert(0, start)
elif color == 4: # YELLOW: critical condition, 2 beeps
debug_print("Color sensor: YELLOW")
beep(2)
reset_neighbourhood_search()
locations.insert(0, start)
elif color == 5: # RED: passed away, 3 beeps
debug_print("Color sensor: RED")
beep(3)
reset_neighbourhood_search()
#locations.insert(0, start)
locations = sorted(locations,
key=lambda x: abs(current_location[0] - x[0]) +
abs(current_location[1] - x[1]),
reverse=False)
else:
debug_print("Color sensor: UNKNOWN (" + str(color) + ")")
#locations.insert(0, start)
if not searching_neighbourhood:
searching_neighbourhood = True
radius = 5
for area in range(1, 20):
neighbourhood_locations.append([
next_location[0] + radius * area,
next_location[1] - radius * area
])
neighbourhood_locations.append([
next_location[0] + radius * area,
next_location[1] + radius * area
])
neighbourhood_locations.append([
next_location[0] - radius * area,
next_location[1] + radius * area
])
neighbourhood_locations.append([
next_location[0] - radius * area,
next_location[1] - radius * area
])
locations.insert(0, neighbourhood_locations[0])
neighbourhood_locations = neighbourhood_locations[1:]
# # Rotate back to original orientation
# angle = calculate_angle(0, gy.value())
# rotate_to_angle(angle, mL, mR, gy)
# for i in range (16):
# rotate_to_angle(90, mL, mR, gy)
# rotate_to_angle(0, mL, mR, gy)
#for _ in range (5):
#rotate_to_angle(89, mL, mR, gy)
#rotate_to_angle(178, mL, mR, gy)
#rotate_to_angle(270, mL, mR, gy)
#rotate_to_angle(180, mL, mR, gy)
#rotate_to_angle(90, mL, mR, gy)
#rotate_to_angle(0, mL, mR, gy)
#rotate_to_angle(-91, mL, mR, gy)
#rotate_to_angle(-182, mL, mR, gy)
#rotate_to_angle(-2, mL, mR, gy)
#rotate_to_angle(89, mL, mR, gy)
#rotate_to_angle(178, mL, mR, gy)
#rotate_to_angle(-2, mL, mR, gy)
#rotate_to_angle(87, mL, mR, gy)
#rotate_to_angle(176, mL, mR, gy)
# for i in range (5):
# drive_for_centimeters(50, mL, mR, gy, 0)
# rotate_to_angle(0, mL, mR, gy)
# drive_for_centimeters(-50, mL, mR, gy, 0)
# rotate_to_angle(0, mL, mR, gy)
debug_print("End time: ", time.time())
#!/usr/bin/env python
from ev3dev.ev3 import LargeMotor, GyroSensor, \
OUTPUT_A, INPUT_1, Sound, PowerSupply
from time import time, sleep
from math import pi, copysign
Kp = 15
pwr = 0
motor = LargeMotor(OUTPUT_A)
gyro = GyroSensor(INPUT_1)
battary = PowerSupply()
Sound().beep()
sleep(0.05)
motor.reset()
gyro.mode = gyro.modes[
0] #при выборе режима работы датчика значение сбрасывается в ноль
offset = gyro.value()
a = 0
start_time = time()
while True:
t = time() - start_time
rotation = motor.position
angle = gyro.value() - offset
a = a + angle * 0.001
print("%d\t%d" % (angle, rotation))
e = (90 - rotation) * pi / 180
#!/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()