Turtle Mark II

#!/usr/bin/env python3
# Copyright 2019 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
# 
# You may not use this file except in compliance with the terms and conditions 
# set forth in the accompanying LICENSE.TXT file.
#
# THESE MATERIALS ARE PROVIDED ON AN "AS IS" BASIS. AMAZON SPECIFICALLY DISCLAIMS, WITH 
# RESPECT TO THESE MATERIALS, ALL WARRANTIES, EXPRESS, IMPLIED, OR STATUTORY, INCLUDING 
# THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.

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_D, OUTPUT_B, OUTPUT_C, MoveTank, MoveSteering, SpeedPercent, MediumMotor, follow_for_ms, LargeMotor
from ev3dev2.sensor.lego import TouchSensor, InfraredSensor, ColorSensor 
from sys import stderr
from time import sleep

ir = InfraredSensor()
ts = TouchSensor()
cl = ColorSensor()
cl.mode ='COL-REFLECT' # put the color sensor into COL-REFLECT mode
sound = Sound()

# 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__)

class Direction(Enum):
    """
    The list of directional commands and their variations.
    These variations correspond to the skill slot values.
    """
    FORWARD = ['forward', 'forwards', 'go forward']
    BACKWARD = ['back', 'backward', 'backwards', 'go backward']
    LEFT = ['left', 'go left']
    RIGHT = ['right', 'go right']
    STOP = ['stop', 'brake']

