From 71895226c87b0a06cc42e29a898fbd14492e4fed Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Tue, 14 May 2019 06:52:53 +0200
Subject: [PATCH 01/30] imports and model described

---
 terminalControl.py | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index 92a8a90..0e3fc1c 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -4,9 +4,16 @@ import sys
 import math
 import time
 import csv
+from RobotLib import *
 
 pwm = Adafruit_PCA9685.PCA9685(address=0x41)
 
+joy_it = 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]))
+joy_it.init_depending(Robot.Arm(joy_it.servo1, 10.26, 0.84), Robot.Arm(joy_it.servo2, 9.85),
+                      Robot.Arm(joy_it.servo3, 12, 9), None, [joy_it.servo1], [joy_it.servo1, joy_it.servo2])
 
 # the values for all the servos (are different for each robot)
 geometry = {
-- 
2.39.5


From 66c0f8fb7ee0c8c9141898ed8f96d5bf664124e3 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Thu, 16 May 2019 19:22:49 +0200
Subject: [PATCH 02/30] changed large parts of terminal Control to them using
 robotlib therefore also added functions to robotlib

---
 RobotLib.py        |  20 ++-
 terminalControl.py | 348 ++++++++++++++++++++-------------------------
 2 files changed, 173 insertions(+), 195 deletions(-)

diff --git a/RobotLib.py b/RobotLib.py
index 7aedfeb..d74531e 100644
--- a/RobotLib.py
+++ b/RobotLib.py
@@ -3,7 +3,7 @@ import math
 
 class Robot:
     class Servo:
-        def __init__(self, angle, geometry, previous_servos=None):
+        def __init__(self, angle, geometry):
             self.deg = angle
             self.rad = self._get_rad(self.deg)
             self.geometry = geometry
@@ -12,6 +12,7 @@ class Robot:
             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
@@ -59,12 +60,29 @@ class Robot:
         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
+
     class Arm:
         def __init__(self, attatched_to, length, height=0.0):
             self.attachted_to = attatched_to
diff --git a/terminalControl.py b/terminalControl.py
index 0e3fc1c..11324eb 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -1,12 +1,12 @@
 from __future__ import division
 import Adafruit_PCA9685
 import sys
-import math
 import time
 import csv
 from RobotLib import *
 
 pwm = Adafruit_PCA9685.PCA9685(address=0x41)
+pwm.set_pwm_freq(50)
 
 joy_it = 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)),
