85 lines
2.5 KiB
Python
85 lines
2.5 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('{') and data.endswith('}'):
|
||
|
data = data.lstrip('{').rstrip('}')
|
||
|
else:
|
||
|
return
|
||
|
data = data.split(',')
|
||
|
self.angle = [int(hex, 16) for hex in data]
|
||
|
|
||
|
def serial_read_worker(self, ser):
|
||
|
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=serial_read_worker,
|
||
|
args=(serial_port))
|
||
|
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.")
|