Mercurial > circuitpython-spiderwing

annotate python/gait.py @ 3:8ed8f30b0568 draft

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Update gaits
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Sun, 23 Jul 2017 20:23:29 +0200
parents cf2cd5a35d24
children
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 import utime
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
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
4 class Creep:
3
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
5 speed = 2.3
0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
6 stride = 36
3
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
7 shift = 8
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
8 shift_steps = 1
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
9 raise_leg = 30
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
10 raise_steps = 1
0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
11 next_leg = (
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
12 (2, 0, 3, 1), # forward
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
13 (1, 3, 0, 2), # backward
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
14 (1, 2, 3, 0), # clockwise
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
15 (3, 0, 1, 2), # counter-clockwise
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
16 (2, 3, 1, 0), # left
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
17 (3, 2, 0, 1), # right
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
18 )
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
19
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
20 def __init__(self, robot):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
21 self.robot = robot
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
22 self.shift_x = 0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
23 self.shift_y = 0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
24 self.on_ground = list(robot.legs)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
25 self.dx = 1
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
26 self.dy = 0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
27 self.rotation = 0
3
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
28 self.spread = 0
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
29 self.raise_spread = 10
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
30 self.height = 30
0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
31 self.last_leg = 0
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 move(self):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
34 for leg in self.on_ground:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
35 dx = self.dx * leg.x_dir * self.speed
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
36 dy = self.dy * leg.y_dir * self.speed
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
37 leg.move_to(leg.x - dx, leg.y - dy, self.height)
3
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
38 leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
39 utime.sleep_ms(10)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
40
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
41 def step(self, leg):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
42 dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
43 dy = (leg.y_dir * self.shift - self.shift_y) / self.shift_steps
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
44 for step in range(self.shift_steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
45 self.robot.move_by(dx, dy, 0)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
46 self.shift_x += dx
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
47 self.shift_y += dy
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
48 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
49 self.on_ground.remove(leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
50 for step in range(self.raise_steps):
3
8ed8f30b0568 Update gaits
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 0
diff changeset
51 leg.move_by(self.raise_spread, self.raise_spread, self.raise_leg)
0
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
52 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
53 leg.move_to(
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
54 (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread,
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
55 (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread,
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
56 self.raise_leg * self.raise_steps + self.height
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
57 )
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
58 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
59 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
60 for step in range(self.raise_steps - 1):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
61 leg.move_by(0, 0, -self.raise_leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
62 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
63 self.on_ground.append(leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
64 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
65
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
66 def run(self, steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
67 for step in range(steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
68 leg = self.robot.legs[self.last_leg]
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
69 self.step(leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
70 self.last_leg = self.next_leg[0][self.last_leg]