From 5453ca185136314ca0f1a291af36dd9abccc7a61 Mon Sep 17 00:00:00 2001
From: DeDiamondPro <67508414+DeDiamondPro@users.noreply.github.com>
Date: Mon, 4 Mar 2024 16:45:27 +0100
Subject: [PATCH] stuff

---
 .../inverse_kinematics.py                     | 324 ++++++++++++++++--
 1 file changed, 303 insertions(+), 21 deletions(-)

diff --git a/code/inverse-kinematics-test/inverse_kinematics.py b/code/inverse-kinematics-test/inverse_kinematics.py
index fda70b8..1c9c652 100644
--- a/code/inverse-kinematics-test/inverse_kinematics.py
+++ b/code/inverse-kinematics-test/inverse_kinematics.py
@@ -1,5 +1,24 @@
 from math import *
 
+from adafruit_servokit import ServoKit
+from pyPS4Controller.controller import Controller
+from time import sleep
+
+pca = ServoKit(channels=16)
+
+servo_offsets = [5, 8, -7, 0, -6, 3, 3, 4, 5, -3, 2, 3]
+
+length = 27
+width = 17
+leg_length1 = 10.0
+leg_length2 = 10.0
+pitch_offset = 2
+yaw_offset = 0
+
+
+def set_servo(servo, angle):
+    pca.servo[servo].angle = angle + servo_offsets[servo]
+
 
 # https://en.wikipedia.org/wiki/Rotation_matrix
 def calculate_rotation(x1, y1, angle):
@@ -8,31 +27,294 @@ def calculate_rotation(x1, y1, angle):
     return x2 - x1, y2 - y1
 
 
