Mercurial > circuitpython-spiderwing

comparison python/robot.py @ 0:cf2cd5a35d24 draft

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Initial commit
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Mon, 15 May 2017 16:36:08 +0200
parents
children 8ed8f30b0568
comparison
equal deleted inserted replaced
-1:000000000000 0:cf2cd5a35d24
1 # -*- coding: utf-8 -*-
2
3 import math
4
5 from servos import Servos
6 from machine import I2C, Pin
7
8
9 PI2 = math.pi / 2
10 PI4 = math.pi / 4
11 SQRT2 = math.sqrt(2)
12
13
14 def _solve_triangle(a, b, c):
15 a, b, c = abs(a), abs(b), abs(c)
16 if a + b < c or a + c < b or b + c < a:
17 raise ValueError("Impossible triangle: %r, %r, %r" % (a, b, c))
18 cos = float(a ** 2 + b ** 2 - c ** 2) / (2 * a * b)
19 return math.acos(cos)
20
21
22 def _norm(a, b):
23 return math.sqrt(a ** 2 + b ** 2)
24
25
26 class Leg:
27 # Lengths of leg segments, in mm
28 coxa = 36 # between the hip and the knee
29 femur = 39 # between the knee and the ankle
30 tibia = 55 # between the ankle and the foot
31 base = 21 # from the center of body
32
33 def __init__(self, joints, direction, servos):
34 self.servos = servos
35 self.joints = joints
36 self.x_dir, self.y_dir = direction
37 self.home = (self.coxa + self.femur) / math.sqrt(2)
38 self.x = 0
39 self.y = 0
40 self.z = 0
41
42 def move_to(self, x, y, z):
43 print("Moving leg %d, %d to %d, %d, %d" % (self.x_dir, self.y_dir,
44 x, y, z))
45 x += self.home
46 y += self.home
47 z -= self.tibia
48 f = _norm(x, y) - self.coxa
49 d = _norm(f, z)
50 if d > self.femur + self.tibia:
51 raise ValueError("Out of reach")
52 hip = PI4 - math.atan2(y, x)
53 knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
54 ankle = PI2 - _solve_triangle(self.femur, self.tibia, d)
55 for joint, angle in zip(self.joints, (hip, knee, ankle)):
56 self.servos.position(
57 joint,
58 radians=PI2 - angle * self.x_dir * self.y_dir
59 )
60 self.x = x - self.home
61 self.y = y - self.home
62 self.z = z + self.tibia
63
64 def move_by(self, dx=0, dy=0, dz=0):
65 self.move_to(self.x + dx, self.y + dy, self.z + dz)
66
67 def rotate_by(self, radians):
68 x = self.x + self.base
69 y = self.y + self.base
70 nx = x * math.cos(radians) - y * math.sin(radians) - self.base
71 ny = x * math.sin(radians) + y * math.cos(radians) - self.base
72 self.move_to(nx, ny, self.z)
73
74
75 class Robot:
76 def __init__(self):
77 i2c = I2C(-1, Pin(5, Pin.OUT, Pin.PULL_UP),
78 Pin(4, Pin.OUT, Pin.PULL_UP), freq=100000)
79 self.servos = Servos(i2c)
80 self.legs = (
81 Leg((0, 1, 2), (1, -1), self.servos),
82 Leg((7, 6, 5), (-1, -1), self.servos),
83 Leg((8, 9, 10), (-1, 1), self.servos),
84 Leg((15, 14, 13), (1, 1), self.servos),
85 )
86
87 def move_by(self, dx=0, dy=0, dz=0):
88 for leg in self.legs:
89 leg.move_by(dx * leg.x_dir, dy * leg.y_dir, dz)
90
91 def rotate_by(self, radians):
92 for leg in self.legs:
93 leg.rotate_by(radians * leg.x_dir * leg.y_dir)
94
95 def home(self):
96 for leg in self.legs:
97 leg.move_to(0, 0, 0)