@@ -16,43 +16,154 @@ joy_it.init_depending(Robot.Arm(joy_it.servo1, 10.26, 0.84), Robot.Arm(joy_it.se
                       Robot.Arm(joy_it.servo3, 12, 9), None, [joy_it.servo1], [joy_it.servo1, joy_it.servo2])
 
 # the values for all the servos (are different for each robot)
-geometry = {
-        "servo0min": 0.5,
-        "servo0max": 2.5,
-        "servo0mid": 1.85,
-        "servo1min": 2.3,
-        "servo1max": 0.55,
-        "servo1mid": 1.4,
-        "servo2min": 0.55,
-        "servo2max": 2.5,
-        "servo2mid": 1.55,
-        "servo3min": 2.25,
-        "servo3max": 0.7,
-        "servo3mid": 2.25,
-        "servo4min": 0.5,
-        "servo4max": 2.5,
-        "servo4mid": 1.5
-    }
-# the length of the arms (same for each joy it robot)
-arms = {
-    0: 0.84,
-    1: 10.26,
-    2: 9.85,
-    "claw": 12,
-    "height": 9
-}
+servo0min = 0.5
+servo0max = 2.5
+servo0mid = 1.85
+servo4min = 0.5
+servo4max = 2.5
+servo4mid = 1.5
 
 
-def summe(i):
-    """
-    Returns the sum of a list
-    :param i: list with float, int
-    :return: int sum
-    """
-    r = 0
-    for element in i:
-        r += element
-    return r
+class argvReader:
+    def __init__(self, argv):
+        self.argv = argv
+
+    def home(self):
+        for i, servo in enumerate([joy_it.servo1, joy_it.servo2, joy_it.servo3]):
+            servo.set_angle(0)
+            set_servo_pulse(i+1, servo.get_ms(True))
+
+    def servo(self):
+        servo_int = int(self.argv[2])
+        pos = float(self.argv[4])
+        if servo_int == 1:
+            servo = joy_it.servo1
+        elif servo_int == 2:
+            servo = joy_it.servo2
+        elif servo_int == 3:
+            servo = joy_it.servo3
+        if servo_int <= 3:
+            if not servo.geometry.is_inside(pos):
+                print("Position to big or to small")
+                sys.exit()
+            elif 0 < servo < 5:
+                print("Servo is not on robotarm")
+            else:
+                servo.set_ms(pos)
+                set_servo_pulse(servo_int, servo.ms)
+        elif servo_int <= 5:
+            set_servo_pulse(servo_int, pos)
+        else:
+            sys.exit()
+        print(str(servo_int) + ": " + str(pos))
+
+    def list(self):
+        commands = self.argv[2:]
+        for entry in commands:
+            servo_int = int(entry.split(',')[0])
+            pos = float(entry.split(',')[1])
+            if servo_int == 1:
+                servo = joy_it.servo1
+            elif servo_int == 2:
+                servo = joy_it.servo2
+            elif servo_int == 3:
+                servo = joy_it.servo3
+            if 0 < servo_int <= 3:
+                if not servo.geometry.is_inside(pos):
+                    print("Position to big or to small")
+                    sys.exit()
+                elif 0 < servo < 5:
+                    print("Servo is not on robotarm")
+                else:
+                    servo.set_ms(pos)
+                    set_servo_pulse(servo_int, servo.ms)
+            elif servo_int <= 5 or servo_int == 0:
+                set_servo_pulse(servo_int, pos)
+            else:
+                sys.exit()
+            print(str(servo_int) + ": " + str(pos))
+
+    def angle(self):
+        commands = self.argv[2:]
+        for entry in commands:
+            servo_int = int(entry.split(',')[0])
+            pos = float(entry.split(',')[1])
+            if servo_int == 1:
+                servo = joy_it.servo1
+            elif servo_int == 2:
+                servo = joy_it.servo2
+            elif servo_int == 3:
+                servo = joy_it.servo3
+            if 0 < servo_int <= 3:
+                servo.set_angle(pos)
+                ms = servo.get_ms(True)
+                if not servo.geometry.is_inside(ms):
+                    print("Position to big or to small")
+                    sys.exit()
+                elif 0 < servo < 5:
+                    print("Servo is not on robotarm")
+                else:
+                    set_servo_pulse(servo_int, servo.ms)
+            elif servo_int == 4:
+                ms = get_ms_servo4(pos)
+                set_servo_pulse(servo_int, ms)
+            elif servo_int == 0:
+                ms = get_ms_servo0(pos)
+                set_servo_pulse(servo_int, ms)
+            else:
+                sys.exit()
+
+    def csv(self):
+        file = self.argv[2]
+
+        with open(file) as f:
+            csv_reader = csv.reader(f, delimiter=',')
+            for row in csv_reader:
+                if "delay" in row[0]:
+                    print("sleep " + row[0].split(" ")[1])
+                    time.sleep(int(row[0].split(" ")[1]))
+                else:
+                    for servo_int, pos in enumerate(row):
+                        if servo_int == 1:
+                            servo = joy_it.servo1
+                        elif servo_int == 2:
+                            servo = joy_it.servo2
+                        elif servo_int == 3:
+                            servo = joy_it.servo3
+                        if 0 < servo_int <= 3:
+                            servo.set_angle(pos)
+                            ms = servo.get_ms(True)
+                            if not servo.geometry.is_inside(ms):
+                                print("Position to big or to small")
+                                sys.exit()
+                            elif 0 < servo < 5:
+                                print("Servo is not on robotarm")
+                            else:
+                                set_servo_pulse(servo_int, servo.ms)
+                        elif servo_int == 4:
+                            ms = get_ms_servo4(pos)
+                            set_servo_pulse(servo_int, ms)
+                        elif servo_int == 0:
+                            ms = get_ms_servo0(pos)
+                            set_servo_pulse(servo_int, ms)
+                        else:
+                            sys.exit()
+
+
+def get_ms_servo4(deg):
+    change = 1 / 90
+    if servo4min > servo4max:
+        change *= -1
+    ms = (deg * change + servo4mid)
+    return ms
+
+
+def get_ms_servo0(deg):
+    change = 1 / 90
+    if servo0min > servo0max:
+        change *= -1
+    ms = (deg * change + servo0mid)
+    return ms
 
 
 def set_servo_pulse(channel, pulse):
@@ -72,91 +183,6 @@ def set_servo_pulse(channel, pulse):
     pwm.set_pwm(channel, 0, pulse)
 
 
-pwm.set_pwm_freq(50)
-
-
-def get_pos(anglesdeg):
-    """
-    Calculates the position of the claw relative to the center of the baseplate
-    Further details in documentation
-    :param anglesdeg: dict with the servo number and position in degree (deg)
-    :return: list: [x, y, z] in cm from the center of the baseplate of the robot
-    """
-    factor = math.pi / 180
-    angles = anglesdeg.copy()
-    for i in angles:
-        # print(angles[i])
-        angles[i] = angles[i] * factor
-        # print(angles[i])
-    x = 0
-    y = 0
-    z = 0
-    for angle in range(1, 3):
-        x += (math.sin(summe([angles[i] for i in range(1, angle + 1)])) * arms[angle])
-        z += (math.cos(summe([angles[i] for i in range(1, angle + 1)])) * arms[angle])
-    x += arms[0]
-    z += arms["height"]
-    angle = 3
-    x += (math.sin(summe([angles[i] for i in range(1, angle + 1)]) - 15 * factor) * arms["claw"])
-    z += (math.cos(summe([angles[i] for i in range(1, angle + 1)]) - 15 * factor) * arms["claw"])
-    y += (math.sin(angles[0]) * x)
-    x = (math.cos(angles[0]) * x)
-    coordinate = [x, y, z]
-    # print(str(x))
-    # print(str(y))
-    # print(str(z))
-    return coordinate
-
-
-def get_angles(ms):
-    """
-    Calculates the theoretical angles relative to a vertical position based on the ms values for each of the servos
-    :param ms: dict with the servo number and ms
-    :return: dict with the servo number and position in degree (deg)
-    """
-    angles = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0}
-    for servo in angles:
-        if ms[servo]:
-            minimum = "servo" + str(servo) + "min"
-            maximum = "servo" + str(servo) + "max"
-            middle = "servo" + str(servo) + "mid"
-            val_min = geometry[minimum]
-            val_max = geometry[maximum]
-            val_mid = geometry[middle]
-            change = 1 / 90
-            if val_min > val_max:
-                change *= -1
-            angles[servo] = (ms[servo] - val_mid) / change
-    return angles
-
-
-def get_ms(servo, angle):
-    """
-    Calculates the ms value based on the given angle (deg)
-    :param servo: servo number
-    :param angle: angle in degree based of the vertical position
-    :return: ms value for the servo
-    """
-    minimum = "servo" + str(servo) + "min"
-    maximum = "servo" + str(servo) + "max"
-    middle = "servo" + str(servo) + "mid"
-    val_min = geometry[minimum]
-    val_max = geometry[maximum]
-    val_mid = geometry[middle]
-    change = 1/90
-    if val_min > val_max:
-        change *= -1
-    ms = (angle * change + val_mid)
-    if change > 0:
-        if ms < val_min or ms > val_max:
-            return 0
-    else:
-        if ms > val_min or ms < val_max:
-            return 0
-    # ms = round(ms, 2)
-    return ms
-
-
 def read_argv():
     """
     Reades and processes the start arguments. The main core of the program
@@ -167,83 +193,17 @@ def read_argv():
     :return:
     """
     if sys.argv[1] == "-home":
-        for sv in range(0, 5):
-            set_servo_pulse(sv, 1.5)
+        argv_reader.home()
     elif sys.argv[1] == "-servo":
