88 lines
2.6 KiB
Python
88 lines
2.6 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
"""robot.py: class up 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 logging
|
|
import configparser
|
|
import serial
|
|
import threading
|
|
|
|
logger = logging.getLogger('__main__')
|
|
|
|
|
|
class Robot:
|
|
def __init__(self):
|
|
self.serial_port = None
|
|
self.config = None
|
|
self.serial_read_thread = None
|
|
self.angle = []
|
|
self.target = []
|
|
|
|
def move(self, coords=None):
|
|
if coords is not None:
|
|
self.target = coords
|
|
hex = [hex(dec).lstrip("0x").rstrip("L") for dec in self.target]
|
|
str = '{'+','.join(hex)+'}'
|
|
str = str.lower()
|
|
try:
|
|
self.serial_port.write(str)
|
|
except serial.serialutil.SerialTimeoutExceptionn:
|
|
pass
|
|
|
|
def isMoving(self):
|
|
if len(self.angle) != len(self.target):
|
|
values = []
|
|
for i in range(len(self.angle)):
|
|
try:
|
|
values.append(self.target[i])
|
|
except Exception as e:
|
|
values.append(0)
|
|
return self.angle != values
|
|
return self.angle != self.target
|
|
|
|
def serial_read_handler(self, data):
|
|
data = data.strip()
|
|
if data.startswith(b'{') and data.endswith(b'}'):
|
|
data = data.lstrip(b'{').rstrip(b'}')
|
|
else:
|
|
return # malformed packet
|
|
data = data.split(b',')
|
|
try:
|
|
self.angle = [int(hex, 16) for hex in data]
|
|
except ValueError:
|
|
return # malformed packet
|
|
|
|
def serial_read_worker(self):
|
|
while True:
|
|
data = self.serial_port.readline()
|
|
self.serial_read_handler(data)
|
|
|
|
def start(self):
|
|
# Start a serial read thread, if the serial port can be opened.
|
|
try:
|
|
self.serial_port = serial.Serial(self.config.get("port", None),
|
|
self.config.get("baud", 9600),
|
|
timeout=0)
|
|
self.serial_read_thread = threading.Thread(
|
|
target=self.serial_read_worker,
|
|
daemon=True)
|
|
self.serial_read_thread.start()
|
|
except NameError:
|
|
logger.error("ERROR: configuration hasn't been set!")
|
|
raise
|
|
except serial.serialutil.SerialException:
|
|
logger.error("ERROR: Unable to start serial port.")
|