class EventName(Enum):
    """
    The list of custom event name sent from this gadget
    """
    SENTRY = "Sentry"
    PROXIMITY = "Proximity"
    SPEECH = "Speech"

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 = ['up', 'lift', 'lyft']
    FIRE_ALL = ['all shot']
    CLEAN_KITCHEN = ['clean kitchen', 'kitchen', 'clean']
    FIND_ME = ['find', 'find me', 'found me', 'come to me', 'come']
    REMOVE_TRASH = ['remove', 'remove trash', 'trash']


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.drive = MoveTank(OUTPUT_B, OUTPUT_C)
        self.drive_smooth = MoveSteering(OUTPUT_B, OUTPUT_C)
        self.updown = MediumMotor(OUTPUT_D)
        self.motorB = LargeMotor(OUTPUT_B)
        self.motorC = LargeMotor(OUTPUT_C)

        # 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 == "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"])

        except KeyError:
            print("Missing expected parameters: {}".format(directive), file=sys.stderr)

    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 _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.CLEAN_KITCHEN.value:
            ir.mode = 'IR-PROX' # put the infrared sensor into proximity mode.
            #self._send_event(EventName.SPEECH, {'speechOut': "Turtle is coming. What a lovely day!"})
            
            # go straight (start -> hall)
            self.drive.on_for_rotations(100, 100, 10, brake=True)
            
            # turn left (hall -> hall)
            self.drive.on_for_rotations(-100, 100, 1.3, brake=True) # 1.51 was
            
            # hall -> kitchen
            obstacle = 0
            while cl.value() > 15:
                if obstacle == 0:
                    self.drive.on(85, 85) # was 85. TEST
                    if ir.value() < 15:
                        self.drive.off()
                        obstacle = 1 # kitchen door is closed
                         #TODO: ALexa speak
                        sound.speak('It seems the door to the kitchen is closed, please open it')
                    if ir.value() > 15:
                        obstacle = 0
            # black line in the kitchen is reached
            
            # one body size forward (was 1.7)
            self.drive.on_for_rotations(50, 50, 1.3, brake=True)
            
            # turn right until black line is reached
            while cl.value() > 15:
                self.drive.on_for_degrees(100, -100, 20)
            self.drive.off()
            
            # follow line Part_1: Turtle alignment for 6 seconds 
            self.drive.cs = ColorSensor()
            #  kp=2 not working
            self.drive.follow_line(
                kp=11.3, ki=0.05, kd=3.2,
                speed=SpeedPercent(30),
                follow_for=follow_for_ms,
                ms=2000
            )

            # down and on the vacuum cleaner
            self.updown.on_for_rotations(SpeedPercent(-50), 2) 

            # follow line Part_2: basic cleaning (10 seconds)
            self.drive.follow_line(
                kp=11.3, ki=0.05, kd=3.2,
                speed=SpeedPercent(30),
                follow_for=follow_for_ms,
                ms=12000
            )
            #self.drive.off()

            # follow line Part_3: prepare to stop (reach kitchen corner)
            while ir.value() > 20:
                self.drive.follow_line(
                    kp=10.0, ki=0.05, kd=3.2,
                    speed=SpeedPercent(20),
                    follow_for=follow_for_ms,
                    ms=500
                )
            self.drive.off()

            # up and off the vacuum cleaner
            self.updown.on_for_rotations(SpeedPercent(100), 2) 

            # go back to the kitchen enterency
            self.drive.on_for_rotations(-50, -50, 18, brake=True)

            # turn to the hall
            self.drive.on_for_rotations(100, -100, 1.3, brake=True)
            self.motorB.wait_while('running')
            # kitchen -> hall
            self.drive.on_for_rotations(50, 50, 5, brake=True)
            self.motorB.wait_while('running')
            self.drive.off()

            # go to start point (beacon)
            flagTarget = 0 # 1 - beacon is reached
            ir.mode = 'IR-SEEK'
            distance = ir.heading_and_distance()
            while flagTarget != 1: 
                self.drive_smooth.on(distance[0]*4, 50)
                distance = ir.heading_and_distance()
                if distance[1] > -5 and distance[1] < 5:
                    flagTarget = 1            
            self.drive_smooth.off()
            sound.speak('The kitchen is clean! Lets start the party!') #TODO: Alexa talk
            
        if command in Command.FIND_ME.value:
            ir.mode = 'IR-SEEK'
            sound.speak('On my way!')
            # start -> kitchen
            # go to beacon
            distance = ir.heading_and_distance()
            #if distance==None:
            # distance() returns None if no beacon detected
            #else:
                        # ir sensor update (TODO: maybe delete this while) 
            count = 0 # for first distance[1] error equalization
            while count < 10:
                distance = ir.heading_and_distance()
                count += 1

            count = 0 # for second distance[1] error equalization
            flagTarget = 0 # 1 - beacon is reached
            pos1_distance = 0 # average value of istance[1] in position 1 (initial)
            pos2_distance = 0  # average value of istance[1] in position 2 (after go back for 3 rotation)

            while count < 10: # get average value of istance[1] in position 1 (initial)
                distance = ir.heading_and_distance()
                pos1_distance += distance[1]
                count += 1
            pos1_distance = pos1_distance / 10

            self.drive.on_for_rotations(-40, -40, 3) # change Turtle location from position1 to position2
            self.motorB.wait_while('running')
            count = 0 # for third distance[1] error equalization

            while count < 10: # get average value of istance[1] in position 2 
                distance = ir.heading_and_distance()
                pos2_distance += distance[1]
                count += 1
            pos2_distance = pos2_distance / 10
            
            if pos2_distance <= pos1_distance:
                self.drive.on_for_rotations(40, -40, 3) # turn around Turtle
                self.motorB.wait_while('running')

            while flagTarget != 1: 
                self.drive_smooth.on(distance[0]*4, 50)
                distance = ir.heading_and_distance()
                if distance[1] > -2 and distance[1] < 2:
                    flagTarget = 1
            flagTarget = 0
            self.drive.off()
            sound.speak('Hey! Turtle arrived!')

        if command in Command.REMOVE_TRASH.value:
            ir.mode = 'IR-SEEK'
            index = 0 # for movements
            movements = [] # history of all distance[0] during the journey to the beacon

            # hall - > kitchen take from CLEAN_KITCHEN            
            # go to beacon from black line
            distance = ir.heading_and_distance()
            sound.speak('Garbage spot detected!') #TODO: Alexa talk
            flagTarget = 0
            while flagTarget != 1: 
                self.drive_smooth.on(distance[0]*4, 50)
                movements.append(distance[0])
                index = index + 1
                distance = ir.heading_and_distance()
                if distance[1] > -5 and distance[1] < 5:
                    flagTarget = 1
            self.drive_smooth.off

            # clean area
            # down and on the vacuum cleaner
            self.updown.on_for_rotations(SpeedPercent(-50), 2) 
			# one body size forward
            self.drive.on_for_rotations(50, 50, 2, brake=True)
            self.motorB.wait_while('running')
			# go back to area start point
            self.drive.on_for_rotations(-50, -50, 2, brake=True)
            self.motorB.wait_while('running')
            # up and off the vacuum cleaner
            self.updown.on_for_rotations(SpeedPercent(100), 2)
            sound.speak('Garbage cleaned, no more litter please.') #TODO: Alexa talk

            # go to kitchen start point
            i = len(movements)-1
            while i != -1:
                self.drive_smooth.on(movements[i]*3, -50)
                sleep(0.005) #with cleaner 0.005, without cleaner 0.0055
                i = i - 1
            self.drive_smooth.off

            #turn around
            self.drive.on_for_rotations(-100, 100, 2.5, brake=True) 
            self.motorB.wait_while('running')
            #TODO: new hall and beacon home
            # kitchen -> hall
            self.drive.on_for_rotations(50, 50, 5, brake=True)
            # go to start point (beacon)
            flagTarget = 0 # 1 - beacon is reached
            ir.mode = 'IR-SEEK'
            distance = ir.heading_and_distance()
            while flagTarget != 1: 
                self.drive_smooth.on(distance[0]*4, 50)
                distance = ir.heading_and_distance()
                if distance[1] > -5 and distance[1] < 5:
                    flagTarget = 1            
            self.drive_smooth.off()
            sound.speak('Garbage removed! Lets have fun!') #TODO: Alexa talk

        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:
            # Set patrol mode to resume patrol thread processing
            self.patrol_mode = True

        if command in Command.FIRE_ONE.value:
            self.updown.on_for_rotations(SpeedPercent(100), 10)

        if command in Command.FIRE_ALL.value:
            self.updown.on_for_rotations(SpeedPercent(100), 10)

    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)

    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)

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")