-        servo = int(sys.argv[2])
-        pos = float(sys.argv[4])
-        if pos < 1 or pos > 2.5:
-            print("Position to big or to small")
-            sys.exit()
-        elif servo < 0 or servo > 5:
-            print("Servo is not on robotarm")
-        print(str(servo) + ": " + str(pos))
-
-        set_servo_pulse(servo, pos)
+        argv_reader.servo()
     elif sys.argv[1] == "-list":  # -list 0,1.5 2,2 3,1.75
-        commands = sys.argv[2:]
-        varTime = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0}
-        for entry in commands:
-            servo = int(entry.split(',')[0])
-            pos = float(entry.split(',')[1])
-            if servo != 4 or servo != 5:
-                varTime[servo] = pos
-            print(str(servo) + ": " + str(pos))
-            set_servo_pulse(servo, pos)
-        calcangles = get_angles(varTime)
-        calcpos = get_pos(calcangles)
-        for sentry in calcangles:
-            print(str(sentry) + ": " + str(round(calcangles[sentry])))
-        for sentry in calcpos:
-            print(str(calcpos.index(sentry)) + ": " + str(sentry))
+        argv_reader.list()
     elif sys.argv[1] == "-angle":
-        commands = sys.argv[2:]
-        # calcangles = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0}
-        varTime = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0}
-        for entry in commands:
-            servo = int(entry.split(',')[0])
-            pos = float(entry.split(',')[1])
-            # calcangles[servo] = pos
-            pos = get_ms(servo, pos)
-            if servo != 4:
-                varTime[servo] = pos
-            print(str(servo) + ": " + str(pos))
-            set_servo_pulse(servo, pos)
-        calcangles = get_angles(varTime)
-        calcpos = get_pos(calcangles)
-        for entry in calcangles:
-            print(str(entry) + ": " + str(round(calcangles[entry])))
-        for entry in calcpos:
-            print(str(calcpos.index(entry)) + ": " + str(entry))
+        argv_reader.angle()
     elif sys.argv[1] == "-csv":
-        file = sys.argv[2]
-
-        with open(file) as f:
-            csv_reader = csv.reader(f, delimiter=',')
-            for row in csv_reader:
-                if "delay" in row[0]:
-                    print("sleep " + row[0].split(" ")[1])
-                    time.sleep(int(row[0].split(" ")[1]))
-                else:
-                    print(row)
-                    varTime = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0}
-                    f = 0
-                    for i in row:
-                        servo = f
-                        print(str(servo))
-                        pos = int(i)
-                        pos = get_ms(servo, pos)
-                        if servo != 4:
-                            varTime[servo] = pos
-                        print(str(servo) + ": " + str(pos))
-                        set_servo_pulse(servo, pos)
-                        f += 1
-                    calcangles = get_angles(varTime)
-                    calcpos = get_pos(calcangles)
-                    for entry in calcangles:
-                        print(str(entry) + ": " + str(round(calcangles[entry])))
-                    for entry in calcpos:
-                        print(str(calcpos.index(entry)) + ": " + str(entry))
+        argv_reader.csv()
 
 
+argv_reader = argvReader(sys.argv)
 read_argv()
+
-- 
2.39.5


From b910997d92bf1f85eaca187479786711bf362390 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:06:08 +0200
Subject: [PATCH 03/30] bug fix

---
 terminalControl.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index 11324eb..71a8108 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -48,6 +48,8 @@ class argvReader:
                 sys.exit()
             elif 0 < servo < 5:
                 print("Servo is not on robotarm")
+            elif servo_int == 0:
+                set_servo_pulse(0, pos)
             else:
                 servo.set_ms(pos)
                 set_servo_pulse(servo_int, servo.ms)
-- 
2.39.5


From 05ff7ee64d93a210ddd6addd05521f6be226afce Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:11:49 +0200
Subject: [PATCH 04/30] f

---
 terminalControl.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index 71a8108..790d2ae 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -36,6 +36,8 @@ class argvReader:
     def servo(self):
         servo_int = int(self.argv[2])
         pos = float(self.argv[4])
+        print(servo_int)
+        print(pos)
         if servo_int == 1:
             servo = joy_it.servo1
         elif servo_int == 2:
-- 
2.39.5


From 5830a6452cec2f05debfb90b19329c455997fd04 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:13:34 +0200
Subject: [PATCH 05/30] f

---
 terminalControl.py | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index 790d2ae..52cf6d6 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -36,22 +36,16 @@ class argvReader:
     def servo(self):
         servo_int = int(self.argv[2])
         pos = float(self.argv[4])
-        print(servo_int)
-        print(pos)
         if servo_int == 1:
             servo = joy_it.servo1
         elif servo_int == 2:
             servo = joy_it.servo2
         elif servo_int == 3:
             servo = joy_it.servo3
-        if servo_int <= 3:
+        if 0 < servo_int <= 3:
             if not servo.geometry.is_inside(pos):
                 print("Position to big or to small")
                 sys.exit()
-            elif 0 < servo < 5:
-                print("Servo is not on robotarm")
-            elif servo_int == 0:
-                set_servo_pulse(0, pos)
             else:
                 servo.set_ms(pos)
                 set_servo_pulse(servo_int, servo.ms)
-- 
2.39.5


From e5df8ce69232f7ff8c9cf6d1b699a21f1e232cd0 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:17:25 +0200
Subject: [PATCH 06/30] bug fix

---
 RobotLib.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/RobotLib.py b/RobotLib.py
index d74531e..c79dfc2 100644
--- a/RobotLib.py
+++ b/RobotLib.py
@@ -81,7 +81,7 @@ class Robot:
                 self.mid = mid
 
             def is_inside(self, ms):
-                return self.min < ms < self.max
+                return self.min < ms < self.max or self.max < ms < self.min
 
     class Arm:
         def __init__(self, attatched_to, length, height=0.0):
-- 
2.39.5


From c482dfe2bf961bb3316365354b323e1a0dfd09c4 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:19:24 +0200
Subject: [PATCH 07/30] bug fix

---
 terminalControl.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index 52cf6d6..45d012e 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -70,8 +70,6 @@ class argvReader:
                 if not servo.geometry.is_inside(pos):
                     print("Position to big or to small")
                     sys.exit()
