Mercurial > circuitpython-spiderwing

annotate python/gait.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 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:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
5 speed = 2
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
6 stride = 36
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
7 shift = 12
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
8 shift_steps = 2
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
9 raise_leg = 10
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
10 raise_steps = 2
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
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
28 self.spread = 20
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
29 leg = robot.legs[0]
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
30 self.height = 20
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 print("Moving legs backwards")
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
35 for leg in self.on_ground:
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
36 dx = self.dx * leg.x_dir * self.speed
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
37 dy = self.dy * leg.y_dir * self.speed
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
38 leg.move_to(leg.x - dx, leg.y - dy, self.height)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
39 #leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
40 utime.sleep_ms(10)
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 step(self, leg):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
43 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
44 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
45 print("Shifting body by (%d, %d) %d times" % (dx, dy, self.shift_steps))
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
46 for step in range(self.shift_steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
47 self.robot.move_by(dx, dy, 0)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
48 self.shift_x += dx
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
49 self.shift_y += dy
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
50 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
51 self.on_ground.remove(leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
52 print("Raising leg by %d %d times" % (self.raise_leg, self.raise_steps))
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
53 for step in range(self.raise_steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
54 leg.move_by(0, 0, self.raise_leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
55 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
56 print("Moving leg forward")
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
57 leg.move_to(
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
58 (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
59 (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
60 self.raise_leg * self.raise_steps + self.height
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
61 )
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.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
64 print("Lowering leg")
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
65 for step in range(self.raise_steps - 1):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
66 leg.move_by(0, 0, -self.raise_leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
67 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
68 self.on_ground.append(leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
69 self.move()
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
70 print("Leg done")
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
71
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
72 def run(self, steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
73 for step in range(steps):
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
74 print("Moving leg %d" % self.last_leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
75 leg = self.robot.legs[self.last_leg]
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
76 self.step(leg)
cf2cd5a35d24 Initial commit
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
77 self.last_leg = self.next_leg[0][self.last_leg]