-pitch = 10.0
-yaw = 10.0
-l = 20.0
-b = 10.0
+# pitch = 10.0
+# yaw = 10.0
+#
+# height_diff_pitch = length * sin(radians(pitch)) / 2
+# height_diff_yaw = width * sin(radians(yaw)) / 2
+# leg_movement_x = leg_length1 * (1 - cos(radians(pitch))) / 2
+# leg_movement_z = width * (1 - cos(radians(yaw))) / 2
+#
+# print(calculate_rotation(1.0, 1.0, -30.0))
+#
+# print("Height difference pitch: {}, height difference yaw: {}".format(height_diff_pitch, height_diff_yaw))
+# print("Leg movement x: {}, leg movement y: {}".format(leg_movement_x, leg_movement_z))
+
+
+def calc_angles(x, y, z):
+    A = acos(((y * y + x * x) * (y * y + z * z) + y * y * leg_length1 * leg_length1 - y * y * leg_length2 * leg_length2)
+             / (2 * y * leg_length1 * sqrt((y * y + x * x) * (y * y + z * z)))) + atan(x / y)
+    B = acos(
+        (y * y * leg_length1 * leg_length1 + y * y * leg_length2 * leg_length2 - (y * y + x * x) * (y * y + z * z)) / (
+                2 * y * y * leg_length1 * leg_length2))
+    C = atan(z / y)
+    # print("A: {}, B: {}, C: {}".format(degrees(A), degrees(B), degrees(C)))
+    return A, B, C
+
+
+def calc_leg_offsets(x, z, pitch, yaw, rotation):
+    pitch += pitch_offset
+    yaw += yaw_offset
+    height_diff_pitch = x * sin(radians(pitch))
+    height_diff_yaw = z * sin(radians(yaw))
+    movement_x_pitch = x * (1 - cos(radians(pitch)))
+    movement_z_yaw = z * (1 - cos(radians(yaw)))
+    movement_x_rotation = (x * cos(radians(rotation)) - z * sin(radians(rotation))) - x
+    movement_z_rotation = (x * sin(radians(rotation)) + z * cos(radians(rotation))) - z
+    return (
+        movement_x_pitch + movement_x_rotation,
+        height_diff_pitch + height_diff_yaw,
+        movement_z_yaw + movement_z_rotation
+    )
+
+
+def set_legs(desired_x, desired_y, desired_z, desired_pitch, desired_yaw, desired_rotation):
+    for leg in range(3, -1, -1):
+        try:
+            x = (length / 2) if leg % 2 else -(length / 2)
+            z = (width / 2) if leg < 2 else -(width / 2)
+            offsets = calc_leg_offsets(x, z, desired_pitch, desired_yaw, desired_rotation)
+            print("Leg {}: X-offset: {}, Y-offset: {}, Z-offset: {}".format(leg, offsets[0], offsets[1], offsets[2]))
+            angles = calc_angles(desired_x + offsets[0], desired_y + offsets[1], desired_z + offsets[2])
+            set_leg(leg, angles)
+        except ValueError:
+            print("Failed to calculate angles")
+
+
+def set_leg(leg, angles):
+    index = 0
+    if leg == 0:
+        index = 3
+    elif leg == 1:
+        index = 0
+    elif leg == 2:
+        index = 2
+    elif leg == 3:
+        index = 1
+
+    set_servo(3 * index, (90 - degrees(angles[0])) if leg < 2 else (90 + degrees(angles[0])))
+    set_servo(3 * index + 1, (degrees(angles[1])) if leg < 2 else (180 - degrees(angles[1])))
+    set_servo(3 * index + 2, (90 + degrees(angles[2])) if leg % 2 else (90 - degrees(angles[2])))
+
+
+for servo in range(12):
+    pca.servo[servo].set_pulse_width_range(500, 2500)
+    # set_servo(servo, 90)
+
+
+set_legs(0, 15, 0, 0, 0, 0)
+
+
+def ease(x):
+    return -(cos(pi * x) - 1) / 2
+
+
+def run(leg, x):
+    # TL-BR-TR-BL
+    lifted_leg = 0
+    if x < 1:
+        lifted_leg = 2
+    elif x < 2:
+        lifted_leg = 1
+    elif x < 3:
+        lifted_leg = 0
+    elif x < 4:
+        lifted_leg = 3
+    actual_x = 0
+    if leg == 0:
+        actual_x = x + 2
+    elif leg == 1:
+        actual_x = x + 3
+    elif leg == 2:
+        actual_x = x
+    elif leg == 3:
+        actual_x = x + 1
+
+    # actual_x = x + leg
+    # lifted_leg = 4 - floor(x)
+    if actual_x >= 4:
+        actual_x -= 4
+
+    leg_start = -2.5
+    leg_end = 3.5
+
+    x_offset = leg_start + ((actual_x - 1) / 3) * (leg_end - leg_start)
+    # if lifted_leg != leg:
+    #     print("{}: {} {}".format(leg, x_offset, (actual_x - 1) / 3))
+    y_offset = 15
+    x_leg = (length / 2) if leg % 2 else -(length / 2)
+    z_leg = (width / 2) if leg < 2 else -(width / 2)
+    if lifted_leg == 0 or lifted_leg == 2:
+        desired_pitch = -5
+    else:
+        desired_pitch = 5
+    if lifted_leg == 0 or lifted_leg == 1:
+        desired_yaw = 7
+    else:
+        desired_yaw = -7
+
+    raised_leg_x = x - floor(x)
+
+    ease_point = 0.2
+    if raised_leg_x < ease_point:
+        smoothing = ease(raised_leg_x / ease_point)
+    elif 1 - ease_point < raised_leg_x <= 1:
+        smoothing = ease(1 + (raised_leg_x - 1 + ease_point) / ease_point)
+    else:
+        smoothing = 1
+    print(smoothing)
+
+    desired_pitch = desired_pitch * smoothing
+    desired_yaw = desired_yaw * smoothing
+    desired_pitch = 0
+    desired_yaw = 0
+
+    offsets = calc_leg_offsets(x_leg, z_leg, desired_pitch, desired_yaw, 0)
+
+    z_offset = 0
+    z_offset = (2 if lifted_leg < 2 else -2) * smoothing
+    x_offset += (2 if lifted_leg % 2 else -2) * smoothing
+    if lifted_leg == leg:
+        x_offset = 3 - ease(actual_x) * 6
+        y_offset = 15 - ease(actual_x * 2) * 5
+
+    # if lifted_leg == 1 and leg == 3:
+    #     y_offset += 2
+    # elif lifted_leg == 3 and leg == 1:
+    #     y_offset += 2
+
+    # if leg == 0:
+    #     print(str(leg) + ": (x:" + str(x_offset + offsets[0]) + ", y: " + str(y_offset + offsets[1]),
+    #           ", z: " + str(z_offset + offsets[2]))
+
+    angles = calc_angles(x_offset + offsets[0], y_offset + offsets[1], z_offset + offsets[2])
+    set_leg(leg, angles)
+
+
+status = 0
+
+# run(0, status)
+# run(1, status)
+# run(2, status)
+# run(3, status)
+
+while True:
+    status += 0.03
+    if status >= 4:
+        status = 0
+    # print(status)
+    run(0, status)
+    run(1, status)
+    run(2, status)
+    run(3, status)
+    sleep(0.01)
+
+
+class AQLARPController(Controller):
+    x = 0
+    y = 15
+    z = 0
+    pitch = 0
+    yaw = 0
+    rotation = 0
+
+    def __init__(self, **kwargs):
+        Controller.__init__(self, **kwargs)
+        self.apply()
+
+    def apply(self):
+        set_legs(self.x, self.y, self.z, self.pitch, self.yaw, self.rotation)
+
+    def on_up_arrow_press(self):
+        self.y += 0.5
+        self.apply()
+
+    def on_down_arrow_press(self):
+        self.y -= 0.5
+        self.apply()
+
+    def on_L3_x(self, value):
+        adjusted = value / 32767
+        self.z = -adjusted * 5
+        self.apply()
+
+    def on_L3_y(self, value):
+        adjusted = value / 32767
+        self.x = -adjusted * 5
+        self.apply()
+
+    def on_R3_x(self, value):
+        adjusted = value / 32767
+        self.yaw = -adjusted * 15
+        self.apply()
+
+    def on_R3_y(self, value):
+        adjusted = value / 32767
+        self.pitch = -adjusted * 15
+        self.apply()
+
+    def on_L3_left(self, value):
+        self.on_L3_x(value)
+
+    def on_L3_right(self, value):
+        self.on_L3_x(value)
+
+    def on_L3_x_at_rest(self):
+        self.on_L3_x(0)
+
+    def on_L3_down(self, value):
+        self.on_L3_y(value)
+
+    def on_L3_up(self, value):
+        self.on_L3_y(value)
+
+    def on_L3_y_at_rest(self):
+        self.on_L3_y(0)
+
+    def on_R3_left(self, value):
+        self.on_R3_x(value)
 