-                elif 0 < servo < 5:
-                    print("Servo is not on robotarm")
                 else:
                     servo.set_ms(pos)
                     set_servo_pulse(servo_int, servo.ms)
-- 
2.39.5


From 0be90b834cdf3df7cba914e79962078f271175e5 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:21:17 +0200
Subject: [PATCH 08/30] bug fix

---
 terminalControl.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index 45d012e..8e41f9d 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -96,8 +96,6 @@ class argvReader:
                 if not servo.geometry.is_inside(ms):
                     print("Position to big or to small")
                     sys.exit()
-                elif 0 < servo < 5:
-                    print("Servo is not on robotarm")
                 else:
                     set_servo_pulse(servo_int, servo.ms)
             elif servo_int == 4:
-- 
2.39.5


From 381af7dc9a17b7913b76ad6ca3ef3d880b6edca2 Mon Sep 17 00:00:00 2001
From: lade043 <48773751+lade043@users.noreply.github.com>
Date: Sat, 18 May 2019 19:23:09 +0200
Subject: [PATCH 09/30] sequence.csv for testing of -csv

---
 sequence.csv | 7 +++++++
 1 file changed, 7 insertions(+)
 create mode 100644 sequence.csv

diff --git a/sequence.csv b/sequence.csv
new file mode 100644
index 0000000..35523e4
--- /dev/null
+++ b/sequence.csv
@@ -0,0 +1,7 @@
+0,0,0,0,0
+delay 10,,,,
+10,10,10,10,10
+delay 5,,,,
+20,0,90,90,20
+delay 3,,,,
+0,0,0,0,0
\ No newline at end of file
-- 
2.39.5


From 07f5be3bc13d60b9c3397cfab2fcc63a8c9ea9d6 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:34:45 +0200
Subject: [PATCH 10/30] bug fix

---
 csv_analyse.py     | 8 +++-----
 terminalControl.py | 2 --
 2 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/csv_analyse.py b/csv_analyse.py
index 69512a3..986c441 100644
--- a/csv_analyse.py
+++ b/csv_analyse.py
@@ -1,7 +1,7 @@
 import time
 import csv
 
-file = "/home/lars/Schreibtisch/sequence.csv"
+file = "sequence.csv"
 with open(file) as f:
     csv_reader = csv.reader(f, delimiter=',')
     for row in csv_reader:
@@ -9,8 +9,6 @@ with open(file) as f:
             print("sleep " + row[0].split(" ")[1])
             time.sleep(int(row[0].split(" ")[1]))
         else:
-            f = 0
-            for i in row:
+            for count, i in enumerate(row):
                 print(i)
-                print(str(f))
-                f += 1
+                print(count)
diff --git a/terminalControl.py b/terminalControl.py
index 8e41f9d..1607c2c 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -130,8 +130,6 @@ class argvReader:
                             if not servo.geometry.is_inside(ms):
                                 print("Position to big or to small")
                                 sys.exit()
-                            elif 0 < servo < 5:
-                                print("Servo is not on robotarm")
                             else:
                                 set_servo_pulse(servo_int, servo.ms)
                         elif servo_int == 4:
-- 
2.39.5


From 9832e47172c3c2e100d3814d75b4c7c0d8d5d12a Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:35:56 +0200
Subject: [PATCH 11/30] f

---
 terminalControl.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/terminalControl.py b/terminalControl.py
index 1607c2c..a5307b8 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -151,6 +151,7 @@ def get_ms_servo4(deg):
 
 
 def get_ms_servo0(deg):
+    print(deg)
     change = 1 / 90
     if servo0min > servo0max:
         change *= -1
-- 
2.39.5


From 6cfd6547e67a4e294b2ce3695b94713288150f63 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:38:54 +0200
Subject: [PATCH 12/30] bug fix

---
 terminalControl.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index a5307b8..d5b514e 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -32,6 +32,10 @@ class argvReader:
         for i, servo in enumerate([joy_it.servo1, joy_it.servo2, joy_it.servo3]):
             servo.set_angle(0)
             set_servo_pulse(i+1, servo.get_ms(True))
+        ms = get_ms_servo0(0)
+        set_servo_pulse(0, ms)
+        ms = get_ms_servo4(0)
+        set_servo_pulse(4, ms)
 
     def servo(self):
         servo_int = int(self.argv[2])
-- 
2.39.5


From 32ee37e54b016a3a8c6bab4172b1c3a5e6ec73ac Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:39:52 +0200
Subject: [PATCH 13/30] bug fix

---
 terminalControl.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/terminalControl.py b/terminalControl.py
index d5b514e..3beae22 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -122,6 +122,7 @@ class argvReader:
                     time.sleep(int(row[0].split(" ")[1]))
                 else:
                     for servo_int, pos in enumerate(row):
+                        pos = int(pos)
                         if servo_int == 1:
                             servo = joy_it.servo1
                         elif servo_int == 2:
-- 
2.39.5


From 404ca9b3ad16d714b03521fbbec29975561ab12e Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:41:30 +0200
Subject: [PATCH 14/30] extreme positions weren't possible

---
 RobotLib.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/RobotLib.py b/RobotLib.py
index c79dfc2..a345440 100644
--- a/RobotLib.py
+++ b/RobotLib.py
@@ -81,7 +81,7 @@ class Robot:
                 self.mid = mid
 
             def is_inside(self, ms):
-                return self.min < ms < self.max or self.max < ms < self.min
+                return self.min <= ms <= self.max or self.max <= ms <= self.min
 
     class Arm:
         def __init__(self, attatched_to, length, height=0.0):
-- 
2.39.5


From 6e33131036e2bc26a8d13012394658551e88075c Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 19:43:12 +0200
Subject: [PATCH 15/30] edited sequence.csv

---
 sequence.csv | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sequence.csv b/sequence.csv
index 35523e4..ba9d7d5 100644
--- a/sequence.csv
+++ b/sequence.csv
@@ -2,6 +2,6 @@
 delay 10,,,,
 10,10,10,10,10
 delay 5,,,,
-20,0,90,90,20
+20,0,70,70,20
 delay 3,,,,
 0,0,0,0,0
\ No newline at end of file
-- 
2.39.5


