#!/usr/bin/env python3
import os
import sys
import time
import logging
import json
import random
import threading
from enum import Enum
from agt import AlexaGadget
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
from ev3dev2.motor import OUTPUT_A,OUTPUT_B, OUTPUT_C, OUTPUT_D, MoveTank, MoveSteering, SpeedPercent, SpeedRPM, MediumMotor, LargeMotor
#from ev3dev2.sensor.lego import TouchSensor,UltrasonicSensor
# Set the logging level to INFO to see messages from AlexaGadget
logging.basicConfig(level=logging.INFO, stream=sys.stdout, format='%(message)s')
logging.getLogger().addHandler(logging.StreamHandler(sys.stderr))
logger = logging.getLogger(__name__)
from ev3dev2.sensor.lego import TouchSensor,UltrasonicSensor
from ev3dev2.sensor import INPUT_1
import rpyc
conn = rpyc.classic.connect('192.168.43.143')
slave_ev3 = conn.modules['ev3dev2.motor']
slave_out_a = slave_ev3.LargeMotor('outA')
sensor_inp = conn.modules['ev3dev2.sensor.lego']
ts = sensor_inp.TouchSensor('in1')
us = sensor_inp.UltrasonicSensor('in2')
class EventName(Enum):
"""
The list of custom event name sent from this gadget
"""
SENTRY = "Sentry"
BINSTATUS = "bin"
SPEECH = "Speech"
class Direction(Enum):
"""
The list of directional commands and their variations.
These variations correspond to the skill slot values.
"""
FORWARD = ['forward', 'forwards', 'go forward','move forward']
BACKWARD = ['back', 'backward', 'backwards', 'go backward', 'move backward']
LEFT = ['left', 'go left']
RIGHT = ['right', 'go right']
STOP = ['stop moving', 'brake', 'stop', 'stop movement']
class Command(Enum):
"""
The list of preset commands and their invocation variation.
These variations correspond to the skill slot values.
"""
MOVE_CIRCLE = ['circle', 'spin']
MOVE_SQUARE = ['square']
PATROL = ['patrol', 'guard mode', 'sentry mode']
FIRE_ONE = ['cannon', '1 shot', 'one shot']
FIRE_ALL = ['all shot']
OPEN_GRIP = ['open','release']
CLOSE_GRIP = ['close','hold','pickup','catch', 'pick up', 'pick']
LIFT_ARM = ['lift', 'pull']
DROP_ARM = ['drop', 'throw', 'leave']
BIN = ['bin', 'release']
BACK = ['back','back to','back to position', 'back to the position']
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.gripper = MediumMotor(OUTPUT_A)
self.bin = LargeMotor(OUTPUT_B)
self.drive_on = MoveTank(OUTPUT_C, OUTPUT_D)
self.steering = MoveSteering(OUTPUT_C, 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
"""
print (directive, file=sys.stderr)
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":
print("(******)")
# Expected params: [direction, duration, speed]
self._move(payload["direction"])
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive), file=sys.stderr)
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 _move(self, direction):
"""
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("********DIRECTION********")
speed = 35
seconds = 3
rotation = 10
turn_speed = 5
left_speed = 20
right_speed = 20
degree = 180
is_blocking = False
if direction in Direction.FORWARD.value:
self.drive_on.on_for_seconds(left_speed, right_speed, 10, brake=True, block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive_on.on_for_seconds(-left_speed, -right_speed,10, brake=True, block=is_blocking)
if direction in Direction.LEFT.value:
self.steering.on_for_seconds(-25, speed,seconds, brake=False, block=is_blocking)
time.sleep(seconds)
self.steering.on_for_seconds(25, speed,seconds, brake=False, block=is_blocking)
time.sleep(seconds)
self.drive_on.on_for_seconds(left_speed, right_speed, 5, brake=True, block=is_blocking)
if direction in Direction.RIGHT.value:
self.steering.on_for_seconds(25, speed, seconds, brake=False, block=is_blocking)
time.sleep(seconds)
self.steering.on_for_seconds(-25, speed,seconds, brake=False, block=is_blocking)
time.sleep(seconds)
self.drive_on.on_for_seconds(left_speed, right_speed, 5, brake=True, block=is_blocking)
if direction in Direction.STOP.value:
self.drive_on.off()
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.MOVE_CIRCLE.value:
self.drive.on_for_seconds(SpeedPercent(int(speed)), SpeedPercent(5), 12)
if command in Command.MOVE_SQUARE.value:
for i in range(4):
self._move("right", 2, speed, is_blocking=True)
if command in Command.PATROL.value:
self.patrol_mode = True
if command in Command.FIRE_ONE.value:
self.weapon.on_for_rotations(SpeedPercent(100), 3)
if command in Command.FIRE_ALL.value:
self.weapon.on_for_rotations(SpeedPercent(100), 10)
if command in Command.OPEN_GRIP.value:
speed = SpeedPercent(10)
print (speed)
rotations = 0.30
is_blocking = True
print("***",slave_out_a,"***")
#slave_out_a.run_timed(time_sp=1000, speed_sp=90)
self.gripper.on_for_rotations(speed, rotations, brake=True, block=is_blocking)
time.sleep(2)
distance = us.value()
self._send_event(EventName.BINSTATUS,{'distance': distance})
if command in Command.CLOSE_GRIP.value:
speed = 10
rotations = -0.30
is_blocking = True
self.gripper.on_for_rotations(SpeedPercent(speed), rotations, brake=True, block=is_blocking)
if command in Command.LIFT_ARM.value:
speed = -5
rotations = 0.80
is_blocking = True
slave_out_a.run_timed(time_sp=2000, speed_sp=-180, stop_action='brake')
if command in Command.DROP_ARM.value:
speed = 5
rotations = 0.80
is_blocking = True
slave_out_a.run_timed(time_sp=2000, speed_sp=180)
if command in Command.BIN.value:
print("ultrasonic sensor", us.value(), file=sys.stderr)
distance = us.value()
self._send_event(EventName.BINSTATUS,{'distance': distance})
speed = -10
rotations = 1.75
is_blocking = True
self.bin.on_for_rotations(SpeedPercent(speed), rotations, brake=False, block=is_blocking)
if command in Command.BACK.value:
speed = 10
rotations = 0.2
is_blocking = True
while not ts.value():
self.bin.on_for_rotations(SpeedPercent(speed), rotations, brake=False, block=is_blocking)
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 _patrol_thread(self):
"""
Performs random movement when patrol mode is activated.
"""
while True:
while self.patrol_mode:
print("Patrol mode activated randomly picks a path", file=sys.stderr)
direction = random.choice(list(Direction))
duration = random.randint(1, 5)
speed = random.randint(1, 4) * 25
while direction == Direction.STOP:
direction = random.choice(list(Direction))
# direction: all except stop, duration: 1-5s, speed: 25, 50, 75, 100
self._move(direction.value[0], duration, speed)
time.sleep(duration)
time.sleep(1)
if __name__ == '__main__':
gadget = MindstormsGadget()
# Set LCD font and turn off blinking LEDs
os.system('setfont Lat7-Terminus12x6')
gadget.leds.set_color("LEFT", "BLACK")
gadget.leds.set_color("RIGHT", "BLACK")
# Startup sequence
gadget.sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
gadget.leds.set_color("LEFT", "GREEN")
gadget.leds.set_color("RIGHT", "GREEN")
# Gadget main entry point
gadget.main()
# Shutdown sequence
gadget.sound.play_song((('E5', 'e'), ('C4', 'e')))
gadget.leds.set_color("LEFT", "BLACK")
gadget.leds.set_color("RIGHT", "BLACK")
_dyynx0bnuf_8u4Xi07d4z.png?auto=compress%2Cformat&w=48&h=48&fit=fill&bg=ffffff)
Comments