Mercurial > tote0
changeset 7:576484754322
Use radians in IK and slow servo startup
| author | Radomir Dopieralski <dev@sheep.art.pl> |
|---|---|
| date | Tue, 26 Jan 2016 23:25:30 +0100 |
| parents | 7bd6c023b248 |
| children | ce5ba7f8163c |
| files | leg.py run.py |
| diffstat | 2 files changed, 14 insertions(+), 6 deletions(-) [+] |
line wrap: on
line diff
--- a/leg.py Tue Jan 26 22:25:31 2016 +0100 +++ b/leg.py Tue Jan 26 23:25:30 2016 +0100 @@ -4,6 +4,10 @@ import math +PI2 = math.pi / 2 +PI4 = math.pi / 4 + + def _solve_triangle(a, b, c): a, b, c = abs(a), abs(b), abs(c) if a + b < c or a + c < b or b + c < a: @@ -37,15 +41,12 @@ d = _norm(f, z) if d > self.femur + self.tibia: raise ValueError("Out of reach") - hip = math.atan2(y, x) + hip = math.atan2(y, x) - PI4 knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f) - ankle = _solve_triangle(self.femur, self.tibia, d) + ankle = _solve_triangle(self.femur, self.tibia, d) - PI2 - hip = math.degrees(hip) - 45 - knee = math.degrees(knee) - ankle = math.degrees(ankle) - 90 for servo, angle in zip(self.servos, (hip, knee, ankle)): - servo.move(degrees=angle) + servo.move(radians=angle) self.x = x self.y = y self.z = z
--- a/run.py Tue Jan 26 22:25:31 2016 +0100 +++ b/run.py Tue Jan 26 23:25:30 2016 +0100 @@ -4,9 +4,16 @@ import tote import servo import creep +import time robot = tote.Robot(servo.Servos()) + +for leg in robot.legs: + for servo in leg.servos: + servo.move(0) + time.sleep(0.25) + gait = creep.Creep(robot) for step in range(16): leg = robot.legs[gait.last_leg]