From 3f0d6a658cb6e3a7661ccfc12ba3b1c3b01779f2 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 21:02:57 +0200
Subject: [PATCH 16/30] added function 'output'

---
 terminalControl.py | 50 ++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 46 insertions(+), 4 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index 3beae22..ee8f2b7 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -2,6 +2,7 @@ from __future__ import division
 import Adafruit_PCA9685
 import sys
 import time
+import numpy as npy
 import csv
 from RobotLib import *
 
@@ -22,13 +23,21 @@ servo0mid = 1.85
 servo4min = 0.5
 servo4max = 2.5
 servo4mid = 1.5
-
+servo0actual = 0
 
 class argvReader:
     def __init__(self, argv):
         self.argv = argv
 
+    def output(self, s0_angle):
+        joy_it.calculate()
+        x = joy_it.x
+        z = joy_it.y
+        x, y = converter_2d_to3d(x, s0_angle)
+        print("{},{},{}".format(x, y, z))
+
     def home(self):
+        global servo0actual
         for i, servo in enumerate([joy_it.servo1, joy_it.servo2, joy_it.servo3]):
             servo.set_angle(0)
             set_servo_pulse(i+1, servo.get_ms(True))
@@ -36,8 +45,11 @@ class argvReader:
         set_servo_pulse(0, ms)
         ms = get_ms_servo4(0)
         set_servo_pulse(4, ms)
+        servo0actual = 0
+        self.output(0)
 
     def servo(self):
+        global servo0actual
         servo_int = int(self.argv[2])
         pos = float(self.argv[4])
         if servo_int == 1:
@@ -55,11 +67,14 @@ class argvReader:
                 set_servo_pulse(servo_int, servo.ms)
         elif servo_int <= 5:
             set_servo_pulse(servo_int, pos)
+            if servo_int == 0:
+                servo0actual = get_angle_servo0(pos)
         else:
             sys.exit()
-        print(str(servo_int) + ": " + str(pos))
+        self.output(servo0actual)
 
     def list(self):
+        global servo0actual
         commands = self.argv[2:]
         for entry in commands:
             servo_int = int(entry.split(',')[0])
@@ -79,11 +94,14 @@ class argvReader:
                     set_servo_pulse(servo_int, servo.ms)
             elif servo_int <= 5 or servo_int == 0:
                 set_servo_pulse(servo_int, pos)
+                if servo_int == 0:
+                    servo0actual = get_angle_servo0(pos)
             else:
                 sys.exit()
-            print(str(servo_int) + ": " + str(pos))
+            self.output(servo0actual)
 
     def angle(self):
+        global servo0actual
         commands = self.argv[2:]
         for entry in commands:
             servo_int = int(entry.split(',')[0])
@@ -108,8 +126,10 @@ class argvReader:
             elif servo_int == 0:
                 ms = get_ms_servo0(pos)
                 set_servo_pulse(servo_int, ms)
+                servo0actual = pos
             else:
                 sys.exit()
+        self.output(servo0actual)
 
     def csv(self):
         file = self.argv[2]
@@ -156,7 +176,6 @@ def get_ms_servo4(deg):
 
 
 def get_ms_servo0(deg):
-    print(deg)
     change = 1 / 90
     if servo0min > servo0max:
         change *= -1
@@ -164,6 +183,22 @@ def get_ms_servo0(deg):
     return ms
 
 
+def get_angle_servo4(ms):
+    change = 1 / 90
+    if servo4min > servo4max:
+        change *= -1
+    angle = (ms - servo4mid) / change
+    return angle
+
+
+def get_angle_servo0(ms):
+    change = 1 / 90
+    if servo0min > servo0max:
+        change *= -1
+    angle = (ms - servo0mid) / change
+    return angle
+
+
 def set_servo_pulse(channel, pulse):
     """
     Sending the position to the servos
@@ -181,6 +216,13 @@ def set_servo_pulse(channel, pulse):
     pwm.set_pwm(channel, 0, pulse)
 
 
+def converter_2d_to3d(hypotenuse, s0_angle):
+    rad = s0_angle * math.pi / 180
+    x = npy.cos(rad) * hypotenuse
+    y = npy.sin(rad) * hypotenuse
+    return x, y
+
+
 def read_argv():
     """
     Reades and processes the start arguments. The main core of the program
-- 
2.39.5


From 495e3c9cd2a5f0c404ada5db31319ea1e0c9082e Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 21:04:58 +0200
Subject: [PATCH 17/30] f

---
 terminalControl.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/terminalControl.py b/terminalControl.py
index ee8f2b7..0b72bd5 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -98,7 +98,7 @@ class argvReader:
                     servo0actual = get_angle_servo0(pos)
             else:
                 sys.exit()
-            self.output(servo0actual)
+        self.output(servo0actual)
 
     def angle(self):
         global servo0actual
-- 
2.39.5


From d348b8f0015ef9e890b2680e1fe62d27d303702d Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 21:10:01 +0200
Subject: [PATCH 18/30] f

---
 terminalControl.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index 0b72bd5..45ac5d9 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -35,6 +35,8 @@ class argvReader:
         z = joy_it.y
         x, y = converter_2d_to3d(x, s0_angle)
         print("{},{},{}".format(x, y, z))
+        print("servo0:{}, servo1:{}, servo2:{}, servo3:{}".format(servo0actual, joy_it.servo1.deg, joy_it.servo2.deg,
+                                                                  joy_it.servo3.deg))
 
     def home(self):
         global servo0actual
-- 
2.39.5


From e22b21c11a87d712dd1a92f278b740b44dbf62bb Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 21:13:06 +0200
Subject: [PATCH 19/30] f

---
 terminalControl.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/terminalControl.py b/terminalControl.py
index 45ac5d9..69fefb4 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -31,6 +31,7 @@ class argvReader:
 
     def output(self, s0_angle):
         joy_it.calculate()
+        print(joy_it.x)
         x = joy_it.x
         z = joy_it.y
         x, y = converter_2d_to3d(x, s0_angle)
-- 
2.39.5


From 86a9dd4311d45fec1edccfb6f0d3a2a08ccb46c0 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 21:43:05 +0200
Subject: [PATCH 20/30] bug fix

