armCtl/armCtl.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""armcCtl.py: control your robot arm."""

__author__ = "Kevin Matz"
__copyright__ = "Copyright 2019, Company 235, LLC"
__credits__ = ["Kevin Matz"]

__license__ = "MIT"
__version__ = "0.1"
__maintainer__ = "Kevin Matz"
__email__ = "kevin@company235.com"

__status__ = "Prototype"

import configparser
import logging
import sys

from firmware.robot import Robot
from antlr4 import CommonTokenStream
from antlr4 import InputStream
from antlr4 import ParseTreeWalker
from antlr4.error.ErrorListener import ErrorListener
from language.ArmControlLexer import ArmControlLexer
from language.ArmControlParser import ArmControlParser
from ArmCtlListener import ArmCtlListener


# define an error listener that raises SyntaxError exceptions
class SyntaxErrorListener(ErrorListener):
    def syntaxError(self, recognizer, offendingSymbol, line, column, msg, e):
        raise SyntaxError("line "+str(line)+":"+str(column)+" "+msg)


# instantiate a robot
r = Robot()

# setup logging
logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
ch = logging.StreamHandler()                # create console handler and
ch.setLevel(logging.INFO)                   # set console log level to INFO
logger.addHandler(ch)                       # add console log handler

# load configuration
config = configparser.ConfigParser(allow_no_value=True)
config.read('robot.cfg')                    # open config file
r.config = config['robot']                  # robot section of config file
r.preset = config['presets']                # presets section of config file
r.target = r.read_preset(r.preset.get('home', None))    # get default position
r.start()                                   # open port, start read thread

script = ArmCtlListener(r)
walker = ParseTreeWalker()


def command(text):
    input_stream = InputStream(text)
    lexer = ArmControlLexer(input_stream)
    stream = CommonTokenStream(lexer)
    parser = ArmControlParser(stream)
    parser._listeners.append(SyntaxErrorListener())
    try:
        tree = parser.prog()
        walker.walk(script, tree)
    except SyntaxError as e:
        logger.debug(e)     # antlr internal listener prints the error
#                           # log it to the debug logger anyway


if __name__ == '__main__':
    if len(sys.argv) > 1:
        logger.debug("found macro at argv[1]")
        with open(sys.argv[1]) as f:
            lines = f.readlines()
        for line in lines:
            try:
                print("command#", line.strip())
                command(line)
            except (KeyboardInterrupt):
                break
        print("command#", "exit")
    else:
        import readline     # for input history and line editing
        while True:
            try:
                text = input("command# ")
            except (KeyboardInterrupt, EOFError):
                text = 'exit'
                print(text)
            if text == 'exit' or text == 'quit':
                break
            else:
                command(text)