def test_single_motor(output):
motor = LargeMotor(output)
# motor.command = LargeMotor.COMMAND_RUN_FOREVER
# for command in motor.commands:
# print('Motor at {} set to {}'.format(output, command))
# motor.command = command
motor.on_for_seconds(SpeedPercent(100), 5)
# print_and_wait(motor)
motor.on_for_rotations(SpeedPercent(30), 0.5)
# print_and_wait(motor)
motor.on_for_degrees(SpeedPercent(30), 45)
# print_and_wait(motor)
motor.on_to_position(SpeedPercent(30), 5)
class ClawThread(threading.Thread):
def __init__(self):
self.claw = LargeMotor(OUTPUT_D)
threading.Thread.__init__(self)
def run(self):
closed = False
while running:
if close_jaw and not self.claw.is_overloaded and not closed:
closed = True
self.claw.on_for_degrees(30, 5 * 360)
elif not close_jaw and not self.claw.is_overloaded and closed:
closed = False
self.claw.on_for_degrees(-30, 2 * 360)
else:
self.claw.off()
self.claw.off()
class CarEngine(SteeringWheel):
"""
Move Bot forward and backward
"""
def __init__(self):
SteeringWheel.__init__(self)
self.car_engine = LargeMotor(OUTPUT_C)
def move_forward(self, degrees=90):
self.leds.set_color("LEFT", "ORANGE")
self.car_engine.on_for_degrees(SpeedPercent(30), degrees, brake=False)
self.leds.set_color("LEFT", "BLACK")
def move_backward(self):
self.leds.set_color("RIGHT", "ORANGE")
self.car_engine.on_for_degrees(SpeedPercent(20), -90, brake=False)
self.leds.set_color("RIGHT", "BLACK")
class RobotHead(rpyc.Service):
ALIASES = ['Head']
def __init__(self, *args, **kwargs):
self.exposed_support_tilt = LargeMotor(OUTPUT_A)
self.exposed_support_power = LargeMotor(OUTPUT_B)
self.exposed_right_us = UltrasonicSensor(INPUT_1)
self.exposed_left_us = UltrasonicSensor(INPUT_2)
self.exposed_drop_us = UltrasonicSensor(INPUT_3)
self.exposed_rear_us = UltrasonicSensor(INPUT_4)
super().__init__(*args, **kwargs)
def exposed_lower_support(self):
self.exposed_support_tilt.on(-100)
while self.exposed_rear_us.distance_centimeters > 5:
time.sleep(0.01)
self.exposed_support_tilt.on(0, True)
def exposed_raise_support(self):
self.exposed_support_tilt.on_for_degrees(25, 540)
def manobra_cano(cor_do_cano):
if cor_do_cano == 'azul': c = 20
if cor_do_cano == 'amarelo': c = 10
if cor_do_cano == 'vermelho': c = 15
q = 13.5 - (c / 2)
steering_pair.on_for_degrees(0, 10, 30 * q)
lm = LargeMotor(ent_motor_esq)
lm.on_for_degrees(20, -10 * 90 / 2.3)
if distancia_do_corre(usf) > 45:
lm.on_for_degrees(10, 10 * 90 / 2.3)
achar_cano()
q = 13.5 - (c / 2)
steering_pair.on_for_degrees(0, 10, 30 * q)
lm.on_for_degrees(10, -10 * 90 / 2.3)
while (distancia_do_corre(usf) > 7):
steering_pair.on(0, 15)
else:
steering_pair.off()
steering_pair.on_for_seconds(0, 15, 1)
class Carriage:
"""Carriage class witch allow managing the position. """
def __init__(self, power=30):
_debug(self, "Creating Carriage instance")
self._default_power = power
self._delta = 0
self._m = LargeMotor(OUTPUT_B)
self.reset()
def reset(self, power=None):
"""Reset the carriage position and define a new origin for carriage's position
:param power: Power used to move the carriage
"""
if power is None:
power = self._default_power
self.right_limit(power=power, soft_limit=False)
self._m.on_for_degrees(power, 50)
self._delta = self._m.position
self.go_to(0, power)
def left(self, power=None):
"""Move carriage (pen will move too) to the 'left'
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
self._m.on(power)
def right(self, power=None):
"""Move carriage (pen will move too) to the 'right'
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
self._m.on(-power)
def stop(self):
""" Stop moving carriage
"""
self._m.off()
@property
def position(self):
""" Get carriage position
:return: carriage position
"""
return self._m.position - self._delta
@position.setter
def position(self, value):
""" Set carriage position
:param value: Reached position"""
self.go_to(value)
def go_to(self, position, power=None, block=True, override=False):
"""Move carriage to absolute position `position`
:param position: Position reached
:param power: Power used to move
:param block: If True, fonction will end at the end of the rotation
:param override: if true, bypass limits"""
if power is None:
power = self._default_power
_debug(self, "Reached position is {}".format(position))
if (not override) and (not -50 <= position <= 1240):
raise ValueError("Position is out of reachable bounds.")
self._m.on_to_position(power, position + self._delta, block=block)
def move(self, value, power=None, block=True, override=False):
"""Move carriage of `value` degrees
:param value: Position reached
:param power: Power used to move
:param block: If True, fonction will end at the end of the rotation
:param override: if true, bypass limits"""
if power is None:
power = self._default_power
self.go_to(self.position + value, power, block, override)
def save_energy(self):
"""Save energy and release motor's holding state"""
self._m.off(False)
def right_limit(self, soft_limit=True, power=None):
"""Go to the right limit
:param soft_limit: If True, carriage will move backward to avoid motor forcing
:param power: power used to move"""
if power is None:
power = self._default_power
self.right(power)
self._m.wait_until_not_moving()
self.stop()
if soft_limit:
self.move(50, power)
def left_limit(self, soft_limit=True, power=None):
"""Go to the left limit
:param soft_limit: If True, carriage will move backward to avoid motor forcing
:param power: power used to move"""
if power is None:
power = self._default_power
self.left(power)
self._m.wait_until_not_moving()
self.stop()
if soft_limit:
self.move(-50, power)
@property
def default_power(self):
return self._default_power
@default_power.setter
def default_power(self, value):
self._default_power = value
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.drive = LargeMotor(OUTPUT_B)
self.dealmotor = MediumMotor(OUTPUT_A)
self.bcolor = ColorSensor(INPUT_1)
self.pushsensor = TouchSensor(INPUT_2)
self.leftmargin = 0
self.rigtmargin = 0
self._init_reset()
def _init_reset(self):
self.numCards = 2
self.numPlayers = 0
self.game = 'blackjack'
self.degreeStep = 180
self.players = ["red","yellow"] #Default player
self._send_event(EventName.SPEECH, {'speechOut': "Restart game"})
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"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
if control_type == "dealcard":
# Expected params: [command] = Number of Cards
# Expected params: [player]
num = payload["command"]
player = payload["player"]
speak = "Give "+ num + " card to " + player
if player == "all":
if (self.numPlayers == 0):
self.numPlayers = 2
for i in range(int(num)):
for j in range(self.numPlayers):
self._dealcard(1,j)
else:
try:
num = self.players.index(player)
self._dealcard(int(payload["command"]),num)
except ValueError:
speak = "There is no user " + player
print (speak,file=sys.stderr)
self._send_event(EventName.SPEECH, {'speechOut': speak})
except KeyError:
print("Missing expected parameters: {}".format(directive), file=sys.stderr)
def _dealcard(self, num, player):
"""
Deal number of cards to player
"""
if num < 1:
num = 1
degree = self._calcDegree(player)
print("Give player : {} card : {} Move angle : {}".format(player, num,degree), file=sys.stderr)
self.drive.on_to_position(SpeedPercent(10),degree)
self.drive.wait_until_not_moving()
for i in range(num):
self.dealmotor.on_for_degrees(SpeedPercent(-100), 340)
self.dealmotor.wait_until_not_moving()
time.sleep(1)
self.dealmotor.on_for_degrees(SpeedPercent(100), 270)
self.dealmotor.wait_until_not_moving()
time.sleep(1)
def _calcDegree (self,player):
degree = (player*self.degreeStep)+self.leftmargin
return degree
def _gameinit(self,game):
"""
Check and start game
"""
if (self.numPlayers == 0):
self.numPlayers = 2
if game == "poker":
self.numCards = 5
if game == "blackjack":
self.numCards = 2
if game == "rummy":
self.numCards = 7
if (self.numPlayers == 2):
self.numCards = 10
speak = ""
for i in range(self.numPlayers):
print("Player : {} Color : {}".format(i,self.players[i]), file=sys.stderr)
speak = speak + self.players[i]
speak = "Start " + game + "with " + str(self.numPlayers) + "player " + speak
self._send_event(EventName.SPEECH, {'speechOut': speak})
self._findboundary()
self.drive.on_to_position(SpeedPercent(10),self.leftmargin)
time.sleep(1)
print("Game : {} Number of Card : {}".format(game,self.numCards), file=sys.stderr)
for i in range(self.numCards):
for j in range(self.numPlayers):
self._dealcard(1,j)
def _findboundary (self):
"Move to left until sensor pressed "
self.drive.on(SpeedPercent(10))
self.pushsensor.wait_for_pressed ()
self.drive.stop()
"Get position"
self.rightmargin = self.drive.position
print("Right position : {} ".format(self.rightmargin), file=sys.stderr)
self.drive.on(SpeedPercent(-10))
time.sleep(1)
self.pushsensor.wait_for_pressed ()
self.drive.stop()
"Get position + offset 45 for not to close limitation"
self.leftmargin = self.drive.position
self.leftmargin = self.leftmargin + 60
print("Left position : {} ".format(self.leftmargin), file=sys.stderr)
self.degreeStep = int(abs((self.leftmargin - self.rightmargin)/self.numPlayers))
print("Degree steps : {} ".format(self.degreeStep), file=sys.stderr)
def _addUser (self):
if self.numPlayers == 0:
self.players.clear()
player = self.bcolor.color_name
self.players.append(player.lower())
print("Player {} color: {}".format(self.players[self.numPlayers],player), file=sys.stderr)
self.numPlayers += 1
self._send_event(EventName.PLAYER, {'player': player})
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_degrees(SpeedPercent(10),90)
if direction in Direction.BACKWARD.value:
self.drive.on_for_degrees(SpeedPercent(-10),90)
if direction in Direction.STOP.value:
self.drive.off()
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 _activate(self, command, speed=50):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed), file=sys.stderr)
if command in Command.GAMES.value:
self.game = command
print("Play game: {}".format(self.game), file=sys.stderr)
self._gameinit(self.game)
if command in Command.RESET_GAME.value:
# Reset game
self._init_reset()
if command in Command.ADD_CMD.value:
self._addUser()
def test_motor_on_for_degrees(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA')])
m = LargeMotor()
# simple case
m.on_for_degrees(75, 100)
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
self.assertEqual(m.position_sp, 100)
# various negative cases; values act like multiplication
m.on_for_degrees(-75, 100)
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
self.assertEqual(m.position_sp, -100)
m.on_for_degrees(75, -100)
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
self.assertEqual(m.position_sp, -100)
m.on_for_degrees(-75, -100)
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
self.assertEqual(m.position_sp, 100)
# zero speed (on-device, this will return immediately due to reported stall)
m.on_for_degrees(0, 100)
self.assertEqual(m.speed_sp, 0)
self.assertEqual(m.position_sp, 100)
# zero distance
m.on_for_degrees(75, 0)
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
self.assertEqual(m.position_sp, 0)
# zero speed and distance
m.on_for_degrees(0, 0)
self.assertEqual(m.speed_sp, 0)
self.assertEqual(m.position_sp, 0)
class MindstormsGadget(AlexaGadget):
def __init__(self):
super().__init__(gadget_config_path='./auth.ini')
# order queue
self.queue = Queue()
self.button = Button()
self.leds = Leds()
self.sound = Sound()
self.console = Console()
self.console.set_font("Lat15-TerminusBold16.psf.gz", True)
self.dispense_motor = LargeMotor(OUTPUT_A)
self.pump_motor = LargeMotor(OUTPUT_B)
self.touch_sensor = TouchSensor(INPUT_1)
# Start threads
threading.Thread(target=self._handle_queue, daemon=True).start()
threading.Thread(target=self._test, daemon=True).start()
def on_connected(self, device_addr):
self.leds.animate_rainbow(duration=3, block=False)
self.sound.play_song((('C4', 'e3'), ('C5', 'e3')))
def on_disconnected(self, device_addr):
self.leds.animate_police_lights('RED',
'ORANGE',
duration=3,
block=False)
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
self.sound.play_song((('C5', 'e3'), ('C4', 'e3')))
def _test(self):
while 1:
self.button.wait_for_pressed('up')
order = {
'name': 'Test',
'tea': 'Jasmine',
'sugar': 100,
'ice': 100
}
self.queue.put(order)
sleep(1)
def _handle_queue(self):
while 1:
if self.queue.empty(): continue
order = self.queue.get()
self._make(name=order['name'],
tea=order['tea'],
sugar=order['sugar'],
ice=order['ice'])
def _send_event(self, name, payload):
self.send_custom_event('Custom.Mindstorms.Gadget', name, payload)
def _affirm_receive(self):
self.leds.animate_flash('GREEN',
sleeptime=0.25,
duration=0.5,
block=False)
self.sound.play_song((('C3', 'e3'), ('C3', 'e3')))
def on_custom_mindstorms_gadget_control(self, directive):
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
# regular voice commands
if control_type == "automatic":
self._affirm_receive()
order = {
"name": payload["name"] or "Anonymous",
"tea": payload["tea"] or "Jasmine Milk Tea",
"sugar": payload["sugar"] or 100,
"ice": payload["ice"] or 100,
}
self.queue.put(order)
# series of voice commands
elif control_type == "manual": # Expected params: [command]
control_command = payload["command"]
if control_command == "dispense":
self._affirm_receive()
if payload['num']:
self._dispense(payload['num'])
else:
self._dispense()
elif control_command == "pour":
self._affirm_receive()
if payload['num']:
self._pour(payload['num'])
else:
self._pour()
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _make(self, name=None, tea="Jasmine Milk Tea", sugar=100, ice=100):
if not self.touch_sensor.is_pressed:
# cup is not in place
self._send_event('CUP', None)
self.touch_sensor.wait_for_pressed()
sleep(3) # cup enter delay
# mid_col = console.columns // 2
# mid_row = console.rows // 2
# mid_col = 1
# mid_row = 1
# alignment = "L"
process = self.sound.play_file('mega.wav', 100,
Sound.PLAY_NO_WAIT_FOR_COMPLETE)
# dispense boba
self._dispense()
# dispense liquid
self._pour(tea=tea)
# self.console.text_at(
# s, column=mid_col, row=mid_row, alignment=alignment, reset_console=True
# )
# notify alexa that drink is finished
payload = {
"name": name,
"tea": tea,
"sugar": sugar,
"ice": ice,
}
self._send_event("DONE", payload)
process.kill() # kill song
self.sound.play_song((('C4', 'q'), ('C4', 'q'), ('C4', 'q')),
delay=0.1)
self.touch_sensor.wait_for_released()
# dispense liquid
def _pour(self, time_in_s=10, tea="Jasmine Milk Tea"):
# send event to alexa
payload = {"time_in_s": time_in_s, "tea": tea}
self._send_event("POUR", payload)
self.pump_motor.run_forever(speed_sp=1000)
sleep(time_in_s)
self.pump_motor.stop()
# dispense boba
def _dispense(self, cycles=10):
# send event to alexa
payload = {"cycles": cycles}
self._send_event("DISPENSE", payload)
# ensure the dispenser resets to the correct position everytime
if cycles % 2:
cycles += 1
# agitate the boba to make it fall
for i in range(cycles):
deg = 45 if i % 2 else -45
self.dispense_motor.on_for_degrees(SpeedPercent(75), deg)
sleep(0.5)
mL.position = 0
prev_mLposition = 0
mR.position = 0
prev_mRposition = 0
### move motors, then take samples while stopped
# for i in range(3,359,3):
# # rotate robot 3 deg cw
# print("rotating robot 3 deg cw, then pausing for 1 sec")
# mL.on_for_degrees(speed=3, degrees= 3 * 4.5)
# for quick testing
for i in range(10,359,10):
# rotate robot 10 deg cw
print("rotating robot 3 deg cw, then pausing for 1 sec; i = ", i)
mL.on_for_degrees(speed=3, degrees= 10 * 4.5)
# pause to let robot top shaking
time.sleep(1)
# read the sensors
readEV3usDistCm()
readHTcompass()
readEV3irProx()
readEV3gyro()
ODmLrotDegVal = mL.position / 4.5
print("usDistCmVal = ", int(usDistCmVal), " compassVal = ", compassVal, " irProxVal = ", irProxVal, " gyroVal = ", gyroVal, "mL.position = ", mL.position)
# append the arrays
cmpDegVal = np.append(cmpDegVal, compassVal)
gyroDegVal = np.append(gyroDegVal, gyroVal)
girar_pro_lado('dir', 90)
while cor('dir') != 'azul': #desce de frente e sobe de costas
steering_pair.on(2, 40)
else:
steering_pair.off()
steering_pair.on_for_degrees(0, 60, 250)
while usf.distance_centimeters > 16:
steering_pair.on(0, 15)
else:
steering_pair.off()
girar_pro_lado('esq', 90)
alinhamento()
steering_pair.on_for_degrees(0, -20, 350)
girar_pro_lado('esq', 180)
garra_g.on_for_degrees(20, -1080) #subir a garra
global gasoduto_com_cano
gasoduto_com_cano = (usl.distance_centimeters < 20)
garra_g.on_for_degrees(20, 1080) #desce a garra
ir_pro_gasoduto = False
no_gasoduto = True
while no_gasoduto: #começa paralelo ao gasoduto, termina quando o cano é posto
while gasoduto_com_cano and distancia(usl) < 15 and distancia(
usf) > 10:
acompanhar_gasoduto(4, 10)
#steering_pair.on_for_degrees(0,15,30)
if gasoduto_com_cano and distancia(usl) < 15 and distancia(
usf) > 10:
garra_g.on_for_degrees(20, -1080)
while colorRight.reflected_light_intensity < Constants.WHITE and False == Constants.STOP:
robot.on(speed=10, steering=0)
robot.off()
acceleration(degrees=DistanceToDegree(2), finalSpeed=20)
GyroTurn(steering=-100, angle=45)
# wall square
robot.on_for_seconds(steering=5, speed=-10, seconds=2)
acceleration(degrees=DistanceToDegree(55), finalSpeed=30, steering=1)
#lineFollowPID(degrees=DistanceToDegree(40), kp=1.25, ki=0.01, kd=5, color=ColorSensor(INPUT_3))
lineSquare()
acceleration(degrees=DistanceToDegree(20), finalSpeed=30, steering=-2)
motorC.on_for_seconds(speed=-20, seconds=0.5, brake=False)
motorC.on_for_degrees(speed=20, degrees=7, brake=True)
accelerationMoveBackward(degrees=DistanceToDegree(20), finalSpeed=30)
lineSquare()
GyroTurn(steering=-45, angle=85)
acceleration(degrees=DistanceToDegree(3.5), finalSpeed=10)
GyroTurn(steering=-50, angle=15)
acceleration(degrees=DistanceToDegree(5), finalSpeed=10)
accelerationMoveBackward(degrees=DistanceToDegree(1.5), finalSpeed=10)
motorC.on_for_seconds(speed=10, seconds=0.5, brake=True)
acceleration(degrees=DistanceToDegree(2), finalSpeed=10)
motorC.on_for_seconds(speed=10, seconds=0.2, brake=False)
acceleration(degrees=DistanceToDegree(7), finalSpeed=10)
motorC.on_for_seconds(speed=10, seconds=0.2, brake=True)
class AthenaRobot(object):
# constructors for the robot with default parameters of wheel radius and ports
def __init__(self,
wheelRadiusCm=2.75,
leftMotorPort=OUTPUT_B,
rightMotorPort=OUTPUT_C,
leftSensorPort=INPUT_1,
rightSensorPort=INPUT_4):
#self is the current object, everything below for self are member variables
self.wheelRadiusCm = wheelRadiusCm
self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
self.leftMotor = LargeMotor(leftMotorPort)
self.rightMotor = LargeMotor(rightMotorPort)
self.leftSensor = ColorSensor(leftSensorPort)
self.rightSensor = ColorSensor(rightSensorPort)
# run a distance in centimeters at speed of centimeters per second
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, False)
self.rightMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, block)
# turn a angle in degrees, positive means turn right and negative means turn left.
def turn(self, degree, brake=True, block=True):
# 1.9 is a scale factor from experiments
degreesToRun = degree * 1.9
# Turn at the speed of 20
self.leftMotor.on_for_degrees(20, degreesToRun, brake, False)
self.rightMotor.on_for_degrees(-20, degreesToRun, brake, block)
# run until find a game line
def onUntilGameLine(self,
consecutiveHit=5,
speed=10,
sleepTime=0.01,
white_threshold=85,
black_threshold=30,
brake=True):
# Start motor at passed speed.
self.leftMotor.on(speed)
self.rightMotor.on(speed)
# flags for whether both left and right wheel are in position
leftLineSquaredWhite = False
rightLineSquaredWhite = False
leftConsecutiveWhite = 0
rightConsecutiveWhite = 0
# first aligned on white
while (not leftLineSquaredWhite or not rightLineSquaredWhite):
left_reflected = self.leftSensor.reflected_light_intensity
right_reflected = self.rightSensor.reflected_light_intensity
# left to detect white
if (left_reflected > white_threshold):
leftConsecutiveWhite += 1
else:
leftConsecutiveWhite = 0
# reset to zero
if (leftConsecutiveWhite >= consecutiveHit):
self.leftMotor.off()
leftLineSquaredWhite = True
# right to detect white
if (right_reflected > white_threshold):
rightConsecutiveWhite += 1
else:
rightConsecutiveWhite = 0
# reset to zero
if (rightConsecutiveWhite >= consecutiveHit):
self.rightMotor.off()
rightLineSquaredWhite = True
print(
"left_reflected: {0:3d}, right_reflected: {1:3d}, leftConsecutiveWhite: {2:3d}, rightConsecutiveWhite: {3:3d}"
.format(left_reflected, right_reflected, leftConsecutiveWhite,
rightConsecutiveWhite),
file=sys.stderr)
sleep(sleepTime)
print("*********** White Line Reached *********", file=sys.stderr)
leftLineSquaredBlack = False
rightLineSquaredBlack = False
leftConsecutiveBlack = 0
rightConsecutiveBlack = 0
# now try black
self.leftMotor.on(speed)
self.rightMotor.on(speed)
while (not leftLineSquaredBlack or not rightLineSquaredBlack):
left_reflected = self.leftSensor.reflected_light_intensity
right_reflected = self.rightSensor.reflected_light_intensity
# left to detect black
if (left_reflected < black_threshold):
leftConsecutiveBlack += 1
else:
leftConsecutiveBlack = 0
# reset to zero
if (leftConsecutiveBlack >= consecutiveHit):
self.leftMotor.off()
leftLineSquaredBlack = True
# right to detect black
if (right_reflected < black_threshold):
rightConsecutiveBlack += 1
else:
rightConsecutiveBlack = 0
# reset to zero
if (rightConsecutiveBlack >= consecutiveHit):
self.rightMotor.off()
rightLineSquaredBlack = True
print(
"left_reflected: {0:3d}, right_reflected: {1:3d}, leftConsecutiveBlack: {2:3d}, rightConsecutiveBlack: {3:3d}"
.format(left_reflected, right_reflected, leftConsecutiveBlack,
rightConsecutiveBlack),
file=sys.stderr)
sleep(sleepTime)
self.leftMotor.off()
self.rightMotor.off()
#Go to the Bridge
def goToBridge(self):
# start from base, run 12.5 cm at 20cm/s
self.run(12.5, 20)
sleep(.2)
# turn right 70 degree
self.turn(70)
sleep(.1)
print("test", file=sys.stderr)
# run 90 cm at speed of 30 cm/s
self.run(90, 30, False)
sleep(.1)
# run until hit game line
self.onUntilGameLine()
sleep(.1)
# move forward 2cm at 15cm/s
self.run(2, 15)
# turn left 90 degree
self.turn(-90)
# move forward 13 cm at 20cm/s
self.run(13, 20)
sleep(.1)
# run until hit game line
self.onUntilGameLine()
# Calibrating Color for Sensor
def colorCalibrate(self, sensorInput):
sensor = ColorSensor(sensorInput)
sensor.calibrate_white()
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor from time import sleep lm = LargeMotor() lm.on(speed=50, brake=True, block=False) sleep(1) lm.off() sleep(1) lm.on_for_seconds(speed=50, seconds=2, brake=True, block=True) sleep(1) lm.on_for_rotations(50, 3) sleep(1) lm.on_for_degrees(50, 90) sleep(1) lm.on_to_position(50, 180) sleep(1)
# large_motor.wait_until_not_moving() ### this does not work with BrickPi3, state never changes as needed
# For BrickPi3 (and other non-EV3 platforms) the following may work better as control method for detecting a stall
mAspeed = -35 # speed setting
duty_cycle_margin = 6 # sensitivity buffer/margin; adjust to suite needs
duty_cycle_avg = 0
duty_cycle_min = 101
duty_cycle_max = -101
valChange = 0
dgrs = 30
for i in range(10):
print("iteration = ", i)
startTime = time.time()
large_motor.on_for_degrees(speed=mAspeed, degrees=dgrs, block=False)
while abs(large_motor.duty_cycle) <= abs(abs(mAspeed) + abs(duty_cycle_margin)):
duty_cycle_avg = (duty_cycle_avg + large_motor.duty_cycle) / 2
duty_cycle_min = min(duty_cycle_min, large_motor.duty_cycle)
duty_cycle_max = max(duty_cycle_max, large_motor.duty_cycle)
if valChange != duty_cycle_min + duty_cycle_max:
print("position, duty_cycle avg, min, max = ", large_motor.position, duty_cycle_avg, duty_cycle_min, duty_cycle_max)
valChange = duty_cycle_min + duty_cycle_max
time.sleep(0.01)
if time.time() - startTime > 2:
print("timeout - exiting")
break
print("position, duty_cycle avg, min, max = ", large_motor.position, duty_cycle_avg, duty_cycle_min, duty_cycle_max)
] # container -- [iteration, loop_t, N_t, A_t, G_t, Us_t, S_t, Ns_t, App_t]
print('######## Episode : ', episode, ' ########')
audio.play_tone(4000, 0.1, volume=1, play_type=0)
init(drive, 'drive')
init(grabber, 'grabber')
init(jack, 'jack')
reference_dist = init(U, 'U')
record_transitions = [
] # container -- [state, action, reward, state_next, terminal, run]
# audio.speak('Initialized',volume=50)
audio.play_tone(3000, 0.05, volume=1, play_type=0)
# Close and open the grabber
grabber.on_for_degrees(-70, Grabber_angle, brake=True, block=True)
sleep(1)
jack.on_for_rotations(-100, Jack_angle, brake=True)
sleep(3)
grabber.on_for_degrees(5, Grabber_angle * 0.2, brake=False)
grabber.on_for_degrees(70, Grabber_angle * 0.4, brake=False)
grabber.on_for_degrees(70,
Grabber_angle * 0.4,
brake=True,
block=False)
sleep(1)
init(G, 'G')
print("Gyro angle: ", G.angle)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B from ev3dev2.motor import SpeedDPS, SpeedRPS, SpeedRPM from time import sleep large_motor = LargeMotor(OUTPUT_B) # We'll make the motor turn 180 degrees # at speed 50 (525 dps for the large motor) large_motor.on_for_degrees(speed=50, degrees=180) # then wait one second sleep(1) # then – 180 degrees at 500 dps large_motor.on_for_degrees(speed=SpeedDPS(500), degrees=-180) sleep(1) # then 0.5 rotations at speed 50 (525 dps) large_motor.on_for_rotations(speed=50, rotations=0.5) sleep(1) # then – 0.5 rotations at 1 rotation per second (360 dps) large_motor.on_for_rotations(speed=SpeedRPS(1), rotations=-0.5)
time.sleep(1)
result = []
n = 12 # limit how many times we rotate 30 degrees looking for a tag
np.set_printoptions(
formatter={'float': lambda x: "{0:0.3f}".format(x)})
print("Capturing camera frame...")
tic = time.time() # for timing analysis
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
result = detector.detect(gray)
while result == []:
if n > 0: # limit # of rotation iterations; 12 times (360 degs) should do it
n = n - 1 # decrement n
# rotate robot 30 deg cw as we look for a tag
mL.on_for_degrees(speed=14, degrees=135)
time.sleep(1)
# flush the camera frame buffer
for i in range(7):
ret, frame = cap.read()
np.set_printoptions(
formatter={'float': lambda x: "{0:0.3f}".format(x)})
print("Trying again...capturing camera frame...")
# for timing analysis
tic = time.time()
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
result = detector.detect(gray)
# for initial testing/devopment
# threading.Thread(target= activeMotorC).start()
# threading.Thread(target= activeMotorD).start()
# threading.Thread(target= activeMotorE).start()
# threading.Thread(target= activeMotorF).start()
# threading.Thread(target= activeMotorG).start()
# threading.Thread(target= activeMotorA).start()
# threading.Thread(target= activeMotorB).start()
# threading.Thread(target= activeMotorC2).start()
for msg in MidiFile('beliver.mid'):
time.sleep(msg.time)
if not msg.is_meta:
# C
if (msg.type == "note_on" and msg.note == 60):
portA.on_for_degrees(armSpeed, rightArm)
motorC = True
elif (msg.type == "note_off" and msg.note == 60):
portA.on_for_degrees(armSpeed, leftArm)
motorC = False
# D
if (msg.type == "note_on" and msg.note == 62):
portA.on_for_degrees(armSpeed, leftArm)
motorD = True
elif (msg.type == "note_off" and msg.note == 62):
portA.on_for_degrees(armSpeed, rightArm)
motorD = False
# E
if (msg.type == "note_on" and msg.note == 64):
class Pen:
"""Pen class witch allow managing the pen position. """
def __init__(self, power=20):
_debug(self, "Creating Pen instance")
self._default_power = power
self._pen_up_position = 0 # Lambda values. Needed to be set before !
self._pen_down_position = 40
self._m = LargeMotor(OUTPUT_C)
self._m.stop_action = LargeMotor.STOP_ACTION_HOLD
self.reset()
def up(self, power=None):
""" Set pen to 'up' position (only if is not already)
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
if not self.is_up:
self._m.on_to_position(power, self._pen_up_position)
def down(self, power=None):
""" Set pen to 'down' position (only if is not already)
:param power: Power of the rotation
"""
if power is None:
power = self._default_power
if self.is_up:
self._m.on_to_position(power, self._pen_down_position)
@property
def is_up(self):
""" Get the pen position
:return: True if pen up, False otherwise
"""
return self._m.position < self._pen_up_position + 15
def toggle(self):
""" Change pen position to the opposite
Pen Up ==> Pen Down
Pen Down ==> Pen Up
"""
if self.is_up:
self.down()
else:
self.up()
def reset(self, power=None):
"""Reset the pen position (set it to 'up')
:param power: Power used to move the pen
"""
if power is None:
power = self._default_power
self._m.on(-power)
self._m.wait_until_not_moving()
self._m.off()
self._m.on_for_degrees(power, 20)
sleep(1)
def setup(self, validator):
""" Setup the pen, define up and down position.
:param validator: A callable objet used to validate the pen down position.
When `True` is returned, the pen position will be saved to the down position."""
self.reset()
self._pen_up_position = self._m.position
self._m.off(False)
while not validator():
sleep(0.1)
self._pen_down_position = self._m.position
self.up()
def save_energy(self):
"""Save energy and release motor's holding state"""
self._m.off(False)
#!/usr/bin/python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank, MediumMotor
from ev3dev2.led import Leds
from ev3dev2.button import Button
from ev3dev2.sound import Sound
from time import sleep
import os
os.system('setfont Lat15-TerminusBold14')
penLift = MediumMotor(OUTPUT_C)
penMove = LargeMotor(OUTPUT_A)
btn = Button()
penLift.on_for_degrees(speed=40, degrees=-850)
penMove.on_for_degrees(speed=20, degrees=500)
sleep(2)
while True:
penMove.on_for_degrees(speed=20, degrees=-1000)
penLift.on_for_degrees(speed=-20, degrees=10)
if btn.enter:
penMove.on_for_degrees(speed=20, degrees=500)
penLift.on_for_degrees(speed=20, degrees=900)
break
penMove.on_for_degrees(speed=20, degrees=1000)
penLift.on_for_degrees(speed=-20, degrees=10)
if btn.enter:
penMove.on_for_degrees(speed=20, degrees=-500)
penLift.on_for_degrees(speed=20, degrees=900)
break
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
) #no fim da programação, alinhar com o gasoduto
if gasoduto_com_cano:
sound.speak('ok')
garra_g.on_for_degrees(20, 1200)
sound.beep()
sound.beep()
sleep(3)
ir_pro_gasoduto = False
sound.speak('finish')
# reset gyro sensor
gy.mode = 'GYRO-RATE'
gy.mode = 'GYRO-ANG'
log.info('prepare for the initial position')
# prepare for the initial position
# detect the upper position of the lifter
lm_lifter.on( -100, brake=True)
while( not ts.is_pressed ):
sleep(0.05)
# approaching the initial position with high speed
lm_lifter.on_for_rotations(90, 7)
# nearly approached the initial position, approaching with lower speed
lm_lifter.on_for_degrees(20, 240)
# clear the lcd display
lcd.clear()
# show the steps
lcd.text_pixels( str(steps), True, 80, 50, font='courB18')
lcd.update()
log.info('wait user to supply the steps')
# wait user to supply the steps to go
while( True ):
if(not btn.buttons_pressed):
sleep(0.01)
continue
block=False)
while INFRARED_SENSOR.heading(channel=1) >= 3:
pass
GO_MOTOR.off(brake=True)
SPEAKER.play_file(
wav_file='/home/robot/sound/Blip 4.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
for i in range(3):
GO_MOTOR.on_for_degrees(
speed=-100,
degrees=10,
block=True,
brake=True)
STING_MOTOR.on_for_degrees(
speed=-75,
degrees=220,
brake=True,
block=True)
sleep(0.1)
STING_MOTOR.on_for_seconds(
speed=-100,
seconds=1,
brake=True,
class AthenaRobot(object):
# constructors for the robot with default parameters of wheel radius and ports
def __init__(self,
wheelRadiusCm=4,
leftLargeMotorPort=OUTPUT_B,
rightLargeMotorPort=OUTPUT_C,
leftMediumMotorPort=OUTPUT_A,
rightMediumMotorPort=OUTPUT_D,
leftSensorPort=INPUT_1,
rightSensorPort=INPUT_4,
ultraSonicSensorPort=INPUT_2):
#self is the current object, everything below for self are member variables
self.wheelRadiusCm = wheelRadiusCm
self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
self.leftLargeMotor = LargeMotor(leftLargeMotorPort)
self.rightLargeMotor = LargeMotor(rightLargeMotorPort)
self.leftMediumMotor = MediumMotor(leftMediumMotorPort)
self.rightMediumMotor = MediumMotor(rightMediumMotorPort)
self.leftSensor = ColorSensor(leftSensorPort)
self.rightSensor = ColorSensor(rightSensorPort)
# run a distance in centimeters at speed of centimeters per second
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
if speedCmPerSecond < 0:
raise Exception('speed cannot be negative')
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Run - Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftLargeMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, block)
# turn a angle in degrees, positive means turn right and negative means turn left.
def turn(self, degree, speed=10, brake=True, block=True):
# 1.9 is a scale factor from experiments
degreesToRun = degree * 1.275
# Turn at the speed
self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)
def turnRight(self, degree, speed=10, brake=True, block=True):
self.turn(degree, speed, brake, block)
def turnLeft(self, degree, speed=10, brake=True, block=True):
self.turn(-degree, speed, brake, block)
def turnOnRightWheel(self, degree, speed=10, brake=True, block=True):
degreesToRun = degree * 2.7
self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)
def turnRightOnRightWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnRightWheel(degree, speed, brake, block)
def turnLeftOnRightWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnRightWheel(-degree, speed, brake, block)
def turnOnLeftWheel(self, degree, speed=10, brake=True, block=True):
degreesToRun = degree * 2.7
self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, block)
def turnRightOnLeftWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnLeftWheel(degree, speed, brake, block)
def turnLeftOnLeftWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnLeftWheel(-degree, speed, brake, block)
#Medium Motor Movement
def moveMediumMotor(self, isLeft, speed, degrees, brake=True, block=True):
#sees which motor is running
if isLeft == False:
self.rightMediumMotor.on_for_degrees(speed, degrees, brake, block)
else:
self.leftMediumMotor.on_for_degrees(speed, degrees, brake, block)
# Following a line with one sensor
def lineFollow(self,
whiteThreshold=98,
blackThreshold=15,
scale=0.2,
useLeftSensor=True,
useLeftEdge=True,
runDistanceCM=300):
self.leftLargeMotor.reset()
self.rightLargeMotor.reset()
# Allow an attached backsensor. Ixf useBackSensor, defining back sensor and revert useLeftEdge since motor is actually going backward
initialPos = self.leftLargeMotor.position # remember initial position
loop = True
while loop:
# use left or right sensor based on passed in useLeftSensor
reflect = self.leftSensor.reflected_light_intensity if useLeftSensor == True else self.rightSensor.reflected_light_intensity
# Allow an attached backsensor. If useBackSensor, use reflected_light_intensity of that sensor
leftPower = abs(reflect - blackThreshold) * scale
rightPower = abs(whiteThreshold - reflect) * scale
# if we follow the right edge, need to swap left and right
if useLeftEdge == False:
oldLeft = leftPower
leftPower = rightPower
rightPower = oldLeft
self.leftLargeMotor.on(-leftPower)
self.rightLargeMotor.on(-rightPower)
# Calculate the distance run in CM
distanceRanInCM = abs((self.leftLargeMotor.position - initialPos) *
(self.wheelCircumferenceCm /
self.leftLargeMotor.count_per_rot))
# Printing the reflected light intensity with the powers of the two motors
print(
"LineFollow - reflect: {0:3d} leftPower: {1:3f} rightPower: {2:3f} lMotorPos: {3:3d} distanceRanInCM {4:3f}"
.format(reflect, leftPower, rightPower,
self.leftLargeMotor.position, distanceRanInCM),
file=sys.stderr)
if distanceRanInCM >= runDistanceCM:
loop = False
# Stopping the motor once the loop is over
self.leftLargeMotor.off()
self.rightLargeMotor.off()
# run until both conditions are met
def onUntilTwoConditions(self,
leftCondition,
rightCondition,
caller,
speed=5,
consecutiveHit=5,
sleepTime=0.01):
# Start motor at passed speonUntilTwoConditionsed.
self.leftLargeMotor.on(-speed)
self.rightLargeMotor.on(-speed)
condLeftCounter = 0
condRightCounter = 0
condLeftMet = False
condRightMet = False
while (not condLeftMet or not condRightMet):
# check left condition
if (leftCondition()):
condLeftCounter += 1
else:
condLeftCounter = 0
# reset to zero
if (condLeftCounter >= consecutiveHit):
if (condRightMet):
sleep(.1)
self.leftLargeMotor.off()
condLeftMet = True
# check right condition
if (rightCondition()):
condRightCounter += 1
else:
condRightCounter = 0
# reset to zero
if (condRightCounter >= consecutiveHit):
if (condLeftMet):
sleep(.1)
self.rightLargeMotor.off()
condRightMet = True
print(
"{4}-onUntilTwoConditions: left_reflected: {0:3d}, right_reflected: {1:3d}, leftHit: {2:3d}, rightHit: {3:3d}"
.format(self.leftSensor.reflected_light_intensity,
self.rightSensor.reflected_light_intensity,
condLeftCounter, condRightCounter, caller),
file=sys.stderr)
sleep(sleepTime)
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def onUntilWhiteLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilTwoConditions(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, lambda: self.rightSensor.reflected_light_intensity
> white_threshold, "onUntilWhiteLine", speed, consecutiveHit,
sleepTime)
def onUntilBlackLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilTwoConditions(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, lambda: self.rightSensor.reflected_light_intensity
< black_threshold, "onUntilBlackLine", speed, consecutiveHit,
sleepTime)
# run until find a game line
def onUntilGameLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
white_threshold=85,
black_threshold=30):
self.onUntilWhiteLine(consecutiveHit, speed, sleepTime,
white_threshold)
self.onUntilBlackLine(consecutiveHit, speed, sleepTime,
black_threshold)
# run until condition is met
def onUntilCondition(self,
condition,
caller,
leftSpeed=5,
rightSpeed=5,
consecutiveHit=5,
sleepTime=0.01,
revert=False):
# Start motor at passed speonUntilTwoConditionsed.
self.leftLargeMotor.on(-leftSpeed if revert == False else leftSpeed)
self.rightLargeMotor.on(-rightSpeed if revert == False else rightSpeed)
counter = 0
condMet = False
while (not condMet):
# check condition
if (condition()):
counter += 1
else:
counter = 0
# reset to zero
if (counter >= consecutiveHit):
self.leftLargeMotor.off()
self.rightLargeMotor.off()
condMet = True
print(
"{4}-onUntilCondition: ColorSensor(left_reflected: {0:3d}, right_reflected: {1:3d}, hit: {2:3d}), UltraSonicSensor(distance_centimeters: {3:3f})"
.format(self.leftSensor.reflected_light_intensity,
self.rightSensor.reflected_light_intensity, counter,
self.ultraSonicSensor.distance_centimeters, caller),
file=sys.stderr)
sleep(sleepTime)
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def onUntilLeftBlack(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, "onUntilLeftBlack", speed, speed, consecutiveHit,
sleepTime)
def onUntilLeftWhite(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, "onUntilLeftWhite", speed, speed, consecutiveHit,
sleepTime)
def onUntilRightBlack(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity <
black_threshold, "onUntilRightBlack", speed, speed, consecutiveHit,
sleepTime)
def onUntilRightWhite(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity >
white_threshold, "onUntilRightWhite", speed, speed, consecutiveHit,
sleepTime)
def turnUntilLeftBlack(self,
turnLeft,
speed,
consecutiveHit=2,
black_threshold=30):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, "turnUntilLeftBlack",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilLeftWhite(self,
turnLeft,
speed,
consecutiveHit=2,
white_threshold=85):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, "turnUntilLeftWhite",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilRightBlack(self,
turnLeft,
speed,
consecutiveHit=2,
black_threshold=30):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity <
black_threshold, "turnUntilRightBlack",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilRightWhite(self,
turnLeft,
speed,
consecutiveHit=2,
white_threshold=85):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity >
white_threshold, "turnUntilRightWhite",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
# Go until sensor reading has a specified offset or reach to the threshhold
def onUntilRightDarkerBy(self,
difference,
black_threshold=20,
speed=10,
consecutiveHit=2):
originalValue = self.rightSensor.reflected_light_intensity
print("onUntilRightDarkerBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity - originalValue
< -difference or self.rightSensor.reflected_light_intensity <
black_threshold,
"onUntilRightSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilRightLighterBy(self,
difference,
white_threshold=80,
speed=10,
consecutiveHit=2):
originalValue = self.rightSensor.reflected_light_intensity
print("onUntilRightLighterBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity - originalValue
> difference or self.rightSensor.reflected_light_intensity >
white_threshold,
"onUntilRightSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilLeftDarkerBy(self,
difference,
black_threshold=20,
speed=10,
consecutiveHit=2):
originalValue = self.leftSensor.reflected_light_intensity
print(
"onUntilLeftDarkerBy - originalValue: {0:3d}, diff: {1:3d}".format(
originalValue, difference),
file=sys.stderr)
self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
- originalValue < -difference or self.leftSensor.
reflected_light_intensity < black_threshold,
"onUntilLeftSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilLeftLighterBy(self,
difference,
white_threshold=80,
speed=10,
consecutiveHit=2):
originalValue = self.leftSensor.reflected_light_intensity
print("onUntilLeftLighterBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
- originalValue > difference or self.leftSensor.
reflected_light_intensity > white_threshold,
"onUntilLeftSensorDiff",
consecutiveHit=consecutiveHit)
#uses Ultrasonic sensor to see wall as going back
def revertSafely(self,
speed=100,
distanceToStop=10,
consecutiveHit=1,
sleepTime=0.01):
self.onUntilCondition(
lambda: self.ultraSonicSensor.distance_centimeters <
distanceToStop, "revertSafely", speed, speed, consecutiveHit,
sleepTime, True)
# Calibrating White for Sensor
def calibrateColorSensor(self, sensorInput):
sensor = ColorSensor(sensorInput)
# Calibration
sensor.calibrate_white()
# Done Signal
sound.beep()
# Calibrating Color for Sensor
def testColorSensor(self,
sensorInput,
sensorPort,
repeatNumber=10,
pauseNumber=0.2,
speed=0):
sensor = ColorSensor(sensorInput)
if (speed > 0):
self.leftLargeMotor.on(speed)
self.rightLargeMotor.on(speed)
times = 0
# For loop
while times != repeatNumber:
# Print
print("testColorSensor- Port: {0:3d}: {1:3d}".format(
sensorPort, sensor.reflected_light_intensity),
file=sys.stderr)
time.sleep(pauseNumber)
times = times + 1
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def resetMediumMotor():
self.moveMediumMotor(isLeft=False, speed=50, degrees=1000)
self.rightMediumMotor.reset()
def testRobot(self):
self.leftLargeMotor.on_for_degrees(20, 180)
sleep(.1)
self.rightLargeMotor.on_for_degrees(20, 180)
sleep(.1)
self.moveMediumMotor(True, 10, 180)
sleep(.1)
self.moveMediumMotor(False, 10, 180)
sleep(.1)
self.run(20, 10)
sleep(.1)
self.run(-20, 10)
sleep(.1)
self.turn(90)
sleep(.1)
self.turn(-90)
sleep(.1)
self.turnOnLeftWheel(90)
sleep(.1)
self.turnOnLeftWheel(-90)
sleep(.1)
self.turnOnRightWheel(90)
sleep(.1)
self.turnOnRightWheel(-90)
sleep(.1)
self.calibrateColorSensor(INPUT_1)
self.calibrateColorSensor(INPUT_4)
self.testColorSensor(INPUT_1, 1)
self.testColorSensor(INPUT_4, 4)
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
Two types of commands are supported, directional movement and preset.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Gadget state
self.patrol_mode = False
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.turnMotor = LargeMotor(OUTPUT_B)
self.cardsMotor = LargeMotor(OUTPUT_D)
# Start threads
# threading.Thread(target=self._patrol_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 == "deal-initial":
# Expected params: [game, playerCount]
self._dealCardOnGameStart(payload["game"],
int(payload["playerCount"]))
if control_type == "deal-turn":
# Expected params: [game, playerCount, playerTurn]
self._dealCardOnGameTurn(payload["game"],
int(payload["playerCount"]),
int(payload["playerTurn"]),
payload["gameCommand"])
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _dealCardOnGameStart(self, game, playerCount: int):
"""
Handles dealing the cards based on game type and the number of players
"""
print("Dealing cards: ({}, {})".format(game, playerCount),
file=sys.stderr)
if game in GameType.BLACKJACK.value:
for i in range(playerCount):
print("Dealing card to: ({})".format(i + 1), file=sys.stderr)
self._moveToPlayer(i + 1, playerCount, False)
self._dispenseCard()
self._moveToBase(playerCount, playerCount)
for i in range(playerCount):
print("Dealing card to: ({})".format(i + 1), file=sys.stderr)
self._moveToPlayer(i + 1, playerCount, False)
self._dispenseCard()
self._moveToBase(playerCount, playerCount)
if game in GameType.UNO.value:
for k in range(4):
for i in range(playerCount):
print("Dealing card to: ({})".format(i + 1),
file=sys.stderr)
self._moveToPlayer(i + 1, playerCount, False)
self._dispenseCard()
self._moveToBase(playerCount, playerCount)
if game in GameType.SHUFFLE_CARDS.value:
for k in range(5):
self._moveToPlayer(1, 2, True)
cardsToDispense = random.randint(1, 3)
for k in range(cardsToDispense):
self._dispenseCard()
self._moveToBase(1, 2)
self._moveToPlayer(2, 2, True)
cardsToDispense = random.randint(1, 3)
for k in range(cardsToDispense):
self._dispenseCard()
self._moveToBase(2, 2)
def _dealCardOnGameTurn(self, game, playerCount: int, playerTurn: int,
gameCommand):
"""
Handles dealing the card based on game type and the turn in the game
"""
print("Dealing cards: ({}, {}, {})".format(game, playerCount,
playerTurn),
file=sys.stderr)
if game in GameType.BLACKJACK.value:
if gameCommand in GameCommand.BLACKJACK_HIT.value:
print("Dealing card to: ({})".format(playerTurn),
file=sys.stderr)
self._moveToPlayer(playerTurn, playerCount, True)
self._dispenseCard()
self._moveToBase(playerTurn, playerCount)
if game in GameType.UNO.value:
if gameCommand in GameCommand.UNO_DEAL_ONCE.value:
print("Dealing card to: ({})".format(playerTurn),
file=sys.stderr)
cardsToDispense = random.randint(0, 3)
if cardsToDispense > 0:
for k in range(cardsToDispense):
self._dispenseCard()
if gameCommand in GameCommand.UNO_DEAL_TWICE.value:
print("Dealing card to: ({})".format(playerTurn),
file=sys.stderr)
cardsToDispense = random.randint(0, 3)
if cardsToDispense > 0:
for k in range(cardsToDispense):
self._dispenseCard()
cardsToDispense = random.randint(0, 3)
if cardsToDispense > 0:
for k in range(cardsToDispense):
self._dispenseCard()
def _moveToPlayer(self, playerIndex: int, playerCount: int,
oneTimeMove: bool):
angle = -180 / playerCount
turnAngle = angle
print("Moving to player: ({}) out of ({})".format(
playerIndex, playerCount),
file=sys.stderr)
if oneTimeMove == True:
if playerIndex > 1:
turnAngle = angle * (playerIndex - 1)
if playerIndex == 1:
turnAngle = 0
print("Angle: ({})".format(turnAngle), file=sys.stderr)
self.turnMotor.on_for_degrees(SpeedPercent(15), turnAngle, True)
time.sleep(.25)
def _moveToBase(self, playerIndex: int, playerCount: int):
print("Reset to base", file=sys.stderr)
time.sleep(.50)
angle = 180 / playerCount
turnAngle = angle
if playerIndex > 1:
turnAngle = angle * (playerIndex - 1)
if playerIndex == 1:
turnAngle = 0
print("Angle: ({})".format(turnAngle), file=sys.stderr)
self.turnMotor.on_for_degrees(SpeedPercent(15), turnAngle, True)
def _dispenseCard(self):
self.cardsMotor.on_for_rotations(SpeedPercent(-50), 0.5, True)
time.sleep(.25)
self.cardsMotor.on_for_rotations(SpeedPercent(-50), 0.2, True)
time.sleep(.25)
self.cardsMotor.on_for_rotations(SpeedPercent(50), 0.7, True)
class IO():
"""
Provides functions for high-level IO operations (move left, move forward, is left side free to move etc.).
"""
def __init__(self):
# Large motors
self.lm_left_port = "outB"
self.lm_right_port = "outA"
self.lm_left = LargeMotor(self.lm_left_port)
self.lm_right = LargeMotor(self.lm_right_port)
# distance at which sensor motor start moving
self.move_sensor_check_degrees = 400
self.move_degrees = 570 # one tile distance
self.move_speed = 35
self.after_crash_degrees = 200
self.steering_turn_speed = 30 # turning left or right
self.steering_turn_degrees = 450
self.steering_turn_fwd_degrees = 150 # distance to move after turning
# distance at which sensors start spinning
self.steering_sensor_check_degrees = 50
self.btn = Button()
# small motor
self.sm_port = "outC"
self.sm = Motor(self.sm_port)
self.sm_turn_speed = 30
self.sm_center_turn_angle = 90
self.sm_side_turn_angle = 110
self.sm_is_left = False
# color sensor
self.color_sensor_port = "in1"
self.color_sensor = ColorSensor(self.color_sensor_port)
self.color_sensor.mode = ColorSensor.MODE_COL_REFLECT
self.color_sensor_values = []
# regulations
self.regulation_desired_value = 4
self.regulation_max_diff = 3
self.regulation_p = 1.5
self.regulation_tick = 0.03
# ultrasonic sensor
self.ultrasonic_sensor_port = "in4"
self.ultrasonic_sensor = UltrasonicSensor(self.ultrasonic_sensor_port)
self.ultrasonic_sensor.mode = 'US-DIST-CM'
self.ultrasonic_tile_length = 30
self.ultrasonic_max_value = 255
self.ultrasonic_sensor_values = []
# touch sensors
self.touch_right = TouchSensor("in2")
self.touch_left = TouchSensor("in3")
def go_left(self):
ok = self.__turn_left()
if (ok):
ok = self.__move_reg(self.steering_turn_fwd_degrees,
self.steering_sensor_check_degrees)
return ok
def go_right(self):
ok = self.__turn_right()
if (ok):
ok = self.__move_reg(self.steering_turn_fwd_degrees,
self.steering_sensor_check_degrees)
return ok
def go_forward(self):
return self.__move_reg(self.move_degrees,
self.move_sensor_check_degrees)
def go_back(self):
self.__turn(stop_motor=self.lm_left,
turn_motor=self.lm_right,
degrees=self.steering_turn_degrees,
speed=self.steering_turn_speed)
return self.__turn(stop_motor=self.lm_right,
turn_motor=self.lm_left,
degrees=self.steering_turn_degrees,
speed=-self.steering_turn_speed)
def after_crash(self):
debug_print("crash")
self.__move(self.after_crash_degrees, self.move_speed)
self.read_sensors()
def __turn(self, stop_motor: Motor, turn_motor: Motor, degrees: int,
speed: int):
stop_motor.stop()
start = turn_motor.degrees
turn_motor.on(speed)
while (abs(turn_motor.degrees - start) < degrees):
if (not self.__check_button()):
self.lm_left.stop()
self.lm_right.stop()
return False
return True
def __turn_left(self):
return self.__turn(stop_motor=self.lm_left,
turn_motor=self.lm_right,
degrees=self.steering_turn_degrees,
speed=-self.steering_turn_speed)
def __turn_right(self):
return self.__turn(stop_motor=self.lm_right,
turn_motor=self.lm_left,
degrees=self.steering_turn_degrees,
speed=-self.steering_turn_speed)
def __move(self, degrees, speed) -> None:
self.lm_left.on_for_degrees(speed, degrees, block=False)
self.lm_right.on_for_degrees(speed, degrees, block=True)
def __reg(self):
val = self.color_sensor.reflected_light_intensity
diff = (self.regulation_desired_value - val)
if diff >= 0 and val > 0:
diff = min(diff, self.regulation_max_diff)
elif val == 0:
diff = 0
else:
diff = -min(abs(diff), self.regulation_max_diff)
diff *= self.regulation_p
if self.sm_is_left:
return (-self.move_speed + diff, -self.move_speed - diff)
return (-self.move_speed - diff, -self.move_speed + diff)
def __check_button(self):
timeout = time.time() + self.regulation_tick
while (time.time() <= timeout): # check for touch sensor
if (self.touch_left.is_pressed or self.touch_right.is_pressed):
return False
if (self.btn.left or self.btn.right):
debug_print("pressed")
return None
time.sleep(0.01)
return True
def __move_reg(self, degrees, sensor_degrees):
t = Thread(target=self.read_sensors)
start_l, start_r = (self.lm_left.degrees, self.lm_right.degrees)
distance_l, distance_r = 0, 0
while (distance_l < degrees or distance_r < degrees):
speed_l, speed_r = self.__reg()
self.lm_left.on(speed_l, brake=True)
self.lm_right.on(speed_r, brake=True)
ok = self.__check_button()
if (ok is False):
self.lm_left.stop()
self.lm_right.stop()
return False
elif (ok is None):
debug_print("none")
self.lm_left.stop()
self.lm_right.stop()
return None
if ((distance_l >= sensor_degrees or distance_r >= sensor_degrees)
and not t.isAlive()):
t.start()
distance_l = abs(start_l - self.lm_left.degrees)
distance_r = abs(start_r - self.lm_right.degrees)
t.join()
return True
def read_sensors(self):
self.color_sensor_values = [] # List[float]
self.ultrasonic_sensor_values = []
speed = self.sm_turn_speed
if (self.sm_is_left):
speed = -self.sm_turn_speed
# side 1
self.color_sensor_values.append(
self.color_sensor.reflected_light_intensity)
self.ultrasonic_sensor_values.append(
self.ultrasonic_sensor.distance_centimeters)
# center
self.sm.on_for_degrees(speed, self.sm_center_turn_angle)
self.color_sensor_values.append(
self.color_sensor.reflected_light_intensity)
self.ultrasonic_sensor_values.append(
self.ultrasonic_sensor.distance_centimeters)
# side 2
self.sm.on_for_degrees(speed, self.sm_side_turn_angle)
self.color_sensor_values.append(
self.color_sensor.reflected_light_intensity)
self.ultrasonic_sensor_values.append(
self.ultrasonic_sensor.distance_centimeters)
if not self.sm_is_left:
self.ultrasonic_sensor_values.reverse()
self.color_sensor_values.reverse()
self.sm_is_left = not self.sm_is_left
def directions_free(self) -> List[bool]:
'''
Returns list of bools (left, center, right), representing if the directions are free to move.
'''
return [a == 0 for a in self.color_sensor_values]
def ghost_distance(self) -> List[int]:
'''
Returns list of ints (left, center, right), representing the distance from closest ghost.
'''
return [
int(a) //
self.ultrasonic_tile_length if a < self.ultrasonic_max_value
and a // self.ultrasonic_tile_length > 0 else None
for a in self.ultrasonic_sensor_values
]
class Spik3r:
def __init__(self,
claw_motor_port: str = OUTPUT_A,
move_motor_port: str = OUTPUT_B,
sting_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.claw_motor = MediumMotor(address=claw_motor_port)
self.move_motor = LargeMotor(address=move_motor_port)
self.sting_motor = LargeMotor(address=sting_motor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.speaker = Sound()
def snap_claw_if_touched(self):
if self.touch_sensor.is_pressed:
self.claw_motor.on_for_seconds(speed=50,
seconds=1,
brake=True,
block=True)
self.claw_motor.on_for_seconds(speed=-50,
seconds=0.3,
brake=True,
block=True)
def move_by_ir_beacon(self):
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=100, block=False, brake=False)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=-100, brake=False, block=False)
else:
self.move_motor.off(brake=False)
def sting_by_ir_beacon(self):
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.sting_motor.on_for_degrees(speed=-75,
degrees=220,
brake=True,
block=False)
self.speaker.play_file(wav_file='Blip 3.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
self.sting_motor.on_for_seconds(speed=-100,
seconds=1,
brake=True,
block=True)
self.sting_motor.on_for_seconds(speed=40,
seconds=1,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def main(self):
while True:
self.snap_claw_if_touched()
self.move_by_ir_beacon()
self.sting_by_ir_beacon()
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
) #no fim da programação, alinhar com o gasoduto
if gasoduto_com_cano:
sound.speak('ok')
garra_g.on_for_degrees(20, 1200)
sound.beep()
sound.beep()
sleep(3)
ir_pro_gasoduto = False
no_gasoduto = True
while no_gasoduto:
while gasoduto_com_cano and distancia(usl) < 15 and distancia(
usf) > 10:
disp.draw.text((10, 10), 'Hello World!', font=fonts.load('courB18'))
disp.update()
# 改变面板显示灯的颜色
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "RED")
# 英文转语音
sleep(1)
sounds.speak(text='Welcome to the E V 3 dev project!')
sleep(1)
# 控制电机转动
m = LargeMotor(OUTPUT_A)
#m.on_for_rotations(SpeedPercent(75), 1)
m.on_for_degrees(50, 50)
sleep(1)
m.on_for_degrees(50, -50)
sleep(1)
# 检测接触传感器，并显示不同颜色
while True:
if ts.is_pressed:
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
else:
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "RED")
# 等待1秒
sleep(1)
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)