---
 RobotLib.py          | 2 ++
 databaseCreatorv2.py | 2 +-
 terminalControl.py   | 3 ++-
 3 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/RobotLib.py b/RobotLib.py
index a345440..ee379e5 100644
--- a/RobotLib.py
+++ b/RobotLib.py
@@ -33,6 +33,7 @@ class Robot:
 
         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
@@ -40,6 +41,7 @@ class Robot:
                     self.delta *= -1
             else:
                 self.delta = self.deg
+                self.total_angle_deg = self.deg
             self.total_angle_rad = self._get_rad(self.total_angle_deg)
 
         def set_angle(self, angle):
diff --git a/databaseCreatorv2.py b/databaseCreatorv2.py
index 1993fb4..5caad1d 100644
--- a/databaseCreatorv2.py
+++ b/databaseCreatorv2.py
@@ -34,7 +34,7 @@ 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),
+test.init_depending(Robot.Arm(test.servo1, 10.26, 0.84), Robot.Arm(test.servo2, 9.85), Robot.Arm(test.servo3, 12, -3),
                     None, [test.servo1], [test.servo1, test.servo2])
 
 looper = Looper(test)
diff --git a/terminalControl.py b/terminalControl.py
index 69fefb4..6104600 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -14,7 +14,7 @@ joy_it = Robot(Robot.Servo(1, Robot.Servo.Geometry(0.55, 2.3, 1.4)),
                Robot.Servo(3, Robot.Servo.Geometry(0.7, 2.25, 2.25)),
                Robot.CoordinateSystem([-100, 100], [-100, 100]))
 joy_it.init_depending(Robot.Arm(joy_it.servo1, 10.26, 0.84), Robot.Arm(joy_it.servo2, 9.85),
-                      Robot.Arm(joy_it.servo3, 12, 9), None, [joy_it.servo1], [joy_it.servo1, joy_it.servo2])
+                      Robot.Arm(joy_it.servo3, 12, -3), None, [joy_it.servo1], [joy_it.servo1, joy_it.servo2])
 
 # the values for all the servos (are different for each robot)
 servo0min = 0.5
@@ -25,6 +25,7 @@ servo4max = 2.5
 servo4mid = 1.5
 servo0actual = 0
 
+
 class argvReader:
     def __init__(self, argv):
         self.argv = argv
-- 
2.39.5


From 6ea7ca16e34e623b1465851d5f1b362fc0cc134a Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sat, 18 May 2019 22:31:25 +0200
Subject: [PATCH 21/30] cleaned up

---
 terminalControl.py | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index 6104600..20872b3 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -1,9 +1,12 @@
 from __future__ import division
-import Adafruit_PCA9685
+
+import csv
 import sys
 import time
+
+import Adafruit_PCA9685
 import numpy as npy
-import csv
+
 from RobotLib import *
 
 pwm = Adafruit_PCA9685.PCA9685(address=0x41)
@@ -32,11 +35,10 @@ class argvReader:
 
     def output(self, s0_angle):
         joy_it.calculate()
-        print(joy_it.x)
         x = joy_it.x
         z = joy_it.y
         x, y = converter_2d_to3d(x, s0_angle)
-        print("{},{},{}".format(x, y, z))
+        print("x:{},y:{},z:{}".format(x, y, z))
         print("servo0:{}, servo1:{}, servo2:{}, servo3:{}".format(servo0actual, joy_it.servo1.deg, joy_it.servo2.deg,
                                                                   joy_it.servo3.deg))
 
-- 
2.39.5


From 42cfd47923d47a776fd9c3063692b721fc723e7c Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 07:47:07 +0200
Subject: [PATCH 22/30] added loop

---
 terminalControl.py | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index 20872b3..fde08db 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -33,6 +33,9 @@ class argvReader:
     def __init__(self, argv):
         self.argv = argv
 
+    def set_argument(self, argv):
+        self.argv = argv
+
     def output(self, s0_angle):
         joy_it.calculate()
         x = joy_it.x
@@ -248,6 +251,20 @@ def read_argv():
         argv_reader.angle()
     elif sys.argv[1] == "-csv":
         argv_reader.csv()
+    elif sys.argv[1] == "-loop":
+        while True:
+            _input = input(">").split(" ")
+            argv_reader.set_argument(_input)
+            if _input[1] == "-home":
+                argv_reader.home()
+            elif _input[1] == "-servo":
+                argv_reader.servo()
+            elif _input[1] == "-list":  # -list 0,1.5 2,2 3,1.75
+                argv_reader.list()
+            elif _input[1] == "-angle":
+                argv_reader.angle()
+            elif _input[1] == "-csv":
+                argv_reader.csv()
 
 
 argv_reader = argvReader(sys.argv)
-- 
2.39.5


From 26a47e79d3cfd0871d3a02e51dbe694baf2006ed Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 07:54:27 +0200
Subject: [PATCH 23/30] bug fix

---
 terminalControl.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/terminalControl.py b/terminalControl.py
index fde08db..1f5ae18 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -254,6 +254,7 @@ def read_argv():
     elif sys.argv[1] == "-loop":
         while True:
             _input = input(">").split(" ")
+            _input.insert(0, "loop")
             argv_reader.set_argument(_input)
             if _input[1] == "-home":
                 argv_reader.home()
-- 
2.39.5


From eb164bf058a5299dd7ab203d1102c985d219aa69 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 08:53:10 +0200
Subject: [PATCH 24/30] serialize and deserialize added

---
 terminalControl.py | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/terminalControl.py b/terminalControl.py
index 1f5ae18..a4658a4 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -12,6 +12,8 @@ from RobotLib import *
 pwm = Adafruit_PCA9685.PCA9685(address=0x41)
 pwm.set_pwm_freq(50)
 
+file = ".robotpos.file"
+
 joy_it = 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)),
@@ -175,6 +177,21 @@ class argvReader:
                         else:
                             sys.exit()
 
