class AutoCar:
def __init__(self, left_motor_port, right_motor_port):
self.__movesteering = MoveSteering(left_motor_port, right_motor_port)
self.__touch = TouchSensor()
def __run_forward(self):
self.__movesteering.on(0, 30)
def __stop(self):
self.__movesteering.off()
def __play_text_sound(self, words):
sound = Sound()
sound.speak(words)
def __back_left(self):
self.__movesteering.on_for_rotations(50, 20, -1)
def __touch_sensor_pressed(self):
return self.__touch.is_pressed
def __touch_execute(self):
self.__stop()
self.__play_text_sound("Ouch") #take some time, consume resource
self.__back_left()
def run(self):
self.__run_forward()
touched = self.__touch_sensor_pressed()
if (touched):
self.__touch_execute()
class MovementController:
"""Class to move the robot"""
_ROTATION_TO_DEGREE = 1
_DISTANCE_TO_DEGREE = 1
def __init__(self):
self.moveSteering = MoveSteering(constants.LEFT_MOTOR_PORT,
constants.RIGHT_MOTOR_PORT)
def rotate(self, degrees, speed=100, block=True):
"""Rotate the robot a certain number of degrees. Positive is counter-clockwise"""
self.moveSteering.on_for_degrees(
100,
SpeedPercent(speed),
degrees * MovementController._ROTATION_TO_DEGREE,
block=block)
def travel(self, distance, speed=100, block=True):
"""Make the robot move forward or backward a certain number of cm"""
self.moveSteering.on_for_degrees(
0,
speed,
distance * MovementController._DISTANCE_TO_DEGREE,
block=block)
def steer(self, direction, speed=100):
"""Make the robot move in a direction"""
self.moveSteering.on(direction, speed)
def stop(self):
"""Make robot stop"""
self.moveSteering.off()
class WalkDog:
def __init__(self, left_motor_port, right_motor_port, sensor_mode):
self._movesteering = MoveSteering(left_motor_port, right_motor_port)
self._ir = InfraredSensor()
self._ir.mode = sensor_mode
@property
def ir(self):
return self._ir
@ir.setter
def ir(self, ir):
self._ir = ir
@property
def movesteering(self):
return _movesteering
@movesteering.setter
def movesteering(self, ms):
self._movesteering = ms
def convert_heading_to_steering(self, heading):
return heading * 2
def run(self, channel=1):
beacon_distance = self._ir.distance(channel)
head_angle = self._ir.heading(channel)
steering = self.convert_heading_to_steering(head_angle)
if (beacon_distance > 30):
self._movesteering.on(steering, 50)
else:
self._movesteering.off()
class ThirdStage:
def __init__(self):
self.steeringDrive = MoveSteering(OUTPUT_B, OUTPUT_C)
self.moveTank = MoveTank(OUTPUT_B, OUTPUT_C)
self.mvInfrared = MVInfraredSensor()
def start(self):
# go until wall
self.goUntilDistanceFromWall(41)
# turn right
self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40),
1.33)
#self.steeringDrive.on(100, SpeedPercent(40))
# go until wall
self.goUntilDistanceFromWall(36.4)
# turn right
self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40),
1.33)
#self.steeringDrive.on(100, SpeedPercent(40))
sleep(4)
self.steeringDrive.on_for_seconds(0, SpeedPercent(-100), 6)
def goUntilDistanceFromWall(self, distance, speed=-40):
while True:
#self.moveTank.on_for_rotations(SpeedPercent(speed), SpeedPercent(speed), 0.25)
self.steeringDrive.on(0, SpeedPercent(speed))
print(self.mvInfrared.getDistance())
if self.mvInfrared.getDistance() < distance:
self.steeringDrive.off()
return
class AutoDrive:
def __init__(self, left_motor_port, right_motor_port, infra_mode,
ultra_mode):
self.__moveSteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediumMotor = MediumMotor()
self.__ir = InfraredSensor()
self.__ir.mode = infra_mode
self.__us = UltrasonicSensor()
self.__us.mode = ultra_mode
def __run_forward(self):
self.__moveSteering.on(0, 30)
def __run_backward_rotations(self, rotations):
self.__moveSteering.on_for_rotations(0, 20, -rotations)
def __stop(self):
self.__moveSteering.off()
def __back_and_turn(self, steering):
self.__moveSteering.on_for_rotations(-steering, 20, -1)
def __scan_around_distance(self):
proximities = {}
distance = 0
index = 0
for deg in [-90, 30, 30, 30, 30, 30, 30]:
self.__mediumMotor.on_for_degrees(10, deg)
distance = self.__ir.proximity
proximities[-90 + index * 30] = distance
index += 1
time.sleep(1)
self.__mediumMotor.on_for_degrees(10, -90)
# print("%s" % proximities)
return proximities
def __find_clear_direction(self, proximitiesDic):
max_distance = max(list(proximitiesDic.values()))
direction = list(proximitiesDic.keys())[list(
proximitiesDic.values()).index(max_distance)]
#print("%d" % direction)
steering = self.__convert_direction_to_steering(direction)
return steering
def __convert_direction_to_steering(self, direction):
return 5 * direction / 9
def __ultrasonic_distance(self):
return self.__us.distance_centimeters
def run(self):
self.__run_forward()
block_distance = self.__ultrasonic_distance()
if (block_distance < 20):
self.__stop()
around_distance = self.__scan_around_distance()
steering = self.__find_clear_direction(around_distance)
self.__run_backward_rotations(0.5)
self.__back_and_turn(steering)
class Drive(object):
def __init__(self):
self.pid = PIDController(kP=1.0, kI=0.0, kD=0.1)
# motors
try:
self.steerPair = MoveSteering(OUTPUT_B, OUTPUT_C)
except ev3dev2.DeviceNotFound as e:
Debug.print("Error:", e)
self.speed = Config.data['pid']['fast']['speed_max']
def updateConfig(self):
self.speed = Config.data['pid']['fast']['speed_max']
self.pid.updateConfig()
def followLine(self, sensValues):
colorLeft = sensValues["ColorLeft"][1] # TODO: HSL? Lichtwert anpassen
colorRight = sensValues["ColorRight"][1]
feedback = colorLeft - colorRight
self.pid.update(feedback)
turn = self.pid.output
self.steerPair.on(turn, self.speed)
def followLineSlow(self, speed, sensValues):
colorLeft = sensValues["ColorLeft"][1] # TODO: HSL? Lichtwert anpassen
colorRight = sensValues["ColorRight"][1]
feedback = colorLeft - colorRight
self.pid.update(feedback)
turn = self.pid.output
self.steerPair.on(turn, self.speed)
def turn(self, action):
def right():
self.steerPair.on_for_degrees(-100, 20, 200)
def left():
self.steerPair.on_for_degrees(100, 20, 200)
if action == "right":
right()
elif action == "left":
left()
elif action == "skip":
self.steerPair.on_for_degrees(
0, -20, 50) # back off until centered on cross
self.steerPair.wait_until_not_moving(2000)
left()
else:
raise AttributeError("no valid action string given for turn()")
def brake(self):
self.steerPair.off()
class MVFollowLine:
def __init__(self):
self.colorSensor = MVColorSensor()
self.steeringDrive = MoveSteering(OUTPUT_B, OUTPUT_C)
self.oldError = 0.01
self.timer = 0
def followLine(self):
isRed = False
self.timer = time()
while not isRed:
self._followLine(self.oldError)
if (time() - self.timer) > 100:
isRed = self._isRed(self.colorSensor.getRGB())
self.steeringDrive.off()
def lookForLine(self, speed=-40):
intensity = self.colorSensor.getRefectionLight()
while intensity < 17: # prev 20
print(intensity)
self.steeringDrive.on(0, SpeedPercent(speed))
intensity = self.colorSensor.getRefectionLight()
def turnOnLine(self):
epsilon = 10
while True:
intensity = self.colorSensor.getRefectionLight()
if 18 < intensity and intensity < 22:
self.steeringDrive.on_for_seconds(0, SpeedPercent(-30), 2)
break
else:
self._followLine(self.oldError)
def _followLine(self, oldError, kp=100, ki=0):
intensity = self._mapping(self.colorSensor.getRefectionLight())
speed = -20
self.steeringDrive.on(0.7 * intensity, SpeedPercent(speed))
#self.oldError = intensity
def _mapping(self, x):
if x <= 20:
return max(100 / 12 * x - 1000 / 6, -100)
return min(5 * x - 100, 100)
# red : 150, 175, 286
# white: 137, 151, 181
# black: 38, 32, 40
# yellow: 190, 143, 40
def _isRed(self, rgb):
return rgb[0] > 145 and rgb[1] > 165 and rgb[2] > 250
class Wheels:
def __init__(self,
left_address=OUTPUT_A,
right_address=OUTPUT_B,
touch_address=INPUT_3):
self._wheels = MoveSteering(left_address, right_address)
self._left = self._wheels.left_motor
self._right = self._wheels.right_motor
self._touch = TouchSensor(touch_address)
def move_straight(self, speed=50):
self._wheels.on(0, SpeedPercent(speed))
def move_for_distance(self, speed=50, distance_mm=100):
MoveDifferential(OUTPUT_A, OUTPUT_B, EV3EducationSetTire,
80).on_for_distance(speed, distance_mm, brake=False)
def rotate_right_in_place(self,
speed=50,
amount=1.0,
brake=True,
block=True):
self._wheels.on_for_rotations(-100,
SpeedPercent(speed),
amount,
brake=brake,
block=block)
def rotate_left_in_place(self,
speed=50,
amount=1.0,
brake=True,
block=True):
self._wheels.on_for_rotations(100,
SpeedPercent(speed),
amount,
brake=brake,
block=block)
def reverse(self, speed=50):
self._wheels.on(0, SpeedPercent(-speed))
def reverse_until_bumped(self, speed=50, timeout=None):
self.reverse(speed)
time = datetime.now()
ms = time.microsecond
while not timeout or time.microsecond - ms < timeout:
if self._touch.is_pressed:
self._wheels.off()
break
def stop(self):
self._wheels.off(brake=False)
class Ev3Robot:
def __init__(self, wheel1=OUTPUT_B, wheel2=OUTPUT_C):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motorB = LargeMotor(OUTPUT_B)
self.motorC = LargeMotor(OUTPUT_C)
def pivot_right(self, degrees, speed=20):
self.tank_pair.on(left_speed=0, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def pivot_left(self, degrees, speed=20):
self.tank_pair.on(left_speed=speed, right_speed=0)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def spin_right(self, degrees, speed=20):
self.gyro.mode = 'GYRO-ANG'
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed, right_speed=speed * -1)
self.gyro.wait_until_angle_changed_by(degrees)
self.tank_pair.off()
value2 = self.gyro.angle
self.tank_pair.on(left_speed=-20, right_speed=20)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees)
def spin_left(self, degrees, speed=20):
self.gyro.mode = 'GYRO-ANG'
value1 = self.gyro.angles
self.gyro.reset()
self.tank_pair.on(left_speed=speed * -1, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees)
self.tank_pair.off()
value2 = self.gyro.angle
self.tank_pair.on(left_speed=20, right_speed=-20)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees)
def go_straight_forward(self, cm, speed=30):
value1 = self.motorB.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while (self.motorB.position - value1) / 360 < rotations:
self.gyro.mode = 'GYRO-ANG'
angle = self.gyro.angle - angle0
print(angle, file=stderr)
self.steer_pair.on(steering=angle * -1, speed=speed)
async def on_connect(socket, path):
color_sensor = ColorSensor(INPUT_1)
motor = MoveSteering(OUTPUT_A, OUTPUT_B)
try:
while True:
try:
cmd = await asyncio.wait_for(socket.recv(), timeout=0.25)
if cmd == 'go':
motor.on(0, SpeedPercent(-25))
elif cmd == 'stay':
motor.off()
except TimeoutError:
pass
await socket.send(str(color_sensor.color_name))
except ConnectionClosed:
pass
def move():
motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
touch_sensor = TouchSensor()
# Start robot moving forward
motor_pair.on(steering=0, speed=60)
# Wait until robot touches wall
touch_sensor.wait_for_pressed()
# Stop moving forward
motor_pair.off()
# Reverse away from wall
motor_pair.on_for_seconds(steering=0, speed=-10, seconds=2)
motor_pair.on_for_rotations(steering=-100, speed=15, rotations=0.5)
motor_pair.on_for_rotations(steering=-100, speed=15, rotations=0.5)
class GyroCar:
def __init__(self, left_motor_port, right_motor_port, gyro_mode):
self.__moveSteering = MoveSteering(left_motor_port, right_motor_port)
self.__gyro = GyroSensor()
self.__gyro.mode = gyro_mode
def __run_forward(self):
self.__moveSteering.on(0, 20)
def run_rotations(self, rotations):
self.__moveSteering.on_for_rotations(0, 20, rotations)
def stop(self):
self.__moveSteering.off()
def __back_and_turn(self, steering):
self.__moveSteering.on_for_rotations(-steering, 20, -1)
def reset_angle(self):
self.__gyro.reset()
def is_on_flat(self):
print("is_on_flat: %d" % self.__gyro.angle)
time.sleep(1)
return (math.fabs(self.__gyro.angle) < 10)
def is_on_slope(self):
print("is_on_slope: %d" % self.__gyro.angle)
time.sleep(1)
return (math.fabs(self.__gyro.angle) >= 10)
def run(self):
self.__run_forward()
@property
def angle(self):
return self.__gyro.angle
class Driver:
def __init__(self):
self.driver = MoveSteering(OUTPUT_B, OUTPUT_C)
self.speed = 40
def set_speed(self, speed):
self.speed = max(-100, min(100, speed))
def get_speed(self):
return self.speed
def move(self):
self.driver.on(0, SpeedPercent(self.speed))
def move_cm(self, cm):
TRANSFORM_CONST = 37.0
self.driver.on_for_degrees(0, SpeedPercent(self.speed), cm * TRANSFORM_CONST)
def move_neg_cm(self, cm):
TRANSFORM_CONST = 37.0
self.driver.on_for_degrees(0, SpeedPercent(self.speed), -cm * TRANSFORM_CONST)
def reverse(self):
self.driver.on(0, SpeedPercent(-self.speed))
def reverse_cm(self, cm):
TRANSFORM_CONST = 37.0
self.driver.on_for_degrees(0, SpeedPercent(-self.speed), cm*TRANSFORM_CONST)
def stop(self):
self.driver.off()
def turn(self, steering):
steering = max(-100, min(100, steering))
self.driver.on(steering, self.speed)
def turn_rotations(self, steering, rotations):
steering = max(-100, min(100, steering))
self.driver.on_for_rotations(steering, SpeedPercent(self.speed), rotations)
def turn_degrees(self, degrees):
TRANSFORM_CONST = 3.9
self.driver.on_for_degrees(100, SpeedPercent(self.speed), degrees * TRANSFORM_CONST)
def turn_neg_degrees(self, degrees):
TRANSFORM_CONST = 3.9
steering = 100 if degrees > 0 else -100
self.driver.on_for_degrees(steering, SpeedPercent(self.speed), -degrees * TRANSFORM_CONST)
def move_seconds(self, seconds):
self.driver.on_for_seconds(0, self.speed, seconds)
def back_seconds(self, seconds):
self.driver.on_for_seconds(0, -self.speed, seconds)
async def on_connect(socket, path):
movesteering = MoveSteering(OUTPUT_A, OUTPUT_B)
fork = MediumMotor(OUTPUT_C)
color_sensor = ColorSensor(address=INPUT_1)
while True:
try:
raw_cmd = await asyncio.wait_for(socket.recv(), timeout=500)
if raw_cmd != "":
command = json.loads(raw_cmd)
command_type = command['type']
print("MOVE COMMAND")
print(command)
if command_type == 'MOVE':
move = command['move']
if move == 'w':
movesteering.on(0, 100)
elif move == 's':
movesteering.on(0, -100)
elif move == 'a':
movesteering.on(100, 100)
elif move == 'd':
movesteering.on(-100, 100)
elif move == 't':
fork.on(-100)
elif move == 'g':
fork.on(100)
elif move == 'c':
print(color_sensor.color_name)
elif move == 'stop':
movesteering.off()
fork.off()
except TimeoutError:
pass
def Robotrun4():
robot = MoveSteering(OUTPUT_A, OUTPUT_B)
tank = MoveTank(OUTPUT_A, OUTPUT_B)
colorLeft = ColorSensor(INPUT_1)
colorRight = ColorSensor(INPUT_3)
gyro = GyroSensor(INPUT_2)
motorC = LargeMotor(OUTPUT_C)
motorD = LargeMotor(OUTPUT_D)
motorB = LargeMotor(OUTPUT_B)
motorA = LargeMotor(OUTPUT_A)
Constants.STOP = False
gyro.reset()
GyroDrift()
gyro.reset()
show_text("Robot Run 2")
motorC.off(brake=True)
#GyroTurn(steering=-50, angle=5)
acceleration(degrees=DistanceToDegree(30), finalSpeed=30)
lineFollowPID(degrees=DistanceToDegree(30), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3))
acceleration(degrees=DistanceToDegree(5), finalSpeed=30)
accelerationMoveBackward(degrees = DistanceToDegree(7), finalSpeed=50, steering=0)
motorC.on_for_seconds(speed=15, seconds=1, brake=False)
GyroTurn(steering=50, angle=20)
acceleration(degrees=DistanceToDegree(20), finalSpeed=30)
GyroTurn(steering=-55, angle=22)
acceleration(degrees=DistanceToDegree(17), finalSpeed=30)
gyro.mode = "GYRO-ANG"
while gyro.value() < -10 and False == Constants.STOP:
motorA.on(speed = 20)
lineFollowPID(degrees=DistanceToDegree(15), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3))
lineFollowTillIntersectionPID(kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1))
lineFollowPID(degrees=DistanceToDegree(10), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3))
lineFollowTillIntersectionPID(kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1))
if gyro.angle > 2 and False == Constants.STOP:
GyroTurn(steering=-50, angle=gyro.angle)
elif gyro.angle < -2 and False == Constants.STOP:
ang = -1 * gyro.angle
GyroTurn(steering=50, angle=ang)
accelerationMoveBackward(degrees = DistanceToDegree(5), finalSpeed=50, steering=0)
acceleration(degrees=DistanceToDegree(12), finalSpeed=50, steering=2.5)
motorC.on_for_degrees(speed=-25, degrees=150, brake=True)
motorD.on_for_degrees(speed=-25, degrees=150, brake=True)
acceleration(degrees=DistanceToDegree(12), finalSpeed=45, steering=4)
#acceleration(degrees=DistanceToDegree(12), finalSpeed=45, steering=5)
#Moving treadmill
if False == Constants.STOP:
tank.on_for_seconds(left_speed=1, right_speed=20, seconds=5.5)
#motorB.on_for_seconds(speed=15, seconds=10)
motorC.on_for_seconds(speed=25, seconds=2, brake=False)
motorD.on_for_seconds(speed=25, seconds=2, brake=False)
accelerationMoveBackward(degrees = DistanceToDegree(5), finalSpeed=20, steering=0)
while colorLeft.reflected_light_intensity < Constants.WHITE:
robot.on(steering=0, speed=-20)
accelerationMoveBackward(degrees = DistanceToDegree(2), finalSpeed=10, steering=0)
GyroTurn(steering=-50, angle=gyro.angle)
MoveBackwardBlack(10)
ang = -1 * (88 + gyro.angle)
GyroTurn(steering=-100, angle=ang)
# wall square
if False == Constants.STOP:
robot.on_for_seconds(steering=5, speed=-10, seconds=2.7, brake=False)
# moving towards row machine
acceleration(degrees=DistanceToDegree(3), finalSpeed=50, steering=0)
ang = math.fabs(89 + gyro.angle)
show_text(str(ang))
if ang > 2 and False == Constants.STOP:
GyroTurn(steering=-100, angle=ang)
acceleration(degrees=DistanceToDegree(26), finalSpeed=50, steering=0)
GyroTurn(steering=100, angle=68)
#acceleration(degrees=DistanceToDegree(1), finalSpeed=20, steering=0)
if False == Constants.STOP:
motorC.on_for_seconds(speed=-10, seconds=1.5, brake=False)
GyroTurn(steering=100, angle=2)
sleep(0.2)
GyroTurn(steering=-100, angle=2)
motorC.on_for_seconds(speed=-10, seconds=0.2, brake=True)
motorC.off(brake=True)
acceleration(degrees=DistanceToDegree(1), finalSpeed=20, steering=0)
accelerationMoveBackward(degrees = DistanceToDegree(10), finalSpeed=20, steering=0)
GyroTurn(steering=-100, angle=10)
#DOING Row Machine
if False == Constants.STOP:
motorC.on_for_seconds(speed=20, seconds=2)
ang = 90 + gyro.angle
GyroTurn(steering=-100, angle=ang)
acceleration(degrees=DistanceToDegree(28), finalSpeed=50, steering=0)
lineSquare()
#Moving towards weight machine
show_text(str(gyro.angle))
ang = math.fabs(87 + gyro.angle)
show_text(str(ang))
GyroTurn(steering=100, angle=ang)
acceleration(degrees=DistanceToDegree(22), finalSpeed=30, steering=0)
if False == Constants.STOP:
motorD.on_for_degrees(speed=-20, degrees=160)
GyroTurn(steering=-100, angle=ang)
accelerationMoveBackward(degrees = DistanceToDegree(20), finalSpeed=20, steering=0)
if False == Constants.STOP:
motorD.on_for_seconds(speed=20, seconds=2, brake=True)
lineSquare()
if False == Constants.STOP:
GyroTurn(steering=-40, angle=85)
lineFollowRightPID(degrees=DistanceToDegree(30), kp=1.3, ki=0.01, kd=15, color=colorLeft)
lineFollowTillIntersectionRightPID(kp=1.3, ki=0.01, kd=15, color=colorLeft, color2=colorRight)
lineFollowRightPID(degrees=DistanceToDegree(34), kp=1.3, ki=0.01, kd=15, color=colorLeft)
if False == Constants.STOP:
GyroTurn(steering=50, angle=20)
acceleration(degrees=DistanceToDegree(12), finalSpeed=30, steering=2)
lineSquare()
if False == Constants.STOP:
GyroTurn(steering=100, angle=75)
motorC.on_for_seconds(speed=-10, seconds=1, brake=False)
acceleration(degrees=DistanceToDegree(6.5), finalSpeed=30, steering=0)
motorC.on_for_seconds(speed=10, seconds=2, brake=True)
if False == Constants.STOP:
GyroTurn(steering=50, angle=75)
acceleration(degrees=DistanceToDegree(5), finalSpeed=30, steering=0)
while Constants.STOP == False:
acceleration(degrees=DistanceToDegree(3), finalSpeed=31, steering=0)
accelerationMoveBackward(degrees = DistanceToDegree(3), finalSpeed=30, steering=0)
motorC.off(brake=False)
motorD.off(brake=False)
alinhamento() #achou a faixa preta
# sleep(0.1)
# k = (cor('dir') != 'amarelo') and (cor('dir') != 'azul') and (cor('dir') != 'vermelho')
# l = (cor('esq') != 'amarelo') and (cor('esq') != 'azul') and (cor('esq') != 'vermelho')
# while k and l:
# alinhamento() #achou a primeira cor
# sleep(0.1)
# k = (cor('dir') != 'amarelo') and (cor('dir') != 'azul') and (cor('dir') != 'vermelho')
# l = (cor('esq') != 'amarelo') and (cor('esq') != 'azul') and (cor('esq') != 'vermelho')
# steering_pair.on_for_degrees(0,20,100)
steering_pair.on_for_degrees(0,20,100)
girar_pro_lado('esq',90)
cor1 = cor('dir') #salvar primeira cor
while cor('dir')==cor1 or cor('dir')=='preto':
while cor('dir')!='preto':
steering_pair.on(-10,15)
else:
steering_pair.off()
steering_pair.on_for_degrees(50,15,100)
# sleep(0.01)
# girar_pro_lado('esq',85) #90 graus pra esquerda
# alinhamento()
# steering_pair.on_for_degrees(0,20,350)
cor2 = cor('dir') #salvar segunda cor
cor3 = autocompletar(cor1, cor2) #autocompletar 3a cor
cores = open("cores.txt","a")
escrever = [cor1,'\n',cor2,'\n',cor3]
cores.writelines(escrever)
cores.close()
sound.beep()
else:
sleep(0.01) # Wait 0.01 second
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
garra_g.on_for_seconds(10, 1.5)
garra_m.on_for_seconds(10, 1)
mapadecores = ['vermelho', 'amarelo', 'azul']
while True:
while meeting_area: #começa aleatório, termina virado pro preto
while cor('esq') == 'branco' and cor('dir') == 'branco':
log = open('log.txt', 'a')
log.write('Sensores no branco, indo pra frente\n')
log.close()
steering_pair.on(0, 20)
else:
log = open('log.txt', 'a')
log.write('Algum sensor saiu do branco, para.\n')
log.close()
steering_pair.off()
#trocar isso acima por alinhamento()
#steering_pair.on_for_degrees(0,10,30)
if cor('esq') == 'vazio' or cor('dir') == 'vazio':
log = open('log.txt', 'a')
log.write('Um dos sensores está no vazio\n')
log.close()
steering_pair.on_for_degrees(0, -10, 120)
sleep(1)
alinhamento()
sleep(1)
ir_pro_gasoduto = True
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
else:
sleep(0.01) # Wait 0.01 second
sleep(10)
sound.beep()
sound.beep()
while ir_pro_gasoduto: #começa com os sensores no vazio certo
#termina paralelo ao gasoduto
steering_pair.on_for_degrees(0, -20, 350)
girar_pro_lado('esq', 90)
while cor('esq') != 'azul' and cor('dir') != 'azul':
steering_pair.on(0, -40)
else:
steering_pair.off()
steering_pair.on_for_degrees(0, -55, 300)
girar_pro_lado('esq', 180)
while usf.distance_centimeters > 15:
steering_pair.on(0, 10)
else:
steering_pair.off()
if usf.distance_centimeters < 16 and cor('esq') == 'azul':
girar_pro_lado('dir', 90)
sound.speak('go go go')
garra_g.on_for_degrees(20, -1200)
sleep(0.1)
global gasoduto_com_cano
gasoduto_com_cano = (usl.distance_centimeters < 25
#!/usr/bin/env python3
from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C, MoveSteering
from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor
from time import sleep
from ev3dev2.sound import Sound
from threading import Thread
us = UltrasonicSensor()
us.mode = 'US-DIST-CM'
ts = TouchSensor()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
speed = 0
while True:
if ts.wait_for_bump() == True:
speed += 10
steer_pair.on(steering=0, speed=speed)
else:
sleep(0.01)
class ColorTank:
def __init__(self, left_motor_port, right_motor_port, infra_sensor_mode,
color_sensor_mode):
self.__movesteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediummotor = MediumMotor()
self.__cs = ColorSensor()
self.__cs.mode = color_sensor_mode
self.__ir = InfraredSensor()
self.__ir.mode = infra_sensor_mode
def __turn_left(self):
self.__movesteering.on(-50, 30)
def __turn_right(self):
self.__movesteering.on(50, 30)
def __run_forward(self):
self.__movesteering.on(0, 50)
def __run_backward(self):
self.__movesteering.on(0, -20)
def __stop(self):
self.__movesteering.off()
def __play_text_sound(self, words):
sound = Sound()
sound.speak(words)
def __lift_up(self):
self.__mediummotor.on_for_degrees(10, 50)
def __lift_down(self):
self.__mediummotor.on_for_degrees(10, -50)
def __get_button_pressed_value(self, buttons):
BUTTON_VALUES = {
0: [],
1: ['top_left'],
2: ['bottom_left'],
3: ['top_right'],
4: ['bottom_right'],
5: ['top_left', 'top_right'],
6: ['top_left', 'bottom_right'],
7: ['bottom_left', 'top_right'],
8: ['bottom_left', 'bottom_right'],
9: ['beacon'],
10: ['top_left', 'bottom_left'],
11: ['top_right', 'bottom_right']
}
return list(BUTTON_VALUES.keys())[list(
BUTTON_VALUES.values()).index(buttons)]
def __run(self, button_value):
if (button_value == 1):
self.__turn_left()
elif (button_value == 3):
self.__turn_right()
elif (button_value == 5):
self.__run_forward()
elif (button_value == 8):
self.__run_backward()
elif (button_value == 2):
self.__lift_up()
elif (button_value == 4):
self.__lift_down()
# elif(button_value == 2):
# self.__play_text_sound("Lily, I love you")
else:
self.__stop()
def __color_detect(self):
color = self.__cs.color
if (color == 1):
self.__play_text_sound("black")
elif (color == 2):
self.__play_text_sound("blue")
elif (color == 3):
self.__play_text_sound("green")
elif (color == 4):
self.__play_text_sound("yellow")
elif (color == 5):
self.__play_text_sound("red")
elif (color == 6):
self.__play_text_sound("white")
elif (color == 7):
self.__play_text_sound("brown")
else:
pass
def process(self):
self.__ir.process()
buttons_pressed = self.__ir.buttons_pressed()
button_value = self.__get_button_pressed_value(buttons_pressed)
self.__run(button_value)
self.__color_detect()
# c.write(n)
# c.write(') ')
# c.write(str(d))
# c.write('\n')
# c.close()
#precisa achar onde os valores tão estáveis
#ver o comprimento dessa faixa estável
#voltar pra onde essa faixa acabou
achou_fim_cano = False
achou_cano = False
dist = []
while distancia_do_corre(usl) > 40:
steering_pair.on(0, 50)
else:
sleep(2)
sound.beep()
steering_pair.off()
while not (achou_fim_cano):
sleep(0.02)
steering_pair.on_for_degrees(0, 10, 10)
sleep(0.02)
d = distancia_do_corre(usl)
dist.append(d)
if len(dist) > 20: #ter certeza que já tem 7
a = 0
for i in range(20): #pega os 7 últimos e faz a média
a = a + dist[len - 1 - i]
media_dos_7 = a / 20
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import TouchSensor, ColorSensor
from time import sleep
from ev3dev2.sound import Sound
ts = TouchSensor()
cl = ColorSensor()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) # 스티어링 모드 설정
while not ts.is_pressed: # 터치 센서를 클릭하면 멈춤
if cl.reflected_light_intensity < 30:
steer_pair.on(30,10) # 오른쪽 방향으로 10 속도로 주행
else:
steer_pair.on(-30,10) # 왼쪽 방향으로 10 속도로 주행
class myRobot:
# Initialize the Class
def __init__(self, motorL=None, motorR=None, motorM=None, gyroPort=None, colorL=None, colorR=None, colorM=None):
if motorL: self.motorL = LargeMotor(motorL)
if motorR: self.motorR = LargeMotor(motorR)
if motorM: self.motorM = Motor(motorM)
if gyroPort:
self.gyro = GyroSensor(gyroPort)
self.gyro.mode = 'GYRO-CAL'
time.sleep(0.2)
self.gyro.mode = 'GYRO-ANG'
if colorL: self.colorL = ColorSensor(colorL)
if colorR: self.colorR = ColorSensor(colorR)
if colorM: self.colorM = ColorSensor(colorM)
if motorL and motorR:
self.drive = MoveSteering(motorL, motorR)
self.move = MoveTank(motorL, motorR)
print("Successful initialization!")
# Function to go forward, by default it uses the gyro for correcting, but it can be disabled.
# Can use either rotations to measure distance, or time
def forward(self, speed, distance, useRots=True, correction=2, useGyro=True, way=None):
initialGyro = way if way else self.gyro.angle
if useRots:
initialMotor = abs((self.motorL.position/360 + self.motorR.position/360)/2) # Get the average of the 2 motor values
while abs((self.motorL.position/360 + self.motorR.position/360)/2) < initialMotor+distance:
self.drive.on(initialGyro - self.gyro.angle*correction, speed)
print("%f : %i" % (abs((self.motorL.position/360 + self.motorR.position/360)/2), initialMotor+distance))
print("gyro: %i" % self.gyro.angle)
elif not useRots:
initialTime = time.time()
while time.time()-initialTime < distance:
self.drive.on(initialGyro - self.gyro.angle*correction, speed)
print("%f : %i" % (time.time()-initialTime, distance))
print("gyro: %i" % self.gyro.angle)
def stop(self, brake=False):
self.drive.stop(brake=brake)
# Function that can turn either absolute or relative to current rotation
# if way is float, then destination will be gyro.angle+way, if int, then just way
def turn(self, way, speed, linear=False, leeway=1, debug=False, minSpeed=5):
if type(way)==type(1.1):
way = int(self.gyro.angle+way)
leewayList = range(way-leeway, way+leeway+1)
if linear:
while self.gyro.angle not in leewayList:
if debug: print(self.gyro.angle)
lin = abs(way/self.gyro.angle)
print(lin)
self.move.on(speed*lin+minSpeed, speed*-1*lin+minSpeed)
elif not linear:
while self.gyro.angle not in leewayList:
if debug: print(self.gyro.angle)
self.move.on(speed, speed*-1)
# Will fix gyro drift when called
def nodrift(self, t=0.2):
self.gyro.mode = 'GYRO-CAL'
time.sleep(t)
self.gyro.mode = 'GYRO-ANG'
class Ev3Robot:
def __init__(self,
wheel1=OUTPUT_B,
wheel2=OUTPUT_C,
wheel3=OUTPUT_A,
wheel4=OUTPUT_D):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.motor3 = MediumMotor(wheel3)
self.motor4 = MediumMotor(wheel4)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
self.gyro.mode = 'GYRO-ANG'
self.led = Leds()
self.btn = Button()
self.btn._state = set()
def write_color(self, file, value):
# opens a file
f = open(file, "w")
# writes a value to the file
f.write(str(value))
f.close()
def read_color(self, file):
# opens a file
f = open(file, "r")
# reads the value
color = int(f.readline().strip())
f.close()
return color
def pivot_right(self, degrees, speed=20):
# makes the robot pivot to the right until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=speed, right_speed=0)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def pivot_left(self, degrees, speed=20):
# makes the robot pivot to the left until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=0, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def old_spin_right(self, degrees, speed=20):
#old program; don't use
self.gyro.reset()
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed, right_speed=speed * -1)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=-30, right_speed=30)
self.gyro.wait_until_angle_changed_by(value1 - value2 - degrees)
self.stop()
def old_spin_left(self, degrees, speed=20):
#old program; don't use
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed * -1, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=8, right_speed=-8)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees + 5)
self.tank_pair.off()
def dog_gear(self, degrees, motor, speed=30):
if motor == 3:
self.motor3.on_for_degrees(degrees=degrees, speed=speed)
self.motor3.off()
else:
self.motor4.on_for_degrees(degrees=degrees, speed=speed)
def go_straight_forward(self, cm, speed=20, wiggle_factor=1):
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05 #divides by circumference of the wheel
# calculates the amount of degrees the robot has turned, then turns the
# opposite direction and repeats until the robot has gone the required
# number of centimeters
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor * -1,
speed=speed * -1)
self.steer_pair.off()
def go_straight_backward(self, cm, speed=20, wiggle_factor=1):
# see go_straight_forward
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor, speed=speed)
self.steer_pair.off()
def calibrate(self):
# turns the led lights orange, and waits for a button to be pressed
# (signal that the robot is on black), then calculates the reflected
# light intensity of the black line, then does the same on the white line
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._black1 = self.color1.reflected_light_intensity
self._black4 = self.color4.reflected_light_intensity
sleep(2)
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._white1 = self.color1.reflected_light_intensity
self._white4 = self.color4.reflected_light_intensity
sleep(3)
self.write_color("/tmp/black1", self._black1)
self.write_color("/tmp/black4", self._black4)
self.write_color("/tmp/white1", self._white1)
self.write_color("/tmp/white4", self._white4)
def read_calibration(self):
# reads the color files
self._black1 = self.read_color("/tmp/black1")
self._black4 = self.read_color("/tmp/black4")
self._white1 = self.read_color("/tmp/white1")
self._white4 = self.read_color("/tmp/white4")
def align_white(self, speed=20, t=96.8, direction=1):
# goes forward until one of the color sensors sees the white line.
while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI(
self.color4) < t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
# determines which sensor sensed the white line, then moves the opposite
# motor until both sensors have sensed the white line
if self.calibrate_RLI(self.color4) > t:
while self.calibrate_RLI(self.color1) < t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) < t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align_black(self, speed=20, t=4.7, direction=1):
# see align_white
while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI(
self.color4) > t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
if self.calibrate_RLI(self.color4) < t:
while self.calibrate_RLI(self.color1) > t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) > t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align(self, t, speed=-20, direction=1):
# aligns three times for extra accuracy
self.align_white(speed=speed, t=100 - t, direction=direction)
self.align_black(speed=-5, t=t, direction=direction)
self.align_white(speed=-5, t=100 - t, direction=direction)
def calibrate_RLI(self, color_sensor):
# returns a scaled value based on what black and white are
if (color_sensor.address == INPUT_1):
black = self._black1
white = self._white1
else:
black = self._black4
white = self._white4
return (color_sensor.reflected_light_intensity - black) / (white -
black) * 100
def stop(self):
self.tank_pair.off()
def spin_right(self, degrees, speed=30):
self.turn(degrees, 100, speed)
def spin_left(self, degrees, speed=30):
self.turn(degrees, -100, speed)
def turn(self, degrees, steering, speed=30):
# turns at a decreasing speed until it turns the required amount of degrees
value1 = self.gyro.angle
s1 = speed
d1 = 0
while d1 < degrees - 2:
value2 = self.gyro.angle
d1 = abs(value1 - value2)
slope = speed / degrees
s1 = (speed - slope * d1) * (degrees / 90) + 3
self.steer_pair.on(steering=steering, speed=s1)
self.steer_pair.off()
us = UltrasonicSensor()
us.mode = 'US-DIST-CM'
ts = TouchSensor()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
def play_sound():
global Distance
while loop == True: # repeat until 'loop' is set to False in the main thread.
Distance = us.value()
if Distance < 200:
sound.play_tone(440, Distance / 1000)
#sleep(0.1)
else:
sleep(0.1)
loop = True
t = Thread(target=play_sound)
t.start()
steer_pair.on(steering=0, speed=10)
while True:
if Distance < 90:
steer_pair.off(brake=True)
loop = False
sleep(1)
else:
loop = True
sleep(0.01)
meeting_area = True
else:
sleep(0.01) # Wait 0.01 second
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
garra_g.on_for_seconds(10, 1.5)
garra_m.on_for_seconds(10, 1)
mapadecores = ['vermelho', 'amarelo', 'azul']
while True:
while meeting_area: #começa aleatoriamente na meeting area
#termina com os sensores de cor no vazio certo
sound.beep()
while cor('esq') == 'branco' and cor('dir') == 'branco':
steering_pair.on(0, 15)
else:
steering_pair.off()
steering_pair.on_for_degrees(0, -15, 70)
alinhamento()
steering_pair.on_for_degrees(0, 10, 70)
if cor('esq') == 'vazio' or cor('dir') == 'vazio':
global no_vazio
no_vazio = True
if cor('esq') != cor('dir'):
steering_pair.on_for_degrees(0, -15, 150)
alinhamento()
steering_pair.on_for_degrees(0, -25,
350) #volta e vira pra esquera
girar_pro_lado('esq', 90)
sound.speak('empty')
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, GyroSensor, UltrasonicSensor
medMotor = MediumMotor(OUTPUT_C)
ultrasonic_sensor_front = UltrasonicSensor(INPUT_4)
ultrasonic_sensor_side = UltrasonicSensor(INPUT_2)
ultrasonic_sensor_front.mode = 'US-DIST-CM'
ultrasonic_sensor_side.mode = 'US-DIST-CM'
gyro = GyroSensor(INPUT_1)
gyro.mode = GyroSensor.MODE_GYRO_ANG
drivetrain = MoveSteering(OUTPUT_B, OUTPUT_D)
front_dist = ultrasonic_sensor_front.distance_centimeters
side_dist = ultrasonic_sensor_side.distance_centimeters
while True:
if ultrasonic_sensor_front.distance_centimeters <= 10:
if ultrasonic_sensor_side.distance_centimeters <= 15:
drivetrain.on(steering=100, speed=20)
gyro.wait_until_angle_changed_by(90)
drivetrain.on(steering=0, speed=0)
else:
drivetrain.on(steering=-100, speed=20)
gyro.wait_until_angle_changed_by(90)
drivetrain.on(steering=0, speed=0)
else:
drivetrain.on(steering=0, speed=20)
# limitations under the License.
print('Starting up...')
print('Loading EV3 dependencies...')
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, SpeedPercent, MoveSteering, MoveDifferential, MoveTank
from ev3dev2.wheel import EV3Tire
from ev3dev2.button import Button
from ev3dev2.sensor.lego import InfraredSensor, TouchSensor, ColorSensor
from ev3dev2.sound import Sound
from ev3dev2.power import PowerSupply
steering_drive = MoveSteering(OUTPUT_A, OUTPUT_B)
STUD_MM = 8
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16 * STUD_MM)
steering_drive.on(0, SpeedPercent(0))
print('Motors initialized')
print('Loading ROS and other dependencies...')
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import json
from time import sleep
from math import degrees
import threading
import datetime
from random import uniform
class EV3DEV(object):
def __init__(self):
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.button import Button
from ev3dev2.sound import Sound
from time import sleep
cl = ColorSensor()
btn = Button()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
sound.speak('Press the Enter key when the sensor is in dim light')
btn.wait_for_bump('enter')
dim = cl.ambient_light_intensity
sound.beep()
sound.speak('Press the Enter key when the sensor is in bright light')
btn.wait_for_bump('enter')
bright = cl.ambient_light_intensity
sound.beep()
sound.speak('3, 2, 1, go!')
while not btn.any(): # Press any key to exit
intensity = cl.ambient_light_intensity
steer = (200 * (intensity - dim) / (bright - dim)) - 100
steer = min(max(steer, -100), 100)
steer_pair.on(steering=steer, speed=30)
sleep(0.1) # wait for 0.1 seconds
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sound import Sound
from time import sleep
cl = ColorSensor()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
sound = Sound()
steer_pair.on(0, 50)
while cl.reflected_light_intensity > 30:
sleep(0.01)
steer_pair.off()
