First Commit, databaseCreator.py not working

2019-03-20 22:32:31 +01:00
commit 3c3709c57e
2 changed files with 350 additions and 0 deletions
@@ -0,0 +1,143 @@
+import math
+import csv
+
+
+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
+    }
+arms = {
+    0: 0.84,
+    1: 10.26,
+    2: 9.85,
+    "claw": 12,
+    "height": 9
+}
+
+
+def summe(i):
+    r = 0
+    for element in i:
+        r += element
+    return r
+
+
+def get_pos(anglesdeg):
+    factor = math.pi / 180
+    angles = anglesdeg.copy()
+    for i in angles:
+        # print(angles[i])
+        angles[i] = angles[i] * factor
+        # print(angles[i])
+    x = 0
+    z = 0
+    for angle in range(0, 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"])
+    coordinateback = [x, z]
+    # print(str(x))
+    # print(str(y))
+    # print(str(z))
+    return coordinateback
+
+
+def get_angles(ms):
+    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):
+    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 get_max_min(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
+    minimum = (val_min - val_mid) / change
+    maximum = (val_max - val_mid) / change
+
+    return [minimum, maximum]
+
+
+def get_efficency(anglesdeg):
+    eff = 0
+    i = 1
+    for servo in reversed(anglesdeg):
+        eff += i * servo
+        i += 1
+    return eff
+
+
+pos = []
+for servoR in range(5):
+    pos[servoR] = get_max_min(servoR)
+
+"""
+pos = [x, y, z, eff]
+if eff < replace
+"""
+db = {}
+for steps in range(1, 0, -0.2):
+        for servo1 in range(pos[1][0], pos[1][1], steps):
+            for servo2 in range(pos[2][0], pos[2][1], steps):
+                for servo3 in range(pos[3][0], pos[3][1], steps):
+                    for servo4 in range(pos[4][0], pos[4][1], steps):
+                        coordinate = []
+                        coordinate.append(get_pos([servo1, servo2, servo3, servo4]))
+                        #coordinate.append(get_efficency([servo0, servo1, servo2, servo3, servo4]))
+                        if coordinate in db:
+                            if db[coordinate][]