Mercurial > circuitpython-spiderwing

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