120 lines
4.2 KiB
Python
120 lines
4.2 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
"""ArmCtlListener.py: python listener for ArmControl scripts."""
|
|
|
|
__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 logging
|
|
from language.ArmControlParser import ArmControlParser
|
|
from language.ArmControlListener import ArmControlListener
|
|
from firmware.robot import Robot
|
|
from time import sleep
|
|
|
|
logger = logging.getLogger('__main__')
|
|
|
|
|
|
class ArmCtlListener(ArmControlListener):
|
|
def __init__(self, robot):
|
|
self.r = robot
|
|
|
|
# Enter a parse tree produced by ArmControlParser#statement.
|
|
def enterStatement(self, ctx: ArmControlParser.StatementContext):
|
|
ctx.doMove = False
|
|
|
|
# Exit a parse tree produced by ArmControlParser#statement.
|
|
def exitStatement(self, ctx: ArmControlParser.StatementContext):
|
|
if ctx.doMove:
|
|
self.r.move()
|
|
|
|
# Exit a parse tree produced by ArmControlParser#Go.
|
|
def exitGo(self, ctx: ArmControlParser.GoContext):
|
|
if ctx.coords() is not None:
|
|
self.r.target = ctx.coords().values
|
|
ctx.parentCtx.doMove = True
|
|
return
|
|
if ctx.name() is None:
|
|
logger.error("ERROR: Go command with no name?")
|
|
return
|
|
if ctx.value() is None:
|
|
preset = self.r.preset.get(ctx.name().getText(), None)
|
|
if preset is None:
|
|
logger.error("ERROR: Preset not found")
|
|
return
|
|
preset = self.r.read_preset(preset)
|
|
if preset is None:
|
|
logger.error("ERROR: preset invalid")
|
|
return
|
|
self.r.target = preset
|
|
ctx.parentCtx.doMove = True
|
|
return
|
|
|
|
index = int(self.r.config.get(ctx.name().getText(), -1))
|
|
if index is -1:
|
|
logger.error("Error: axis name not recognized.")
|
|
return
|
|
self.r.target[index] = ctx.value().value
|
|
ctx.parentCtx.doMove = True
|
|
|
|
# Exit a parse tree produced by ArmControlParser#Pause.
|
|
def exitPause(self, ctx: ArmControlParser.PauseContext):
|
|
if ctx.parentCtx.doMove:
|
|
ctx.parentCtx.doMove = False
|
|
self.r.move()
|
|
sleep(ctx.number().value)
|
|
|
|
# Exit a parse tree produced by ArmControlParser#Wait.
|
|
def exitWait(self, ctx: ArmControlParser.WaitContext):
|
|
if ctx.parentCtx.doMove:
|
|
ctx.parentCtx.doMove = False
|
|
self.r.move()
|
|
while self.r.isMoving():
|
|
sleep(.0015)
|
|
if (ctx.number() is not None):
|
|
sleep(ctx.number().value)
|
|
|
|
# Exit a parse tree produced by ArmControlParser#WhereAmI.
|
|
def exitWhereAmI(self, ctx: ArmControlParser.WhereAmIContext):
|
|
print(self.r.to_preset_string(self.r.target))
|
|
|
|
# Enter a parse tree produced by ArmControlParser#coords.
|
|
def enterCoords(self, ctx: ArmControlParser.CoordsContext):
|
|
ctx.values = self.r.target
|
|
ctx.index = 0
|
|
|
|
# Exit a parse tree produced by ArmControlParser#value.
|
|
def exitValue(self, ctx: ArmControlParser.ValueContext):
|
|
if isinstance(ctx.parentCtx, ArmControlParser.CoordsContext):
|
|
if ctx.parentCtx.index < len(ctx.parentCtx.values):
|
|
ctx.parentCtx.values[ctx.parentCtx.index] = ctx.value
|
|
|
|
# Exit a parse tree produced by ArmControlParser#number.
|
|
def exitNumber(self, ctx: ArmControlParser.NumberContext):
|
|
try:
|
|
ctx.value = int(ctx.getText())
|
|
except ValueError:
|
|
ctx.value = float(ctx.getText())
|
|
if isinstance(ctx.parentCtx, ArmControlParser.ValueContext):
|
|
ctx.parentCtx.value = ctx.value
|
|
|
|
# Exit a parse tree produced by ArmControlParser#hexidecimal.
|
|
def exitHexidecimal(self, ctx: ArmControlParser.HexidecimalContext):
|
|
ctx.value = int(ctx.getText(), 16)
|
|
if isinstance(ctx.parentCtx, ArmControlParser.ValueContext):
|
|
ctx.parentCtx.value = ctx.value
|
|
|
|
# Exit a parse tree produced by ArmControlParser#seperator.
|
|
def exitSeperator(self, ctx: ArmControlParser.SeperatorContext):
|
|
if isinstance(ctx.parentCtx, ArmControlParser.CoordsContext):
|
|
ctx.parentCtx.index += 1
|