222 lines
7.8 KiB
Python
222 lines
7.8 KiB
Python
import math
|
|
|
|
|
|
class Robot:
|
|
class Servo:
|
|
def __init__(self, angle, geometry):
|
|
self.deg = angle
|
|
self.rad = self._get_rad(self.deg)
|
|
self.geometry = geometry
|
|
self.max, self.min = self._get_max_min()
|
|
self.previous_servos = None
|
|
self.total_angle_deg = None
|
|
self.total_angle_rad = None
|
|
self.delta = None
|
|
self.ms = None
|
|
|
|
def add_previous_servos(self, previous_servos=None):
|
|
self.previous_servos = previous_servos
|
|
self.total_angle_deg = self.deg
|
|
if previous_servos:
|
|
for servo in self.previous_servos:
|
|
self.total_angle_deg += servo.deg
|
|
self.delta = self.deg + previous_servos[-1].deg
|
|
if self.delta < 0:
|
|
self.delta *= -1
|
|
else:
|
|
self.delta = self.deg
|
|
self.total_angle_rad = self._get_rad(self.total_angle_deg)
|
|
|
|
def _get_rad(self, deg):
|
|
rad = deg * math.pi / 180
|
|
return rad
|
|
|
|
def calc_previous(self):
|
|
if self.previous_servos:
|
|
self.total_angle_deg = 0
|
|
for servo in self.previous_servos:
|
|
self.total_angle_deg += servo.deg
|
|
self.delta = self.deg + self.previous_servos[-1].deg
|
|
if self.delta < 0:
|
|
self.delta *= -1
|
|
else:
|
|
self.delta = self.deg
|
|
self.total_angle_deg = self.deg
|
|
if self.delta < 0:
|
|
self.delta *= -1
|
|
self.total_angle_rad = self._get_rad(self.total_angle_deg)
|
|
|
|
def set_angle(self, angle):
|
|
self.deg = angle
|
|
self.rad = self._get_rad(self.deg)
|
|
self.calc_previous()
|
|
|
|
def _get_angle(self, ms):
|
|
val_min = self.geometry.min
|
|
val_max = self.geometry.max
|
|
val_mid = self.geometry.mid
|
|
change = 1 / 90
|
|
if val_min > val_max:
|
|
change *= -1
|
|
angle = (ms - val_mid) / change
|
|
return angle
|
|
|
|
def _get_max_min(self):
|
|
return self._get_angle(self.geometry.max), self._get_angle(self.geometry.min)
|
|
|
|
def set_ms(self, ms):
|
|
self.ms = ms
|
|
self.deg = self._get_angle(self.ms)
|
|
self.rad = self._get_rad(self.deg)
|
|
self.calc_previous()
|
|
|
|
def get_ms(self, calc=False):
|
|
if calc:
|
|
change = 1 / 90
|
|
if self.geometry.min > self.geometry.max:
|
|
change *= -1
|
|
self.ms = (self.deg * change + self.geometry.mid)
|
|
return self.ms
|
|
|
|
class Geometry:
|
|
def __init__(self, _max, _min, mid):
|
|
self.max = _max
|
|
self.min = _min
|
|
self.mid = mid
|
|
|
|
def is_inside(self, ms):
|
|
return self.min <= ms <= self.max or self.max <= ms <= self.min
|
|
|
|
class Arm:
|
|
def __init__(self, attatched_to, length, height=0.0):
|
|
self.attachted_to = attatched_to
|
|
self.length = length
|
|
self.height = height
|
|
|
|
class CoordinateSystem:
|
|
def __init__(self, x, y):
|
|
self.xmin = x[0]
|
|
self.xmax = x[1]
|
|
self.ymin = y[0]
|
|
self.ymax = y[1]
|
|
|
|
def is_inside(self, x, y):
|
|
return self.xmin <= x <= self.xmax and self.ymin <= y <= self.ymax
|
|
|
|
class Database:
|
|
|
|
class Entry:
|
|
def __init__(self, x, y, effi, s1, s2, s3):
|
|
self.x = x
|
|
self.y = y
|
|
self.effi = effi
|
|
self.s1 = s1.deg
|
|
self.s2 = s2.deg
|
|
self.s3 = s3.deg
|
|
|
|
def pos_is_equal_to(self, other_entry):
|
|
return self.x == other_entry.x and self.y == other_entry.y
|
|
|
|
def other_efficency_better(self, other_entry):
|
|
return self.effi > other_entry.effi
|
|
|
|
def __init__(self):
|
|
self.database = []
|
|
|
|
def _is_contained(self, entry):
|
|
for i, existing_entry in enumerate(self.database):
|
|
if existing_entry.pos_is_equal_to(entry):
|
|
return existing_entry, i
|
|
return None, 0
|
|
|
|
def get_entry(self, x, y):
|
|
wanted = self.Entry(x, y, 0, Robot.Servo(0, Robot.Servo.Geometry(0, 0, 0)),
|
|
Robot.Servo(0, Robot.Servo.Geometry(0, 0, 0)),
|
|
Robot.Servo(0, Robot.Servo.Geometry(0, 0, 0)))
|
|
_return, i = self._is_contained(wanted)
|
|
return _return
|
|
|
|
def add_entry(self, entry):
|
|
contained, i = self._is_contained(entry)
|
|
if contained:
|
|
if contained.other_efficency_better(entry):
|
|
self.database[i] = entry
|
|
else:
|
|
self.database.append(entry)
|
|
|
|
def serialize(self):
|
|
string = ""
|
|
for entry in self.database:
|
|
string += "{},{}:{},{},{}\n".format(str(entry.x), str(entry.y), str(entry.s1), str(entry.s2),
|
|
str(entry.s3))
|
|
return string
|
|
|
|
def deserialize(self, string):
|
|
entries = string.split("\n")
|
|
for entry in entries:
|
|
if entry:
|
|
coordinates = entry.split(":")[0].split(',')
|
|
servos = entry.split(":")[1].split(',')
|
|
new = self.Entry(float(coordinates[0]), float(coordinates[1]), -1,
|
|
Robot.Servo(float(servos[0]), Robot.Servo.Geometry(0, 0, 0)),
|
|
Robot.Servo(float(servos[1]), Robot.Servo.Geometry(0, 0, 0)),
|
|
Robot.Servo(float(servos[2]), Robot.Servo.Geometry(0, 0, 0)))
|
|
self.database.append(new)
|
|
|
|
def __init__(self, s1, s2, s3, coordinatessystem):
|
|
self.servo1 = s1
|
|
self.servo2 = s2
|
|
self.servo3 = s3
|
|
self.coordinatesystem = coordinatessystem
|
|
self.arm1 = None
|
|
self.arm2 = None
|
|
self.arm3 = None
|
|
self.data = self.Database()
|
|
self.x = None
|
|
self.y = None
|
|
self.efficency = None
|
|
|
|
def init_depending(self, a1, a2, a3, s1_prev=None, s2_prev=None, s3_prev=None):
|
|
self.arm1 = a1
|
|
self.arm2 = a2
|
|
self.arm3 = a3
|
|
self.servo1.add_previous_servos(s1_prev)
|
|
self.servo2.add_previous_servos(s2_prev)
|
|
self.servo3.add_previous_servos(s3_prev)
|
|
self.x, self.y = self._get_position()
|
|
self.efficency = self._get_efficency()
|
|
|
|
def _get_position(self):
|
|
x = 0
|
|
y = 0
|
|
for arm in [self.arm1, self.arm2, self.arm3]:
|
|
x += math.sin(arm.attachted_to.total_angle_rad) * arm.length + math.sin(
|
|
arm.attachted_to.total_angle_rad + 1.5708) * arm.height
|
|
y += math.cos(arm.attachted_to.total_angle_rad) * arm.length + math.cos(
|
|
arm.attachted_to.total_angle_rad + 1.5708) * arm.height
|
|
return x, y
|
|
|
|
def _get_efficency(self):
|
|
effi = 0
|
|
multipliers = [8, 4, 2]
|
|
for i, servo in enumerate([self.servo1, self.servo2, self.servo3]):
|
|
effi += servo.delta * multipliers[i]
|
|
return effi
|
|
|
|
def calculate(self):
|
|
for servo in [self.servo1, self.servo2, self.servo3]:
|
|
servo.calc_previous()
|
|
self.x, self.y = self._get_position()
|
|
self.efficency = self._get_efficency()
|
|
self.x = self._round(self.x)
|
|
self.y = self._round(self.y)
|
|
|
|
def _round(self, number, n=None):
|
|
if not n:
|
|
n = 0
|
|
digits = number - math.floor(number)
|
|
if digits < 2:
|
|
return math.floor(number * 10 ** n) / 10 ** n
|
|
elif digits > 8:
|
|
return math.ceil(number * 10 ** n) / 10 ** n
|