+    def serialize(self, filepath):
+        with open(filepath, 'w') as file:
+            string = "{},{},{},{}".format(servo0actual, joy_it.servo1.deg, joy_it.servo2.deg, joy_it.servo3.deg)
+            file.write(string)
+
+    def deserialize(self, filepath):
+        global servo0actual
+        with open(filepath, 'r') as file:
+            string = file.readline()
+            string.split(",")
+            servo0actual = int(string[0])
+            joy_it.servo1.set_angle(int(string[1]))
+            joy_it.servo2.set_angle(int(string[2]))
+            joy_it.servo3.set_angle(int(string[3]))
+
 
 def get_ms_servo4(deg):
     change = 1 / 90
@@ -266,8 +283,11 @@ def read_argv():
                 argv_reader.angle()
             elif _input[1] == "-csv":
                 argv_reader.csv()
+            elif _input[1] == "-exit":
+                break
 
 
 argv_reader = argvReader(sys.argv)
+argv_reader.deserialize(file)
 read_argv()
-
+argv_reader.serialize(file)
-- 
2.39.5


From 63d0263b19274e4f1bee9a55f090e7f7e907c67b Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 08:55:58 +0200
Subject: [PATCH 25/30] bug fix

---
 terminalControl.py | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index a4658a4..03c24ed 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -184,13 +184,19 @@ class argvReader:
 
     def deserialize(self, filepath):
         global servo0actual
-        with open(filepath, 'r') as file:
-            string = file.readline()
-            string.split(",")
-            servo0actual = int(string[0])
-            joy_it.servo1.set_angle(int(string[1]))
-            joy_it.servo2.set_angle(int(string[2]))
-            joy_it.servo3.set_angle(int(string[3]))
+        try:
+            with open(filepath, 'r') as file:
+                string = file.readline()
+                string.split(",")
+                servo0actual = int(string[0])
+                joy_it.servo1.set_angle(int(string[1]))
+                joy_it.servo2.set_angle(int(string[2]))
+                joy_it.servo3.set_angle(int(string[3]))
+        except FileNotFoundError:
+            servo0actual = 0
+            joy_it.servo1.set_angle(0)
+            joy_it.servo2.set_angle(0)
+            joy_it.servo3.set_angle(0)
 
 
 def get_ms_servo4(deg):
-- 
2.39.5


From 26b785456b61ca352605cca4fc354c5ccd8d8a26 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 09:02:52 +0200
Subject: [PATCH 26/30] bug fix

---
 terminalControl.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/terminalControl.py b/terminalControl.py
index 03c24ed..6927ab7 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -187,7 +187,7 @@ class argvReader:
         try:
             with open(filepath, 'r') as file:
                 string = file.readline()
-                string.split(",")
+                string = string.split(",")
                 servo0actual = int(string[0])
                 joy_it.servo1.set_angle(int(string[1]))
                 joy_it.servo2.set_angle(int(string[2]))
-- 
2.39.5


From 84cccc5636f56d76547d2ce6bc57aeb32c3dd6a6 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 09:03:55 +0200
Subject: [PATCH 27/30] bug fix

---
 terminalControl.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index 6927ab7..d70440c 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -188,10 +188,10 @@ class argvReader:
             with open(filepath, 'r') as file:
                 string = file.readline()
                 string = string.split(",")
-                servo0actual = int(string[0])
-                joy_it.servo1.set_angle(int(string[1]))
-                joy_it.servo2.set_angle(int(string[2]))
-                joy_it.servo3.set_angle(int(string[3]))
+                servo0actual = float(string[0])
+                joy_it.servo1.set_angle(float(string[1]))
+                joy_it.servo2.set_angle(float(string[2]))
+                joy_it.servo3.set_angle(float(string[3]))
         except FileNotFoundError:
             servo0actual = 0
             joy_it.servo1.set_angle(0)
-- 
2.39.5


From f53f37cc94406e4b0cdb034a00f8397d0e5e7066 Mon Sep 17 00:00:00 2001
From: lade043 <48773751+lade043@users.noreply.github.com>
Date: Sun, 19 May 2019 09:10:49 +0200
Subject: [PATCH 28/30] Update .gitignore

terminalControl creates file to save last position, which will now not be uploaded
---
 .gitignore | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/.gitignore b/.gitignore
index 36948be..abef767 100644
--- a/.gitignore
+++ b/.gitignore
@@ -105,3 +105,6 @@ venv.bak/
 
 # tests for fast debugging
 tests/
+
+# last position of robot thru terminalControl
+.robotpos.file
-- 
2.39.5


From f049c2133618be815e771e8c6f57b4b08540e77c Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 09:50:35 +0200
Subject: [PATCH 29/30] Comment

---
 terminalControl.py | 112 +++++++++++++++++++++++++++++++++++++--------
 1 file changed, 94 insertions(+), 18 deletions(-)

diff --git a/terminalControl.py b/terminalControl.py
index d70440c..14565c5 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -9,11 +9,14 @@ import numpy as npy
 
 from RobotLib import *
 
+# setting up the Servo Controller
 pwm = Adafruit_PCA9685.PCA9685(address=0x41)
 pwm.set_pwm_freq(50)
 
+# file with latest given servo pos
 file = ".robotpos.file"
 
+# defining robot model of joy_it
 joy_it = 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)),
@@ -33,12 +36,26 @@ servo0actual = 0
 
 class argvReader:
     def __init__(self, argv):
+        """
+        Takes the values from a list of arguments and controls the servos accordingly
+        :param argv: The arguments, that should be analyzed
+        """
         self.argv = argv
 
     def set_argument(self, argv):
+        """
+        sets the argument if it change since the init
+        :param argv: The arguments, that should be analyzed
+        :return:
+        """
         self.argv = argv
 
     def output(self, s0_angle):
+        """
+        Calculates the position of the claw and outputs it with the angles of the servos
+        :param s0_angle: Angle of servo 0 for conversion from 2d to 3d
+        :return: Console output
+        """
         joy_it.calculate()
         x = joy_it.x
         z = joy_it.y
@@ -48,6 +65,10 @@ class argvReader:
                                                                   joy_it.servo3.deg))
 
     def home(self):
+        """
+        Controls all servos to correspond the home position
+        :return: sets all servos to 0deg
+        """
         global servo0actual
         for i, servo in enumerate([joy_it.servo1, joy_it.servo2, joy_it.servo3]):
             servo.set_angle(0)
