97 lines
3.0 KiB
Python
Executable File
97 lines
3.0 KiB
Python
Executable File
#!/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)
|