firehero

#!/usr/bin/env python3
# Copyright 2019 David Springer

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, SpeedPercent, MediumMotor

# Setze den Loglevel auf INFO um Meldungen im Logfile zu sehen und DEBUG um noch mehr Infos zu bekommen
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):
   
    VOR = ['vor', 'vorwrts']
    ZURUECK = ['zurck', 'rckwrts']
    LINKS = ['links', 'drehe links', 'links um', 'links drehen']
    RECHTS = ['drehe rechts', 'rechts um', 'rechts drehen', 'rechts']
    STOP = ['bremse', 'notaus', 'bremsen']

#rechts um 90 grad drehen!!!!!

class Command(Enum):
    """
    Eine Liste der Befehle und deren auslsenden Worte.
    Diese Variationen decken sich mit den Slot Varianten.
    """
    KREISEN = ['kreisen', 'kreis']
    PATROL = ['patrolieren']
    SCHWENKEN = ['lschrichtung ndern', 'lscharm schwenken']
    HYDROSCHILD = ['wasservorhang', 'hydroschild', 'schutzmodus']
    GREIFER_AUF = ['greifer auf']
    GREIFER_ZU = ['greifer zu']
    FOLGETONHORN = ['folgetonhorn']


class MindstormsGadget(AlexaGadget):
    """
    Ein Mindstorm Gadget, welches Bewegungen durchfhrt die mittels Stimme befohlen werden
    Es gibt zwei Befehlsarten, direkte Bewegungen und vordefinierte Bewegungen.
    """

    def __init__(self):
        """
        Fhrt Alexa Gadget Initialisierung der Routinen aus und weist ev3dev Ressourcen zu.
        """
        super().__init__()

        # Gadget state
        self.patrol_mode = False

        # Ev3dev initialization
        self.leds = Leds()
        self.sound = Sound()
        self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
        self.weapon = MediumMotor(OUTPUT_A)
        self.greifer = MediumMotor(OUTPUT_D)

        # Start threads
        threading.Thread(target=self._patrol_thread, daemon=True).start()

    def on_connected(self, device_addr):
        """
        Gadget verbindet sich mit dem Echo Gert
        :param device_addr: die Adresse des verbundenen Gerts
        """
        #wenn fehler orange geht nicht!
        self.leds.set_color("LEFT", "GREEN")
        self.leds.set_color("RIGHT", "GREEN")
        logger.info("{} verbunden mit ECHO Gert".format(self.friendly_name))

    def on_disconnected(self, device_addr):
        """
        Gadget unterbricht Verbindung zum Echo Gert
        :param device_addr: die Adresse des vorher verbundenen Gerts
        """
        self.leds.set_color("LEFT", "BLACK")
        self.leds.set_color("RIGHT", "BLACK")
        logger.info("{} verbindung fehlgeschlagen/abbgebrochen".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("Folgender Parameter fehlt oder ist nicht vorhanden: {}".format(directive), file=sys.stderr)

    def _move(self, direction, duration: int, speed: int, is_blocking=False):
        """
        Behandelt move Kommandos.
        Rechts und links Bewegungen knnen je nach Oberflche unter- oder bersteuern.
        
        :param direction: Bewegungsrichtung
        :param duration: Dauer der Bewegung in Sekunden
        :param speed: Geschwindigkeit in Prozent als Integerwert
        :param is_blocking: wenm gesetzt luft Motor den duration Wert bis neues Kommando gegeben werden kann
        """
        print("Move Befehl: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr)
        if direction in Direction.VOR.value:
            self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking)

        if direction in Direction.ZURUECK.value:
            self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking)

        if direction in (Direction.RECHTS.value + Direction.LINKS.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):
        """
        Behandelt die Vorgabewerte
        :param command: Vorgabewert fr das Kommando
        :param speed: Vorgabewert fr Geschwindigkeit falls anwendbar
        """
        print("Aktiviere Kommando: ({}, {})".format(command, speed), file=sys.stderr)
        if command in Command.KREISEN.value:
            self.drive.on_for_seconds(SpeedPercent(int(speed)), SpeedPercent(5), 12)

        if command in Command.PATROL.value:
            # Set patrol mode to resume patrol thread processing
            self.patrol_mode = True
            
        if command in Command.SCHWENKEN.value:
            self.weapon.on_for_rotations(SpeedPercent(30), 3)

        if command in Command.HYDROSCHILD.value:
            self.weapon.on_for_rotations(SpeedPercent(100), 30)

        if command in Command.GREIFER_AUF.value:
            self.greifer.on_for_rotations(SpeedPercent(-5), 1.5)

        if command in Command.GREIFER_ZU.value:
            self.greifer.on_for_rotations(SpeedPercent(5), 1.5)

        if command in Command.FOLGETONHORN.value:
            self.sound.play_file('martinshorn.wav', volume=100)

    def _turn(self, direction, speed):
        """
        Dreht basierend auf Geschwindigkeit und Richtung.
        Kalibriert fr harte Oberflchen.
        :param direction: Drehrichtung
        :param speed: Drehgeschwindigkeit
        """
        if direction in Direction.LINKS.value:
            self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)

        if direction in Direction.RECHTS.value:
            self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)

    def _patrol_thread(self):
        """
        Fhrt im Patrolliermodus zufllige Bewegungen aus.
        """
        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()

    # Setzt die Schriftart am LCD Monitor und schaltet LEDs ab
    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", "ORANGE")
    gadget.leds.set_color("RIGHT", "ORANGE")
 
    # 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")