def TEST2():
brickButton = Button()
rfidReader = RfidSerialThread("COM5" if (platform.system() == "Windows") else "/dev/ttyUSB0")
print("FOLLOWING LINES, DETECTING TAGS:")
rfidReader.start()
sleep(4)
robot.resetOrientation()
while not brickButton.any():
robot.followLine(200)
if rfidReader.getCurrentTagId() is not None:
robot.stopMotors()
print("TAG FOUND - id", rfidReader.getCurrentTagId())
Sound.beep()
sleep(1)
robot.runMotorsDistance(-5.7, 150)
sleep(4)
robot.runMotorsDistance(12.0, 150) #to go past the tag
if (robot.getDistanceAhead() is not None and robot.getDistanceAhead() < 20):
print("Obstacle! Dist =", robot.getDistanceAhead(), "Exiting...")
break
robot.stopMotors()
rfidReader.stop()
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 Main():
map_var = Map(Ghost_mapping_type.none)
io = IO()
logic = Logic(map_var)
button = Button()
saver = Map_saver(map_var, button)
saver.wait_for_load()
print("READY")
motors = {
Moves.fwd: lambda: io.go_forward(),
Moves.left: lambda: io.go_left(),
Moves.right: lambda: io.go_right(),
Moves.back: lambda: io.go_back()
}
mapping = {
Moves.fwd: lambda: map_var.go_forward(),
Moves.left: lambda: map_var.go_left(),
Moves.right: lambda: map_var.go_right(),
Moves.back: lambda: map_var.go_back()
}
motors[Moves.fwd]()
mapping[Moves.fwd]()
while (True):
clr_sensor = io.directions_free()
us_sensor = io.ghost_distance()
map_var.write_sensor_values(clr_sensor, us_sensor)
debug_print(map_var)
move = logic.get_next_move()
debug_print(map_var)
switch[move]()
if button.any():
saver.wait_for_load()
ok = motors[move]()
if (ok is False):
io.after_crash()
map_var.rotation += int(move)
continue
elif (ok is None):
saver.wait_for_load()
continue
mapping[move]()
saver.save_map()
def __init__(self):
# Large motors
self.lm_left_port = "outB"
self.lm_right_port = "outA"
self.lm_left = LargeMotor(self.lm_left_port)
self.lm_right = LargeMotor(self.lm_right_port)
# distance at which sensor motor start moving
self.move_sensor_check_degrees = 400
self.move_degrees = 570 # one tile distance
self.move_speed = 35
self.after_crash_degrees = 200
self.steering_turn_speed = 30 # turning left or right
self.steering_turn_degrees = 450
self.steering_turn_fwd_degrees = 150 # distance to move after turning
# distance at which sensors start spinning
self.steering_sensor_check_degrees = 50
self.btn = Button()
# small motor
self.sm_port = "outC"
self.sm = Motor(self.sm_port)
self.sm_turn_speed = 30
self.sm_center_turn_angle = 90
self.sm_side_turn_angle = 110
self.sm_is_left = False
# color sensor
self.color_sensor_port = "in1"
self.color_sensor = ColorSensor(self.color_sensor_port)
self.color_sensor.mode = ColorSensor.MODE_COL_REFLECT
self.color_sensor_values = []
# regulations
self.regulation_desired_value = 4
self.regulation_max_diff = 3
self.regulation_p = 1.5
self.regulation_tick = 0.03
# ultrasonic sensor
self.ultrasonic_sensor_port = "in4"
self.ultrasonic_sensor = UltrasonicSensor(self.ultrasonic_sensor_port)
self.ultrasonic_sensor.mode = 'US-DIST-CM'
self.ultrasonic_tile_length = 30
self.ultrasonic_max_value = 255
self.ultrasonic_sensor_values = []
# touch sensors
self.touch_right = TouchSensor("in2")
self.touch_left = TouchSensor("in3")
def menu():
botao = Button()
print("Seta esquerda para calibrar <- | Seta direita para rodar ->")
while True:
if botao.left:
system("clear")
calibrar(botao)
break
elif botao.right:
system("clear")
dados = lerDados()
run(11, 0.5, 0.3, -260, dados)
break
menu()
def main():
botao = Button()
print("---VERDE---")
while True:
if botao.enter:
calibrar("verde.csv")
break
sleep(1)
system("clear")
print("---VERMELHO---")
while True:
if botao.enter:
calibrar("vermelho.csv")
break
sleep(1)
system("clear")
print("---AZUL---")
while True:
if botao.enter:
calibrar("azul.csv")
break
sleep(1)
system("clear")
print("---PRETO---")
while True:
if botao.enter:
calibrar("preto.csv")
break
sleep(1)
system("clear")
print("---BRANCO---")
while True:
if botao.enter:
calibrar("branco.csv")
break
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
jaw_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=ir_beacon_channel)
self.jaw_motor = MediumMotor(address=jaw_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.button = Button()
self.speaker = Sound()
print('KONEC')
motor_left.stop(stop_action='brake')
motor_right.stop(stop_action='brake')
Sound.play_song((
('D4', 'e'),
('C4', 'e'),
('A3', 'h')))
sys.exit(0)
# -----------------------------------------------------------------------------
# NASTAVITVE TIPAL, MOTORJEV IN POVEZAVE S STREŽNIKOM
# -----------------------------------------------------------------------------
# Nastavimo tipala in gumbe.
print('Priprava tipal ... ', end='', flush=True)
btn = Button()
#sensor_touch = init_sensor_touch()
print('OK!')
# Nastavimo velika motorja. Priklopljena naj bosta na izhoda A in D.
print('Priprava motorjev ... ', end='')
motor_left = init_large_motor(MOTOR_LEFT_PORT)
motor_right = init_large_motor(MOTOR_RIGHT_PORT)
print('OK!')
# Nastavimo povezavo s strežnikom.
url = SERVER_IP+'/'+GAME_STATE_FILE
print('Vspostavljanje povezave z naslovom ' + url + ' ... ', end='', flush=True)
conn = Connection(url)
print('OK!')
assert seeker.connected, "Seeker not connected to IN1"
seeker.mode = 'AC-ALL'
compass = Sensor(address='in2:i2c1', driver_name='ht-nxt-compass')
assert compass.connected, "Compass not connected to IN2"
compass.mode = 'COMPASS'
light = Sensor(address='in3', driver_name='lego-nxt-light')
assert light.connected, "Light not connected to IN3"
light.mode = 'REFLECT'
ballLight = Sensor(address='in4', driver_name='lego-nxt-light')
assert ballLight.connected, "Light not connected to IN4"
ballLight.mode = 'REFLECT'
button = Button()
##################################################################
##################################################################
##################################################################
file = open('test.txt', 'r')
calibration = file.readlines()
file.close()
North = int(calibration[0])
green = int(calibration[1])
black = int(calibration[2])
far = float(calibration[3])
near = float(calibration[4])
transition = (green + black) / 2
lista_valores = [0, 0, 0] # indice 0 - VERMELHO, indice 1 - VERDE, indice 2 - AZUL
cores_voltando = [0, 0, 0] # Quant de cores na volta
cores_indo = [1, 1, 1] # Quant de cores na ida
cor_caminho = ["", ""]
cores = ["VERMELHO", "AZUL", "VERDE"]
relacao_cores = {"VERMELHO": "", "VERDE": "", "AZUL": ""}
flag_parar = False
final = False
sentido = "INDO"
cor_atual1 = ColorSensor.COLOR_WHITE
cor_atual2 = ColorSensor.COLOR_WHITE
# Objetos
funcao_motores = Motores(M_ESQUERDO, M_DIREITO, M_PORTA)
pid = PID(KP, KI, KD)
botao = Button()
client = mqtt.Client()
client.connect("169.254.96.68", 1883, 60)
def on_connect(client, userdata, flags, rc):
client.subscribe([("topic/sensor/color1", 0), ("topic/sensor/color2", 0)])
def on_disconnect(client, userdata, rc=0):
client.loop_stop()
def on_message(client, userdata, msg):
global cor_atual1, cor_atual2
from ev3dev.ev3 import Button
from time import sleep
from math import pi
leftMotor = ev3.Motor("outD")
rightMotor = ev3.Motor("outA")
clawMotor = ev3.Motor("outB")
leftSensor = ev3.ColourSensor("in1")
rightSensor = ev3.ColourSensor("in4")
SILVER = 0
ultrasonicSensor = ev3.UltrasonicSensor("in2")
Buttons = Button()
BASE_POWER = 60
CALIBRATED_RANGE = 10 # The margin of error that is used when checking for colour
WHEEL_CIRCUMFERENCE = 13.573
TURNING_DIAMETRE = 10.5
WATER_TOWER_DETECT_DISTANCE = 8
WATER_TOWER_DETECT_ANGLE = 40
WATER_TOWER_DETECT_ANGLED_DISTANCE = 10.44 # 8 / cos(40)
def withinrange(x, y, range):
return (x - range / 2 <= y) and (x + range / 2 >= y)
def iscolour(result, colour):
from LinesNavigation import *
from Exploration import *
###################################
## CONSTANTS AND VARIABLES
NAVDATA_RFID_ADDR = 16
# this magic number must match with the one recorded in the tags to read their nav data (useful for a fully anotated graph)
# if a program input is given, a random magic number is generated, causing current navdata to be ignored and replaced
NAVDATA_MAGIC_NUMBER_STR = "11a12a18"
robot = EnterpriseBot()
rfidReader = RfidSerialThread("COM5" if (platform.system() == "Windows") else "/dev/ttyUSB0")
brickButton = Button()
lastTagTime = 0
RFID_TO_AXIS_DISTANCE = 4.0 #old: 5.7 # measure considering the axis as 0, and considering positive if should move forward to reach RFID reader
###################################
## FUNCTIONS
def dbgSleep(t=1.0):
indebug = False
if indebug:
sleep(t)
def navigateNextPort(lastNode, nodeData, nextNode):
p_last = nodeData.getExitTo(lastNode)