diff --git a/databaseCreatorv2.py b/databaseCreatorv2.py index 52b651d..2c6102c 100644 --- a/databaseCreatorv2.py +++ b/databaseCreatorv2.py @@ -3,14 +3,25 @@ import math class Robot: class Servo: - def __init__(self, angle, geometry): + def __init__(self, angle, geometry, previous_servos=None): self.deg = angle - self.rad = self._get_rad() + 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 - def _get_rad(self): - rad = self.deg * math.pi / 180 + 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.total_angle_rad = self._get_rad(self.total_angle_deg) + + def _get_rad(self, deg): + rad = deg * math.pi / 180 return rad def set_angle(self, angle): @@ -36,6 +47,12 @@ class Robot: self.min = _min self.mid = mid + 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] @@ -51,9 +68,21 @@ class Robot: self.servo2 = s2 self.servo3 = s3 self.coordinatesystem = coordinatessystem + self.arm1 = None + self.arm2 = None + self.arm3 = None + # get position based on servos # get efficency based on servos + 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) + def _get_position(self): return x, y @@ -61,6 +90,10 @@ class Robot: return effi -test = Robot(Robot.Servo(1, Robot.Servo.Geometry(0.55, 2.3, 1.4)), Robot.Servo(2, Robot.Servo.Geometry(2.5, 0.55, 1.55)), - Robot.Servo(3, Robot.Servo.Geometry(0.7, 2.25, 2.25)), Robot.CoordinateSystem([-100, 100], [-100, 100])) +test = Robot(Robot.Servo(1, Robot.Servo.Geometry(0.55, 2.3, 1.4)), + Robot.Servo(2, Robot.Servo.Geometry(2.5, 0.55, 1.55)), + Robot.Servo(3, Robot.Servo.Geometry(0.7, 2.25, 2.25)), + Robot.CoordinateSystem([-100, 100], [-100, 100])) +test.init_depending(Robot.Arm(test.servo1, 10.26, 0.84), Robot.Arm(test.servo2, 9.85), Robot.Arm(test.servo3, 12, 9), + None, [test.servo1], [test.servo1, test.servo2]) print("finished")