@@ -60,6 +81,10 @@ class argvReader:
         self.output(0)
 
     def servo(self):
+        """
+        Controls one servo to according to the given ms value
+        :return: sets one servo to the angle that correspond the ms value
+        """
         global servo0actual
         servo_int = int(self.argv[2])
         pos = float(self.argv[4])
@@ -85,6 +110,10 @@ class argvReader:
         self.output(servo0actual)
 
     def list(self):
+        """
+        Controls multiple servos according to their given ms values
+        :return: sets multiple servos to their ms values
+        """
         global servo0actual
         commands = self.argv[2:]
         for entry in commands:
@@ -112,6 +141,10 @@ class argvReader:
         self.output(servo0actual)
 
     def angle(self):
+        """
+        Controls multiple servos according to their given angle
+        :return: sets multiple servos to the angle that is given
+        """
         global servo0actual
         commands = self.argv[2:]
         for entry in commands:
@@ -143,9 +176,13 @@ class argvReader:
         self.output(servo0actual)
 
     def csv(self):
-        file = self.argv[2]
+        """
+        Follows an procedure given by an csv file
+        :return:sets all the servos to the angles from the csv file
+        """
+        csv_file = self.argv[2]
 
-        with open(file) as f:
+        with open(csv_file) as f:
             csv_reader = csv.reader(f, delimiter=',')
             for row in csv_reader:
                 if "delay" in row[0]:
@@ -178,11 +215,21 @@ class argvReader:
                             sys.exit()
 
     def serialize(self, filepath):
+        """
+        Prints the current position to an file
+        :param filepath: filepath of the file to which the position should be written to
+        :return: The angles of servo0 to servo3 seperated by a ','
+        """
         with open(filepath, 'w') as file:
             string = "{},{},{},{}".format(servo0actual, joy_it.servo1.deg, joy_it.servo2.deg, joy_it.servo3.deg)
             file.write(string)
 
     def deserialize(self, filepath):
+        """
+        Sets all position variables to the values from the file
+        :param filepath: filepath of the file from which should be read
+        :return: The variables are set accordingly
+        """
         global servo0actual
         try:
             with open(filepath, 'r') as file:
@@ -199,15 +246,12 @@ class argvReader:
             joy_it.servo3.set_angle(0)
 
 
-def get_ms_servo4(deg):
-    change = 1 / 90
-    if servo4min > servo4max:
-        change *= -1
-    ms = (deg * change + servo4mid)
-    return ms
-
-
 def get_ms_servo0(deg):
+    """
+    Calculates the ms value for servo 0 at a given angle
+    :param deg: Angle in deg
+    :return: ms value which corresponds to the angle
+    """
     change = 1 / 90
     if servo0min > servo0max:
         change *= -1
@@ -215,15 +259,12 @@ def get_ms_servo0(deg):
     return ms
 
 
-def get_angle_servo4(ms):
-    change = 1 / 90
-    if servo4min > servo4max:
-        change *= -1
-    angle = (ms - servo4mid) / change
-    return angle
-
-
 def get_angle_servo0(ms):
+    """
+    Calculates the angle for servo0 at a given ms-value
+    :param ms: ms value
+    :return: angle which corresponds to the ms value
+    """
     change = 1 / 90
     if servo0min > servo0max:
         change *= -1
@@ -231,6 +272,32 @@ def get_angle_servo0(ms):
     return angle
 
 
+def get_ms_servo4(deg):
+    """
+    see 'get_ms_servo0'
+    :param deg:
+    :return:
+    """
+    change = 1 / 90
+    if servo4min > servo4max:
+        change *= -1
+    ms = (deg * change + servo4mid)
+    return ms
+
+
+def get_angle_servo4(ms):
+    """
+    see 'get_angle_servo0'
+    :param ms:
+    :return:
+    """
+    change = 1 / 90
+    if servo4min > servo4max:
+        change *= -1
+    angle = (ms - servo4mid) / change
+    return angle
+
+
 def set_servo_pulse(channel, pulse):
     """
     Sending the position to the servos
@@ -249,6 +316,12 @@ def set_servo_pulse(channel, pulse):
 
 
 def converter_2d_to3d(hypotenuse, s0_angle):
+    """
+    Converts the 2d model to an 3d model by using the angle of s0
+    :param hypotenuse: The distance from the base to the claw in x direction
+    :param s0_angle: Angle in deg of servo0
+    :return: The calculated x and y coordinates
+    """
     rad = s0_angle * math.pi / 180
     x = npy.cos(rad) * hypotenuse
     y = npy.sin(rad) * hypotenuse
@@ -262,6 +335,9 @@ def read_argv():
                                 python3 terminalControl.py -list servo,pos(ms) servo,pos(ms)
                                 python3 terminalControl.py -angle servo,angle servo,angle
                                 python3 terminalControl.py -csv file
+                                python3 terminalControl.py -loop
+                                > -servo x -pos y(ms)
+                                > ...
     :return:
     """
     if sys.argv[1] == "-home":
-- 
2.39.5


From 72cd9bbebf6f693aff8f6978d901630bcc13f990 Mon Sep 17 00:00:00 2001
From: "L. Bogner" <lars.bogner@outlook.de>
Date: Sun, 19 May 2019 09:57:03 +0200
Subject: [PATCH 30/30] csv output added

---
 terminalControl.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/terminalControl.py b/terminalControl.py
index 14565c5..62b5f1f 100644
--- a/terminalControl.py
+++ b/terminalControl.py
@@ -181,6 +181,7 @@ class argvReader:
         :return:sets all the servos to the angles from the csv file
         """
         csv_file = self.argv[2]
+        global servo0actual
 
         with open(csv_file) as f:
             csv_reader = csv.reader(f, delimiter=',')
@@ -211,8 +212,10 @@ class argvReader:
                         elif servo_int == 0:
                             ms = get_ms_servo0(pos)
                             set_servo_pulse(servo_int, ms)
+                            servo0actual = pos
                         else:
                             sys.exit()
+                    argv_reader.output(servo0actual)
 
     def serialize(self, filepath):
         """
-- 
2.39.5