changeset 144:f06c9a2fd33f

leg directions
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Wed, 06 Aug 2014 18:27:17 +0200
parents b86bba4dc6a2
children f72b9da2f73c
files kubik.py leg.py
diffstat 2 files changed, 18 insertions(+), 16 deletions(-) [+]
line wrap: on
line diff
--- a/kubik.py	Wed Aug 06 18:17:28 2014 +0200
+++ b/kubik.py	Wed Aug 06 18:27:17 2014 +0200
@@ -47,19 +47,19 @@
             Leg(HipServo(m, 1, reverse=True, trim=250),
                 KneeServo(m, 2, reverse=True, trim=-200),
                 AnkleServo1(m, 0, reverse=True, trim=0, center=1500 * 4),
-                False),
+                (-1, 1)),
             Leg(HipServo(m, 4, reverse=True, trim=-100),
                 KneeServo(m, 5, reverse=False, trim=-200),
                 AnkleServo2(m, 3, reverse=False, trim=-240, center=1500 * 4),
-                True),
+                (1, 1)),
             Leg(HipServo(m, 7, reverse=False, trim=-150),
                 KneeServo(m, 8, reverse=True, trim=100),
                 AnkleServo1(m, 6, reverse=True, trim=0, center=1500 * 4),
-                True),
+                (1, -1)),
             Leg(HipServo(m, 10, reverse=False, trim=-250),
                 KneeServo(m, 11, reverse=False, trim=-100),
                 AnkleServo2(m, 9, reverse=False, trim=100, center=1500 * 4),
-                False),
+                (-1, -1)),
         ]
         self.shift = [0.0, 0.0, 0.0]
 
--- a/leg.py	Wed Aug 06 18:17:28 2014 +0200
+++ b/leg.py	Wed Aug 06 18:27:17 2014 +0200
@@ -2,6 +2,10 @@
 # -*- coding: utf-8 -*-
 import math
 
+
+SQRT2 = math.sqrt(2)
+
+
 def _solve_triangle(a, b, c):
     """Calculate an angle of a triangle using the law of cosines."""
     a, b, c = abs(a), abs(b), abs(c)
@@ -35,20 +39,18 @@
     femur = 50.0  # between the knee and the ankle
     tibia = 45.0  # between the ankle and the foot
     base = 45.0, 45.0  # from the center of body
-    home = 45.0, 45.0, -35.0
 
-    def __init__(self, hip, knee, ankle, front=True):
+    def __init__(self, hip, knee, ankle, (x_dir, y_dir)):
         self.hip = hip
         self.knee = knee
         self.ankle = ankle
-        self.front = front
-        if self.front:
-            self.y_direction = 1
-        else:
-            self.y_direction = -1
-        self.hip_offset = -45 * self.y_direction
+        self.direction = x_dir, y_dir
+        self.hip_offset = -45 * self.direction[1]
         self.home = (
-            self.home[0], self.home[1] * self.y_direction, self.home[2])
+            (self.coxa + self.femur) / SQRT2,
+            -(self.coxa + self.femur) / SQRT2 * self.direction[1],
+            -self.tibia,
+        )
         self.position = list(self.home)
 
     def __repr__(self):
@@ -93,15 +95,15 @@
 
     def rotate(self, degrees):
         """Rotate the leg around the center of the body horizontally."""
-        radians = math.radians(degrees * self.y_direction)
+        radians = math.radians(degrees)
         x, y, z = self.position
         bx = self.base[0] + x
-        by = self.base[1] + y * self.y_direction
+        by = self.base[1] - y * self.direction[1]
         r = _length(bx, by)
         a = math.atan2(bx, by)
         rx = r * math.sin(a + radians) - self.base[0]
         ry = r * math.cos(a + radians) - self.base[1]
-        self.move_to((rx, ry * self.y_direction, None))
+        self.move_to((rx, -ry * self.direction[1], None))
 
     def update(self, shift):
         dx, dy, dz = shift