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)
class EV:
def __init__(self):
self.button = Button()
self.tank = MoveSteering(OUTPUT_C, OUTPUT_B)
self.cs = ColorSensor()
self.cs.mode = ColorSensor.MODE_COL_REFLECT
self.gs = GyroSensor()
self.gs.reset()
self.before_direction = self.gyro()
def steer(self, steer, speed=SPEED, interval=INTERVAL):
"""
steer the motor by given params for time intarval [ms]
"""
if self.is_white(): # clockwise
self.tank.on_for_seconds(steer, speed, interval / 1000)
else:
self.tank.on_for_seconds(-steer, speed, interval / 1000)
data = (self._update_direction(), not self.is_white())
sleep(interval / 1000)
return data
def turn_degrees(self, radian):
self.tank.turn_degrees(SPEED, math.degrees(radian))
def on_for_millis(self, millis):
self.tank.on_for_seconds(0, SPEED, millis / 1000)
def gyro(self):
return self.gs.value()
def is_white(self):
return self.cs.value() > 30
# def _send(self, data):
# data = str(data).encode()
# self.socket.send(data)
# print(data)
def _update_direction(self):
current_direction = self.gyro()
m_direction = (current_direction + self.before_direction) / 2
self.before_direction = current_direction
return m_direction
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 Gyro(SimplePeriodicWorkerThread):
def __init__(self, logger=None):
self._logger = logger or logging.getLogger(__name__)
super().__init__(thread_name='EV3Gyro')
self._gyro = GyroSensor(address='in2')
self._gyro.mode = GyroSensor.MODE_GYRO_ANG
self._gyro.reset()
self._angle = 0
def perform_cycle(self):
# Occasionally, the EV3 gyro sensor gives some exeptions. Usually it happens
# for a few seconds after it is started. Maybe its initialization is not yet
# complete. According to experiments, we can ignore this.
try:
self._angle = self._gyro.angle
except:
self._logger.warning('Unable to get angle from EV3 gyro sensor')
def get_orientation(self):
# It is ok to read a little outdated data
return self._angle
def reset(self):
self._gyro.reset()
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()
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)
m.on_for_rotations(SpeedPercent(75), 5)
#hoi bois!!!!
def gyro_test():
m1 = LargeMotor(OUTPUT_A)
m2 = LargeMotor(OUTPUT_D)
m1.on(20)
m2.on(-20)
gyro.wait_until_angle_changed_by(90)
m1.off()
m2.off()
# Resets to 0, does not fix drift
gyro.reset()
def long_gyro_test():
s.beep()
show("push a button")
#b.wait_for_pressed([Button.enter, Button.right])
#b.wait_for_bump([Button.enter, Button.right])
b.wait_for_pressed(["enter"])
s.beep()
gyro_test()
#motor_test()
#for i in range(50):
# Loop until button pressed, displaying gyro angle
while not b.any():
ang = gyro.angle
import sys
import threading
from time import sleep
#netstat -nap | grep 5000 running program check
# fuser -k -n tcp 5000 port kill
sio = socketio.AsyncServer(async_mode='aiohttp',
ping_timeout=1500000,
ping_interval=600000)
app = web.Application()
sio.attach(app)
gy = GyroSensor(INPUT_2)
gy._ensure_mode(gy.MODE_GYRO_CAL)
gy._direct = gy.set_attr_raw(gy._direct, 'direct', bytes(17, ))
gy._ensure_mode(gy.MODE_GYRO_G_A)
gy.reset()
motors = MoveTank(OUTPUT_A, OUTPUT_D)
#motors.on(50,50)
sleep(0.2)
motors.off(brake=False)
@sio.event
async def connect(sid, environ):
#raise ConnectionRefusedError('authentication failed')
print('connected with Client', sid)
steps = 0
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Connect two large motors on output ports B and C
self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
self.grip = MediumMotor(OUTPUT_A)
self.sound = Sound()
self.leds = Leds()
self.ir = InfraredSensor()
self.touch = TouchSensor()
self.gyro = GyroSensor()
self.isComing = False
self.isTaking = False
self.isBringing = False
self.isTurning = False
# Start threads
threading.Thread(target=self._proximity_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(
self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
elif control_type == "come":
self._come()
elif control_type == "take":
self._take()
elif control_type == "bring":
self._bring()
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _come(self, duration=10, speed=50):
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
self.isComing = True
self._move(Direction.FORWARD.value[0], duration, speed)
def _take(self, duration=20, speed=50):
self.grip.on_for_rotations(SpeedPercent(100), 1)
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
self.gyro.reset()
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.isTurning = True
self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40)
self.isTaking = True
self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), duration)
def _bring(self, duration=20):
self.isTurning = True
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40)
self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), 1.5)
self.grip.on_for_rotations(SpeedPercent(100), 1)
self.isTurning = True
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40)
self.isBringing = True
self.now = time.time()
self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), duration)
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking),
file=sys.stderr)
if direction in Direction.FORWARD.value:
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_seconds(SpeedPercent(-speed),
SpeedPercent(-speed),
duration,
block=is_blocking)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
if direction in Direction.RIGHT.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _proximity_thread(self):
"""
Monitors the distance between the robot and an obstacle when sentry mode is activated.
If the minimum distance is breached, send a custom event to trigger action on
the Alexa skill.
"""
while True:
distance = self.ir.proximity
angle = self.gyro.angle
#print("Proximity: {}".format(distance), file=sys.stderr)
if self.isTurning == True:
print("angle: {}".format(angle), file=sys.stderr)
self.leds.set_color("LEFT", "YELLOW", 1)
self.leds.set_color("RIGHT", "YELLOW", 1)
if abs(angle) >= 179 or abs(angle) <= -181:
self.isTurning = False
self._move(
Direction.STOP.value[0],
0,
0,
)
self.gyro.reset()
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
if distance <= 55:
"""
When the bot is coming, it will stop and open up it's arm
"""
if self.isComing == True:
self.isComing = False
print("Proximity breached, stopping")
self._move(
Direction.STOP.value[0],
0,
0,
)
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
while not self.touch.is_pressed:
self.grip.on_for_degrees(SpeedPercent(10), -90)
print("Lowered the grip")
elif self.isTaking == True:
self.isTaking = False
print("Proximity breached, stopping")
self._move(
Direction.STOP.value[0],
0,
0,
)
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
while not self.touch.is_pressed:
self.grip.on_for_degrees(SpeedPercent(10), -90)
print("Dropping Item")
self.drive.on_for_seconds(SpeedPercent(-50),
SpeedPercent(-50), 1)
self.gyro.reset()
self.isTurning = True
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.drive.on_for_seconds(SpeedPercent(100),
SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4),
SpeedPercent(-4),
40,
block=False)
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
elif self.isBringing == True:
self.isBringing = False
print("Proximity breached, stopping")
self._move(Direction.STOP.value[0], 0, 0)
self.later = time.time()
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
while not self.touch.is_pressed:
self.grip.on_for_degrees(SpeedPercent(10), -90)
print("Dropping Item")
difference = int(self.later - self.now)
self.drive.on_for_seconds(SpeedPercent(-50),
SpeedPercent(-50),
difference - 0.5)
time.sleep(0.2)
class CarControl:
def __init__(self):
self.tank_pair = MoveTank(OUTPUT_A, OUTPUT_B)
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
self.line_sensor = ColorSensor(INPUT_2)
self.line_counter = ColorSensor(INPUT_3)
self.sound = Sound()
self.gyro = GyroSensor(INPUT_1)
self.gyro.reset()
self.dir_state = "S"
def drive(self, l_speed, r_speed):
self.tank_pair.on(left_speed=l_speed, right_speed=r_speed)
def reverse(self):
self.tank_pair.on_for_degrees(left_speed=-90,
right_speed=-90,
degrees=360,
brake=True)
def brake(self):
self.tank_pair.off(brake=True)
def turn_left(self):
self.steer_pair.on_for_degrees(steering=-100,
speed=90,
degrees=180,
brake=True)
def turn_right(self):
self.steer_pair.on_for_degrees(steering=100,
speed=90,
degrees=170,
brake=True)
def turn_arround(self):
self.steer_pair.on_for_degrees(steering=-100,
speed=90,
degrees=370,
brake=True)
def read_line_sensor(self):
return (self.line_sensor.reflected_light_intensity)
def read_line_counter(self):
return (self.line_counter.reflected_light_intensity)
def play_sound(self):
self.sound.beep()
def read_gyro(self):
return self.gyro.angle
def turn_left_new(self):
self.tank_pair.on(left_speed=-90, right_speed=90)
if self.dir_state == "S":
while True:
if self.read_gyro(
) <= -90: #Ikke sikker på at den altid kan læse ved præcist -90
self.tank_pair.off(brake=True)
self.dir_state = "L"
return 0
elif self.dir_state == "L":
while True:
if self.read_gyro() == 180 or self.read_gyro() == -180:
self.steer_pair.stop()
self.dir_state = "B"
return 0
elif self.dir_state == "B":
while True:
if self.read_gyro() == 90 or self.read_gyro() == -270:
self.steer_pair.stop()
self.dir_state = "R"
return 0
elif self.dir_state == "R":
while True:
if self.read_gyro(
) == 0 or self.read_gyro == -360 or self.read_gyro == 360:
self.gyro.reset()
self.steer_pair.stop()
self.dir_state = "S"
return 0
def turn_right_new(self):
self.steer_pair.on(90, 90)
print("JEG ER HER")
if self.dir_state == "S":
while True:
if self.read_gyro == 90:
self.steer_pair.stop()
self.dir_state = "R"
return 0
elif self.dir_state == "L":
while True:
if self.read_gyro() == 360 or self.read_gyro() == 0:
self.gyro.reset()
self.steer_pair.stop()
self.dir_state = "S"
return 0
elif self.dir_state == "B":
while True:
if self.read_gyro() == 270 or self.read_gyro == -90:
self.steer_pair.stop()
self.dir_state == "R"
return 0
elif self.dir_state == "R":
while True:
if self.read_gyro() == 180 or self.read_gyro() == -180:
self.steer_pair.stop()
self.dir_state == "B"
return 0
class SimpleRobotDriveBase(MoveDifferential):
''' A simplified drive base class.
Args:
leftDriveMotor: motor port for left side drive motor
rightDriveMotor: motor port for right side drive motor
tire: tire to use
axle_track: distance between centerline of drive wheels (mm)
leftMotor: port for left side attachment motor
rightMotor: port for right side attachment motor
leftColor: sensor port for left side color sensor
rightColor: sensor port for right side color sensor
centerColor: sensor port for center color sensor
useGyro (bool): enable gyro functions
'''
def __init__(self,
leftDriveMotor,
rightDriveMotor,
tire,
axle_track,
leftMotor,
rightMotor,
leftColor,
rightColor,
centerColor,
useGyro=False):
''' Initialize the SimpleRobotDriveBase.
'''
self.defaultSpeed = 200 # mm/s
self.defaultTurnTime = 2 # time to complete turn in place (s)
self.blackThreshold = 10 # values less than this are black
self.whiteThreshold = 55 # values more than this are white
self.defaultTimeout = 15000
# Set up two drive motors
self.leftDriveMotor = leftDriveMotor
self.rightDriveMotor = rightDriveMotor
# set up output shafts
self.leftMotor = leftMotor
self.rightMotor = rightMotor
# aliases for output shafts when using XY table
self.xMotor = self.leftMotor
self.yMotor = self.rightMotor
# set up color sensors
self.leftColor = leftColor
self.rightColor = rightColor
self.centerColor = centerColor
# set up gryo sensor
if useGyro:
self.gyro = GyroSensor()
self.gyro.reset()
# store wheel diameter
self.wheel_diameter = tire().diameter_mm
# initialize the parent class: MoveDifferential
super().__init__(leftDriveMotor.address, rightDriveMotor.address, tire,
axle_track)
# # Invert motor polarities: positive drives CCW
# rightDriveMotor.polarity = "inversed"
# leftDriveMotor.polarity = "inversed"
# set the ratio of motor angle to linear motion (deg/mm)
self.xRatio = 2392 / 144 # deg/mm
self.yRatio = 1695 / 80 # deg/mm
# set time to accelerate or deccelerate to 100% speed (ms)
self.ramp_time = 1000
self.leftDriveMotor.ramp_up_sp = self.ramp_time
self.leftDriveMotor.ramp_down_sp = self.ramp_time
self.rightDriveMotor.ramp_up_sp = self.ramp_time
self.rightDriveMotor.ramp_down_sp = self.ramp_time
self.leftMotor.ramp_up_sp = self.ramp_time
self.leftMotor.ramp_down_sp = self.ramp_time
self.rightMotor.ramp_up_sp = self.ramp_time
self.rightMotor.ramp_down_sp = self.ramp_time
def speed_mm_s(self, speed):
''' Convert speed in mm/s into percent of max speed.
Args:
speed: speed in mm/s.
Returns:
speed in SpeedRPS form. '''
rps = speed / (pi * self.wheel_diameter)
speed = SpeedRPS(rps)
return speed
def driveArc(self,
distance,
radius,
speed=200,
direction=Direction.CW,
brake=True,
block=True):
''' Drive for given distance along arc of specified radius.
Args:
distance: distance to drive in mm
radius: radius of arc in mm
speed: speed in mm/s
direction: direction to travel along arc -- Direction.CW or Direction.CCW
brake (bool): brake at end of move
block (bool): block until move is complete
'''
speed = self.speed_mm_s(speed)
self.enableRampDown(brake)
if direction == Direction.CW:
self.on_arc_right(speed, radius, distance, brake, block)
if direction == Direction.CCW:
self.on_arc_left(speed, radius, distance, brake, block)
self.enableRampDown(True)
def pointTurn(self, degrees_to_turn, speed=8, brake=True, block=True):
''' Turn in place.
Args:
degrees_to_turn: degrees to turn (positive to right)
speed: percentage of maximum speed
brake (bool): brake at end of move
block (bool): block until move is complete
'''
self.enableRampDown(brake)
if degrees_to_turn >= 0:
self.turn_right(speed, degrees_to_turn, brake, block)
else:
self.turn_left(speed, degrees_to_turn, brake, block)
self.enableRampDown(True)
def pointTurnGyro(self, degrees_to_turn, speed=10, brake=True, block=True):
''' Turn in place using gyro sensor.
Args:
degrees_to_turn: degrees to turn (positive to right)
speed: percentage of maximum speed
brake (bool): brake at end of move
block (bool): block until move is complete (unused for now)
'''
startAngle = self.gyro.angle
# start rotation
if degrees_to_turn >= 0:
self.turn_right(speed, 5000, brake, False)
else:
self.turn_left(speed, 5000, brake, False)
# loop until we hit target angle
while self.gyro.angle - startAngle < degrees_to_turn:
sleep(.01)
# stop both motors
self.stop(brake=brake)
def enableRampDown(self, enable):
''' Enable or disable the motor decceleration for drive motors.
Args:
enable (bool): Enable deccleration if True
'''
if enable:
self.leftDriveMotor.ramp_down_sp = self.ramp_time
self.rightDriveMotor.ramp_down_sp = self.ramp_time
else:
self.leftDriveMotor.ramp_down_sp = 0
self.rightDriveMotor.ramp_down_sp = 0
def driveStraight(self, distance, speed=None, brake=True, block=True):
''' Drive for given distance.
Args:
distance: distance to drive in mm
speed: speed in mm/s. Uses default speed if not specified.
brake (bool): brake at end of movement
block (bool): block until move is complete
'''
if speed == None or speed == 0:
speed = self.defaultSpeed
speed = self.speed_mm_s(speed)
self.enableRampDown(brake)
self.on_for_distance(speed, distance, brake, block)
self.enableRampDown(True)
def driveStraightForTime(self,
time,
speedLeft=None,
speedRight=None,
brake=True,
block=True):
''' Drive for given time.
Args:
time: time for move in s
speedLeft: speed in mm/s (left motor)
speedRight: speed in mm/s (right motor)
brake (bool): brake at end of movement
block (bool): block until move is complete
'''
if speedLeft == None or speedLeft == 0:
speedLeft = self.defaultSpeed
speedLeft = self.speed_mm_s(speedLeft)
if speedRight == None or speedRight == 0:
speedRight = self.defaultSpeed
speedRight = self.speed_mm_s(speedRight)
self.enableRampDown(brake)
self.on_for_seconds(speedLeft, speedRight, time, brake, block)
self.enableRampDown(True)
def driveStraightForDistance(self,
distance,
speedLeft=None,
speedRight=None,
stopAtEnd=True):
''' Drive for given distance.
Args:
distance: distance to drive in mm
speedLeft: speed in mm/s (left motor)
speedRight: speed in mm/s (right motor)
stopAtEnd (bool): brake at end of movement
'''
if speedLeft == None or speedLeft == 0:
speedLeft = self.defaultSpeed
if speedRight == None or speedRight == 0:
speedRight = self.defaultSpeed
degPerSecLeft = 360 * speedLeft / (pi * self.wheel_diameter)
degPerSecRight = 360 * speedRight / (pi * self.wheel_diameter)
driveTime = StopWatch()
driveTime.start()
self.leftDriveMotor.on(SpeedNativeUnits(degPerSecLeft), block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSecRight), block=False)
while driveTime.value_ms <= abs(distance / speedLeft) * 1000:
sleep(0.02)
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
def driveStraightGyro(self, distance, speed=None, brake=False):
''' Drive for given distance using gyro sensor.
Robot goes in straight line in the direction it was facing when this
function is called. Uses proportional control.
Args:
distance: distance to drive in mm
speed: speed in mm/s
brake (bool): brake at end of movement
'''
# starting angle for this move
startAngle = self.gyro.angle
if speed == None or speed == 0:
speed = self.defaultSpeed
if distance < 0:
distance = abs(distance)
speed = -speed
assert (
distance > 0 or speed > 0
), "At least one of distance or speed must be posiitve in driveStraightGyro."
# calculate motor speeds in percentage units
rps = speed / (pi * self.wheel_diameter)
left_speed = rps
right_speed = rps
# set encoder counts to zero
self.rightDriveMotor.position = 0
# proportional gain (rps/deg)
kp = 0.01
# number of wheel rotations in this move
targetRotations = distance / (pi * self.wheel_diameter)
while abs(self.rightDriveMotor.rotations) < targetRotations:
error = self.gyro.angle - startAngle
left_speed = SpeedRPS(rps - kp * error)
right_speed = SpeedRPS(rps + kp * error)
self.on(left_speed, right_speed)
sleep(0.01)
# stop both motors
self.stop(brake=brake)
def followLineDistNative(self,
distance,
sensor,
sideToFollow,
stopAtEnd,
speed,
gain_mult=1,
brake=False):
''' Drive forward following line.
Args:
distance: distance to follow line in mm
sensor: colorsensor to use for line following
sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT
stopAtEnd (bool): stop at end of movement
speed: speed in mm/s
gain_mult: multiplier for P and D gains
brake (bool): brake at end of move
'''
# calculate the target reflectance, halfway between black ad white
target = (self.blackThreshold + self.whiteThreshold) / 2
# set parameters for which side to follow
if sideToFollow == LineEdge.LEFT:
followLeft = True
else:
followLeft = False
# calculate the required motor rotation rate in RPS
rotationRate = speed / (pi * self.wheel_diameter)
# time to follow line
timeToFollow_ms = abs(distance / speed * 1000)
# set which color sensor to use
self.cs = sensor
# call line follower built-in function
self.follow_line(kp=5 * gain_mult,
ki=0 * gain_mult,
kd=1 * gain_mult,
speed=SpeedRPS(rotationRate),
target_light_intensity=target,
follow_left_edge=followLeft,
white=self.whiteThreshold,
off_line_count_max=100,
sleep_time=0.01,
follow_for=follow_for_ms,
ms=timeToFollow_ms)
self.stop(brake=brake)
def followLineDist(self,
distance,
sensor,
sideToFollow,
stopAtEnd,
speed,
gain_multiplier=1):
''' Drive forward following line.
Uses direct control of drive motors and PD control.
Args:
distance: distance to follow line in mm
sensor: colorsensor to use for line following
sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT
stopAtEnd (bool): Stop motors at end of line following
speed: speed in mm/s
gain_multiplier: multiplier for P and D gains
'''
degPerSec = 360 * speed / (pi * self.wheel_diameter)
propGain = 6.0 * gain_multiplier
derivGain = 6 * gain_multiplier
target = (self.blackThreshold + self.whiteThreshold) / 2
print("******* starting line following ********")
# print("error,Pterm,Dterm")
# initialize term for derivative calc
previousError = target - avgReflection(sensor, 2)
i = 0
driveTime = StopWatch()
driveTime.start()
while driveTime.value_ms <= abs(distance / speed) * 1000:
# calc error and proportional term
error = target - avgReflection(sensor, 2)
Pterm = propGain * error
# calc d(error)/d(t) and derivative term
d_error_dt = error - previousError
Dterm = derivGain * d_error_dt
previousError = error
if sideToFollow == LineEdge.RIGHT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
if sideToFollow == LineEdge.LEFT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
# eprint("line following complete")
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
# Play sound when we stop line following
sound.beep()
def homeXY(self, speed=60):
''' Move xy table to home position.
Args:
speed: linear speed of the axes (mm/s)
'''
# self.xMotor.set_dc_settings(80, 0)
self.xMotor.position = 0
self.yMotor.position = 0
self.gotoXY(-200, -200, speed)
self.xMotor.position = 0
self.yMotor.position = 0
# self.xMotor.set_dc_settings(100, 0)
def homeXY2(self, speed=60):
''' Move xy table to home position.
Args:
speed: linear speed of the axes (mm/s)
'''
eprint("starting homeXY")
# run motors at fixed duty cycle
self.yMotor.duty_cycle_sp = -90
self.xMotor.run_direct(duty_cycle_sp=-50)
self.yMotor.run_direct(duty_cycle_sp=-80)
# sleep until motors stop (or timeout is reached)
xExitType = self.xMotor.wait_until_not_moving(self.defaultTimeout)
yExitType = self.yMotor.wait_until_not_moving(self.defaultTimeout)
# set xMotor and yMotor encoders to zero
self.xMotor.position = 0
self.yMotor.position = 0
eprint("HomeXY complete.", xExitType, yExitType)
def gotoXY(self, x, y, speed=30, block=True):
''' Move to absolute position (x,y).
moves axes simultaneously.
homeXY must be called first.
Args:
x: position of the x-axis (mm)
y: position of the y-axis (mm)
speed: the speed of the axes (mm/s)
block (bool): block until move is complete
'''
# start move to x position
self.xMotor.on_to_position(SpeedNativeUnits(speed * self.xRatio),
x * self.xRatio,
brake=False,
block=False)
# start move to y position
self.yMotor.on_to_position(SpeedNativeUnits(speed * self.yRatio),
y * self.yRatio,
brake=False,
block=False)
if block:
# sleep until either move is complete or motors stall
self.xMotor.wait_until_not_moving(self.defaultTimeout)
self.yMotor.wait_until_not_moving(self.defaultTimeout)
def incrementXY(self, dx, dy, speed=30):
''' Move xy table by a given amount.
Moves axes simultaneously.
Args:
dx: amount to move x-axis (mm)
dy: amount to move y-axis (mm)
speed: the speed of the axes (mm/s)
'''
# get current table positions in mm from home position
x = self.xMotor.position / self.xRatio
y = self.yMotor.position / self.yRatio
eprint("XY table position", x, y)
self.gotoXY(x + dx, y + dy, speed)
def squareToLine(self):
''' Position robot perpendicular to a line.
'''
# drive forward until one sensor is on edge of line.
firstSensor = self.stopOnLine()
sound.beep()
target = (self.blackThreshold + self.whiteThreshold) / 2
# target = self.blackThreshold
targetTolerance = 2 # how close we need to be to target reflectance
leds.set_color('LEFT', 'RED')
# drive opposite wheel forward to square robot on line
if firstSensor == self.leftColor: # left sensor on line
eprint("found left sensor first")
# start right wheel rotating
self.rightDriveMotor.speed_sp = 30
self.rightDriveMotor.run_forever()
self.leftDriveMotor.speed_sp = -30
self.leftDriveMotor.run_forever()
# drive slowly forward until sensor hits white
while True:
if avgReflection(self.rightColor) >= self.whiteThreshold:
# found a white line
eprint("found white")
break
sleep(0.010)
while True:
if abs(avgReflection(self.rightColor) -
target) < targetTolerance:
# we are centered over edge of line
self.rightDriveMotor.stop()
self.leftDriveMotor.stop()
eprint("found line edge")
break
sleep(0.010)
else: # right sensor is on line
# start left wheel rotating
eprint("found right sensor first")
self.leftDriveMotor.on(30)
while True:
if avgReflection(self.leftColor) >= self.whiteThreshold:
# found a white line
break
while True:
if abs(avgReflection(self.leftColor) -
target) < targetTolerance:
# we are centered over edge of line
self.leftDriveMotor.stop()
break
sleep(0.010)
# set brick light when we are done
leds.set_color('LEFT', 'GREEN')
# play long beep
sound.beep()
eprint(avgReflection(self.rightColor),
avgReflection(self.leftColor, 5))
def stopOnLine(self, driveForward=True, brake=False):
''' Stop when one sensor is centered over edge of a line.
Drives slowly to line.
Args:
driveForward (bool): drive in forward direction
brake (bool): brake at end of move
Returns:
firstSensor: the sensor that hit line first
'''
# calculate the target sensor reading: halfway between black and white
target = (self.blackThreshold + self.whiteThreshold) / 2
# set brick light while we are seeking white
leds.set_color('LEFT', 'RED')
if driveForward:
# drive slowly until at least one sensor hits white
self.on(left_speed=5, right_speed=5) # speeds in percent
else:
# drive slowly until at least one sensor hits white
self.on(left_speed=-5, right_speed=-5) # speeds in percent
while True:
if avgReflection(self.leftColor) >= self.whiteThreshold:
# found a white line
break
if avgReflection(self.rightColor) >= self.whiteThreshold:
# found a white line
break
sleep(0.010)
# set brick light while we are seeking the line
leds.set_color('LEFT', 'ORANGE')
targetTolerance = 5 # how close we need to be to target reflectance
# continue driving until we see the target reflectance
while True:
if abs(avgReflection(self.leftColor) - target) < targetTolerance:
# we are centered over edge of line
firstSensor = self.leftColor # left sensor hit first
eprint("left sensor hit first in stop on line")
break
if abs(avgReflection(self.rightColor) - target) < targetTolerance:
# we are centered over edge of line
firstSensor = self.rightColor # right sensor hit first
eprint("right sensor hit first in stop on line")
break
sleep(0.010)
# stop moving
self.stop(brake=brake)
return (firstSensor)
def stopOnLineOneSensor(self, sensor, driveForward=False, brake=False):
''' Stop when given sensor is centered over edge of a line.
Drives slowly to line.
Args:
sensor: Color sensor to use
Drive forward (bool): weather to drive forward
brake (bool): brake at end of move
Returns:
distanceTravelled: distance travelled in mm
'''
# calculate the target sensor reading: halfway between black and white
target = (self.blackThreshold + self.whiteThreshold) / 2
# set brick light while we are seeking white
leds.set_color('LEFT', 'RED')
# set encoder counts to zero
self.rightDriveMotor.position = 0
if driveForward:
# drive slowly until at least one sensor hits white
self.on(left_speed=15, right_speed=15) # speeds in percent
else:
# drive slowly until at least one sensor hits white
self.on(left_speed=-15, right_speed=-15) # speeds in percent
while avgReflection(sensor) < self.whiteThreshold:
sleep(0.010)
targetTolerance = 5 # how close we need to be to target reflectance
leds.set_color('LEFT', 'ORANGE')
# continue driving until we see the target reflectance
while abs(avgReflection(sensor) - target) > targetTolerance:
sleep(0.010)
# stop moving
self.stop(brake=brake)
leds.set_color('LEFT', 'GREEN')
distanceTravelled = self.rightDriveMotor.rotations * self.wheel_diameter * pi
eprint("Distance travelled:", distanceTravelled)
return distanceTravelled