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 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 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 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 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 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)
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 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 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
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)
def MoveForward(self, steering=0, speed=-20):
steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B, motor_class=LargeMotor)
drill = Drill()
#We make this condition to check if the Robot
if drill.Drilling() == 1:
steer_pair.off()
if self.is_drilled == False:
self.is_drilled = True
print("drilling")
sleep(2)
mm = MediumMotor(OUTPUT_D)
sp = 90
time = 10
mm.on_for_seconds(speed=SpeedRPM(sp), seconds=time)
print("Number of rotations =" + str(sp / 6))
else:
steer_pair.on_for_seconds(steering=0,
speed=-1 * SpeedRPM(12),
seconds=1,
brake=True,
block=True)
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
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
class Ev3Robot:
__slots__ = [
'_black1' ,
'_black4' ,
'_white1' ,
'_white4'
]
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.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
def write_color(self, file, value):
f = open(file, "w")
f.write(str(value))
f.close()
def read_color(self, file):
f = open(file, "r")
color = int(f.readline().strip())
f.close()
return color
def pivot_right(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 pivot_left(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 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.angle
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 = -20):
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while abs(self.motor1.position - value1) / 360 < rotations:
self.gyro.mode = 'GYRO-ANG'
angle = self.gyro.angle - angle0
self.steer_pair.on(steering = angle * -1, speed = speed)
def go_straight_backward(self, cm, speed = 30):
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while abs(self.motor1.position - value1) / 360 < rotations:
self.gyro.mode = 'GYRO-ANG'
angle = self.gyro.angle - angle0
self.steer_pair.on(steering = angle, speed = speed * -1)
def calibrate(self):
print("black", file = stderr)
sleep(10)
self._black1 = self.color1.reflected_light_intensity
self._black4 = self.color4.reflected_light_intensity
print(self._black1, self._black4, file = stderr)
sleep(3)
print("white", file = stderr)
sleep(10)
self._white1 = self.color1.reflected_light_intensity
self._white4 = self.color4.reflected_light_intensity
print(self._white1, self._white4, file = stderr)
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):
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):
print(self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI(self.color4) < t:
self.steer_pair.on(steering = 0, speed = speed)
print(self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
self.steer_pair.off()
if self.calibrate_RLI(self.color4) > t:
while self.calibrate_RLI(self.color1) < t:
self.motor1.on(speed = speed)
print(self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) < t:
self.motor2.on(speed = speed)
self.motor2.off()
print(self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
def align_black(self, speed = 20, t = 4.7):
print("1 ", self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI(self.color4) > t:
self.steer_pair.on(steering = 0, speed = speed)
print("2 ", self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
self.steer_pair.off()
if self.calibrate_RLI(self.color4) < t:
while self.calibrate_RLI(self.color1) > t:
self.motor1.on(speed = speed)
print("3 ", self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) > t:
self.motor2.on(speed = speed)
print("4 ", self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
self.motor2.off()
print("5 ", self.calibrate_RLI(self.color1), self.calibrate_RLI(self.color4), file = stderr)
def align(self, t, speed = -20):
self.align_white(speed = speed, t = 100 - t)
self.align_black(speed = -5, t = t)
self.align_white(speed = -5, t = 100 - t)
def calibrate_RLI(self, color_sensor):
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
class run2019:
def __init__(self):
self.btn = Button()
self.LLight = LightSensor(INPUT_1)
self.RLight = LightSensor(INPUT_4)
self.cs = ColorSensor()
self.drive = MoveTank(OUTPUT_A, OUTPUT_D)
self.steer = MoveSteering(OUTPUT_A, OUTPUT_D)
self.heightmotor = LargeMotor(OUTPUT_B)
self.clawactuator = MediumMotor(OUTPUT_C)
os.system('setfont Lat15-TerminusBold14')
self.speed = 40
self.sectionCache = 0
self.orient = {'N': "1", 'E': "1", 'S': "1", 'W': "1"}
def sensordata(self):
print("Left Light Sensor: ", end="", file=sys.stderr)
print(self.LLight.reflected_light_intensity, end=" ", file=sys.stderr)
print("Right Light Sensor: ", end="", file=sys.stderr)
print(self.RLight.reflected_light_intensity, file=sys.stderr)
def turn(self, direc): # Half - works
self.drive.on_for_degrees(SpeedDPS(225), SpeedDPS(225), 223)
if direc == "L" or direc == "l":
self.steer.on_for_degrees(steering=-100,
speed=SpeedDPS(720),
degrees=400)
elif direc == "R" or direc == "r":
self.steer.on_for_degrees(steering=100,
speed=SpeedDPS(720),
degrees=720)
self.steer.off()
def dti(self,
speed,
n,
startCounting=False,
sectionCache=0): # Drive to nth intersection
kp = 1.1
ki = 0
kd = 0
integral = 0
perror = error = 0
inters = 0
piderror = 0
while not self.btn.any(
): # Remember to try stuff twice, this is a keyboard interrupt
lv = self.LLight.reflected_light_intensity
rv = self.RLight.reflected_light_intensity
error = rv - lv
integral += integral + error
derivative = lv - perror
piderror = (error * kp) + (integral * ki) + (derivative * kd)
if speed + abs(piderror) > 100:
if piderror >= 0:
piderror = 100 - speed
else:
piderror = speed - 100
self.drive.on(left_speed=speed - piderror,
right_speed=speed + piderror)
sleep(0.01)
perror = error
# Drive up to nth intersection
# These values are subject to change depending on outside factors, CHANGE ON COMPETITION DAY
if (lv <= 50 and rv <= 55) or (lv <= 50 and rv >= 55) or (
lv >= 50 and rv <= 55): # Currently at an intersection
inters += 1
if (startCounting == True):
sectionCache += 1
if inters == n: # Currently at nth intersection
self.drive.off()
return
self.drive.off()
self.drive.on_for_seconds(SpeedDPS(115), SpeedDPS(115), 1)
print("Left Value: {}, Right Value: {}, P error: {}, Inters: {}".
format(lv, rv, piderror, inters),
file=sys.stderr)
def main(self):
self.heightmotor.on(speed=self.speed)
self.heightmotor.wait_until_not_moving()
# # while not btn.any():
# # sensordata()
# # ## STORING COLOURS
self.drive.on_for_degrees(
left_speed=self.speed, right_speed=self.speed,
degrees=50) # To drive past little initial intersection
print(self.orient, file=sys.stderr) #DEBUG
self.turn("L")
# # # GO TO FIRST BNPs
self.dti(self.speed, 5, startCounting=True)
self.turn("L")
self.dti(self.speed, 1)
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()
class SecondStage:
def __init__(self):
self.mvFollowLine = MVFollowLine()
self.steeringDrive = MoveSteering(OUTPUT_B, OUTPUT_C)
self.moveTank = MoveTank(OUTPUT_B, OUTPUT_C)
self.mediumMotor = MediumMotor(OUTPUT_D)
self.mvInfrared = MVInfraredSensor()
def start(self):
# reach line
#self.goUntilDistanceFromWall(27)
self.mvFollowLine.lookForLine()
self.steeringDrive.on_for_seconds(0, SpeedPercent(-40), 0.6)
# turn left
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 1.4)
# push button on left
self.goUntilDistanceFromWall(17)
# go back wee bit
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 2.8)
# turn left
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 1.33)
# go to the ramp
#self.goUntilDistanceFromWall(25)
self.mvFollowLine.lookForLine()
self.steeringDrive.on_for_seconds(0, SpeedPercent(-40), 0.4)
# turn left
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 1.35)
# go up
self.mvFollowLine.lookForLine()
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 10.5)
timer = time()
while time() - timer < 5:
self.mvFollowLine._followLine(0)
self.moveTank.on_for_rotations(SpeedPercent(20), SpeedPercent(-20), 0.1)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 6)
self.mvFollowLine.lookForLine()
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 2.5)
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 0.7)
self.mediumMotor.on_to_position(5, -80)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 0.9)
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(-40), 0.4)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 4)
self.mvFollowLine.lookForLine()
self.mediumMotor.on_to_position(5, 0)
timer = time()
while time() - timer < 3:
self.mvFollowLine._followLine(0)
self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40), 0.15)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(-100), 11)
self.goUntilDistanceFromWall(40)
self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40), 1.3)
self.mvFollowLine.lookForLine()
self.moveTank.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 1.0)
self.moveTank.on_for_rotations(SpeedPercent(-100), SpeedPercent(100), 10)
self.mvFollowLine._followLine(0)
def goUntilDistanceFromWall(self, distance, speed=-40):
while True:
self.steeringDrive.on(0, SpeedPercent(speed))
print(self.mvInfrared.getDistance())
if self.mvInfrared.getDistance() < distance:
self.steeringDrive.off()
break
self.steeringDrive.on_for_seconds(0, SpeedPercent(speed/2), 0.5)
return
(392, 25, 100), (784, 350, 100), (739.98, 250, 100), (698.46, 25, 100),
(659.26, 25, 100), (622.26, 25, 100), (659.26, 50, 400), (415.3, 25, 200),
(554.36, 350, 100), (523.25, 250, 100), (493.88, 25, 100),
(466.16, 25, 100), (440, 25, 100), (466.16, 50, 400), (311.13, 25, 200),
(392, 350, 100), (311.13, 250, 100), (466.16, 25, 100),
(392.00, 300, 150), (311.13, 250, 100), (466.16, 25, 100), (392, 700)
], play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE),
1: lambda: (sound.speak("Chomp") for _ in range(5))
}
switch.get(0)
while button_pressed.value == 0:
switch.get(child_conn.recv())()
values = [Value('i', 0), Pipe()]
procs = start_thread(target=[grab_everything, play_sonic, run_big_claw], value=values)
while not button.any():
print('Proximity(cm): {}'.format(proximity.distance_centimeters))
print('Claw(\u00B0): {}'.format(claw.degrees))
values[0].value = 1
procs[0].join()
procs[1].join()
procs[2].join()
wheels.off()
claw.off()
initial_rotations = proximity.rot
from ev3dev2.motor import (MoveSteering, MediumMotor, OUTPUT_A, OUTPUT_B,
OUTPUT_C)
from ev3dev2.sensor.lego import UltrasonicSensor
from time import sleep
motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
medium_motor = MediumMotor(OUTPUT_A)
ultrasonic_sensor = UltrasonicSensor()
# Start robot moving forward
motor_pair.on(steering=0, speed=10)
# Wait until robot less than 3.5cm from cuboid
while ultrasonic_sensor.distance_centimeters > 3.5:
sleep(0.01)
# Stop moving forward
motor_pair.off()
# Lower robot arm over cuboid
medium_motor.on_for_degrees(speed=-10, degrees=90)
# Drag cuboid backwards for 2 seconds
motor_pair.on_for_seconds(steering=0, speed=-20, seconds=2)
# Raise robot arm
medium_motor.on_for_degrees(speed=10, degrees=90)
# Move robot away from cuboid
motor_pair.on_for_seconds(steering=0, speed=-20, seconds=2)
from ev3dev2.sound import Sound
from ev3dev2.motor import MoveSteering
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D
rob = MoveSteering(OUTPUT_D, OUTPUT_B)
mySnd = Sound()
mySnd.speak("Turning right in place")
rob.on_for_seconds(50, -50, 2)
mySnd.speak("Spiraling")
rob.on_for_seconds(20, 50, 2)
rob.off()
# 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
b = 0
for i in range(20):
if abs(dist[len - 1 - i] - media_dos_7
) > 5: #marcar quantos resultados tão instáveis
#!/usr/bin/env python3
from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C, MoveSteering
from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor,ColorSensor
from ev3dev2.sound import Sound
from ev3dev2.display import Display
from textwrap import wrap
from threading import Thread
#from ev3dev2.led import Leds
from time import sleep
ts = TouchSensor()
cl = ColorSensor()
sound=Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
steer_pair.on(steering=0, speed=20)
while True:
#steer_pair.on(steering=0, speed=20)
#while loop==True:
if cl.color_name== 'Red':
steer_pair.off()
sleep(0.1)
elif cl.color_name== 'Green':
steer_pair.on(steering=0, speed=20)
sleep(0.1)
else:
sleep(0.1)
wheels.on(0, SpeedPercent(50))
while not button.any():
if proximity.distance_centimeters < 5.0:
# swatter_sensor.scan_left()
# swatter_sensor.scan_right()
# if max(swatter_sensor.get_sensor_readings()) >= 250:
# wheels.on_for_rotations(100, SpeedPercent(50), 0.75)
# wheels.on_for_rotations(0, SpeedPercent(35), 1)
claw.on_for_rotations(SpeedPercent(50), -2)
wheels.on_for_seconds(0, SpeedPercent(-100), 1, block=True)
wheels.on_for_rotations(45, SpeedPercent(100), 0.5, block=True)
wheels.on_for_rotations(0, SpeedPercent(85), 5, block=True)
claw.on_for_rotations(SpeedPercent(50), 2)
wheels.on_for_rotations(0, SpeedPercent(-100), 1, block=True)
wheels.on_for_degrees(-100, SpeedPercent(100), 360)
elif touch.is_pressed:
wheels.on_for_rotations(0, SpeedPercent(-50), 1, block=True)
wheels.on_for_rotations(100, SpeedPercent(100), 1.5, block=True)
else:
wheels.on(0, SpeedPercent(50))
proc = Thread(target=grab_everything)
proc.start()
proc.join()
wheels.off(brake=False)
claw.off(brake=False)
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)
front_claw.on_for_degrees(SpeedPercent(20), 570) # opens the claw
wheels.on(0, SpeedPercent(50))
while proximity_sensor.distance_centimeters >= 22:
print('First wall: {}'.format(proximity_sensor.distance_centimeters))
wheels.on(-100, SpeedPercent(15))
start_angle = gyro.angle
while gyro.angle > -82:
print(gyro.angle)
wheels.on(0, SpeedPercent(50))
while proximity_sensor.distance_centimeters >= 12:
pass
wheels.off(brake=True)
front_claw.on_for_degrees(SpeedPercent(20), -570) # clenches the claw
wheels.on_for_rotations(0, SpeedPercent(25),
0.5) # moves up to get the claw on the
front_claw.on_for_degrees(SpeedPercent(100), 570,
block=False) # opens the claw again
wheels.on_for_rotations(0, SpeedPercent(-100), 0.5)
wheels.on_for_rotations(0, SpeedPercent(100), 0.5)
wheels.on_for_rotations(0, SpeedPercent(-100), 0.5)
front_claw.on_for_degrees(SpeedPercent(20), -570)
wheels.on(0, SpeedPercent(-50))
while not touch_sensor.is_pressed:
pass
'''.format(left_cs.reflected_light_intensity,
center_cs.reflected_light_intensity,
right_cs.reflected_light_intensity))
if left_cs.reflected_light_intensity > 15 >= center_cs.reflected_light_intensity \
and right_cs.reflected_light_intensity > 15:
straight()
else:
if not turn_left() and not turn_right(
) and center_cs.reflected_light_intensity > 15:
conn.send(
'------------------------------ OFF THE LINE ------------------------------'
)
turn_right() if last_bl_cs is left_cs else turn_left(
) if last_bl_cs is right_cs else straight()
three = [sensor.driver_name
for sensor in list_sensors()].count('lego-ev3-color') == 3
logger_conn, motor_conn = Pipe(False)
p = Process(target=log_data, args=(logger_conn, logger_cs))
p.start()
while not btn.any():
follow_line_three(motor_conn) if three else follow_line(motor_conn)
motors.off()
motor_conn.send(None)
p.join()
print('Process joined')
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) # 스티어링 모드 설정 steer_pair.on(0,50) # 직진 스팅어링으로 속도 50% 무한 주행 sleep(1) steer_pair.off(brake=True) # 주행 멈춤 sleep(1) steer_pair.on_for_seconds(30,50, 2) # 좌회전 스팅어링으로 속도 50% 2초 주행 sleep(1) # 직진 스팅어링으로 속도 50% 반대방향 3회전 주행 steer_pair.on_for_rotations(0,50, -3) sleep(1) steer_pair.on_for_degrees(0,50, 180) # 직진 스팅어링으로 속도 50% 180도 주행 sleep(1)
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()
motorD = LargeMotor(OUTPUT_D)
motorC = LargeMotor(OUTPUT_C)
motorC.off(brake=True)
motorD.off(brake=True)
Constants.STOP = False
GyroDrift()
show_text("Robot Run 2")
acceleration(degrees=DistanceToDegree(70), finalSpeed=50, steering=2)
while colorLeft.reflected_light_intensity > 10 and False == Constants.STOP:
robot.on(steering=2, speed=20)
robot.off()
acceleration(degrees=DistanceToDegree(13), finalSpeed=20, steering=2)
while colorLeft.reflected_light_intensity < Constants.WHITE and False == Constants.STOP:
#robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
#print("RobotRun2 stop=" + str(Constants.STOP), file=stderr)
MoveForwardWhite(distanceInCm=2)
robot.on_for_degrees(degrees=DistanceToDegree(0.75), steering=2, speed=-10)
robot.off()
counter = 0
while counter < 5 and False == Constants.STOP:
robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
robot.on_for_degrees(degrees=DistanceToDegree(0.75), steering=2, speed=-10)
counter += 1