-height_diff_pitch = l * sin(radians(pitch)) / 2
-height_diff_yaw = b * sin(radians(yaw)) / 2
-leg_movement_x = l * (1 - cos(radians(pitch))) / 2
-leg_movement_y = b * (1 - cos(radians(yaw))) / 2
+    def on_R3_right(self, value):
+        self.on_R3_x(value)
 
-print(calculate_rotation(1.0, 1.0, -30.0))
+    def on_R3_x_at_rest(self):
+        self.on_R3_x(0)
 
-print("Height difference pitch: {}, height difference yaw: {}".format(height_diff_pitch, height_diff_yaw))
-print("Leg movement x: {}, leg movement y: {}".format(leg_movement_x, leg_movement_y))
+    def on_R3_down(self, value):
+        self.on_R3_y(value)
 
-l1 = 10.0
-l2 = 10.0
+    def on_R3_up(self, value):
+        self.on_R3_y(value)
 
-x = 0.0
-y = 15.0
-z = 0.0
+    def on_R3_y_at_rest(self):
+        self.on_R3_y(0)
 
-A = acos(((y * y + x * x) * (y * y + z * z) + y * y * l1 * l1 - y * y * l2 * l2)
-         / (2 * y * l1 * sqrt((y * y + x * x) * (y * y + z * z)))) + atan(x / y)
-B = acos((y * y * l1 * l1 + y * y * l2 * l2 - (y * y + x * x) * (y * y + z * z)) / (2 * y * y * l1 * l2))
-C = atan(z / y)
+# controller = AQLARPController(interface="/dev/input/js0", connecting_using_ds4drv=False)
+# controller.listen()
 
-print("A: {}, B: {}, C: {}".format(degrees(A), degrees(B), degrees(C)))
+# pca.servo[2].angle = 95
+# pca.servo[5].angle = 98
+# pca.servo[8].angle = 95
+# pca.servo[11].angle = 92
+#
+# while True:
+#     for h in range(0, 100, 1):
+#         angles = calc_angles(-2 + (100 - h) / 25, 12 + h / 20.0, 0)
+#         for i in range(4):
+#             if i <= 1:
+#                 pca.servo[3 * i].angle = 90 - degrees(angles[0])
+#                 pca.servo[3 * i + 1].angle = degrees(angles[1])
+#             else:
+#                 pca.servo[3 * i].angle = 90 + degrees(angles[0])
+#                 pca.servo[3 * i + 1].angle = 180 - degrees(angles[1])
+#         time.sleep(0.01)
+#     for h in range(100, 0, -1):
+#         angles = calc_angles(-2 + (100 - h) / 25, 12 + h / 20.0, 0)
+#         for i in range(4):
+#             if i <= 1:
+#                 pca.servo[3 * i].angle = 90 - degrees(angles[0])
+#                 pca.servo[3 * i + 1].angle = degrees(angles[1])
+#             else:
+#                 pca.servo[3 * i].angle = 90 + degrees(angles[0])
+#                 pca.servo[3 * i + 1].angle = 180 - degrees(angles[1])
+#         time.sleep(0.01)