Mercurial > tote-had

annotate python/gait.py @ 19:455a46355e72 default tip

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add freq to i2c init
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Thu, 14 Apr 2016 00:05:31 +0200
parents 3b9ed9cd63ef
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
8
7993a8f67e4c Fixing the gait
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 7
diff changeset
1 from pyb import delay
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
2
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
3
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
4 class Creep:
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
5 speed = 1
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
6 stride = 14
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
7 shift = 8
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
8 shift_steps = 2
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
9 raise_leg = 10
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
10 raise_steps = 3
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
11
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
12 def __init__(self, robot):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
13 self.robot = robot
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
14 self.shift_x = 0
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
15 self.shift_y = 0
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
16 self.on_ground = set(robot.legs)
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
17 self.dx = 0
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
18 self.dy = 1
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
19 self.rotation = 0
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
20 self.spread = 0
8
7993a8f67e4c Fixing the gait
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 7
diff changeset
21 self.height = 0
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
22 self.last_leg = 0
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
23
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
24 def move(self):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
25 for leg in self.on_ground:
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
26 dx = self.dx * leg.x_dir * self.speed
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
27 dy = self.dy * leg.y_dir * self.speed
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
28 leg.move_to(leg.x - dx, leg.y - dy, -self.height)
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
29 leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
30
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
31 def step(self, leg):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
32 dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
33 dy = (leg.y_dir * self.shift - self.shift_y) / self.shift_steps
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
34 for step in range(self.shift_steps):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
35 self.robot.move_by(dx, dy, 0)
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
36 self.shift_x += dx
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
37 self.shift_y += dy
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
38 yield
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
39 self.on_ground.remove(leg)
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
40 for step in range(self.raise_steps):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
41 leg.move_by(0, 0, self.raise_leg)
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
42 yield
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
43 leg.move_to(
8
7993a8f67e4c Fixing the gait
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 7
diff changeset
44 (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread,
7993a8f67e4c Fixing the gait
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 7
diff changeset
45 (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread,
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
46 self.raise_leg * self.raise_steps - self.height
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
47 )
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
48 yield
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
49 yield
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
50 for step in range(self.raise_steps - 1):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
51 leg.move_by(0, 0, -self.raise_leg)
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
52 yield
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
53 self.on_ground.add(leg)
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
54 yield
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
55
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
56 def step_order(self):
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
57 if abs(self.dy) > abs(self.dx):
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
58 if self.dy > 0:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
59 return (2, 0, 3, 1) # forward
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
60 else:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
61 return (1, 3, 0, 2) # backward
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
62 elif abs(self.dx) > abs(self.dy):
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
63 if self.dx < 0:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
64 return (2, 3, 1, 0) # left
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
65 else:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
66 return (3, 2, 0, 1) # right
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
67 elif abs(self.rotation) > 0.001:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
68 if self.rotation > 0:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
69 return (1, 2, 3, 0) # clockwise
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
70 else:
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
71 return (3, 0, 1, 2) # counter-clockwise
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
72 return (2, 0, 3, 1) # forward
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
73
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
74
14
3b9ed9cd63ef Add simple client and server
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 13
diff changeset
75 def run(self):
3b9ed9cd63ef Add simple client and server
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 13
diff changeset
76 while True:
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
77 leg = self.robot.legs[self.last_leg]
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
78 for frame in self.step(leg):
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
79 self.move()
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
80 self.robot.update()
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
81 delay(40) # 25 FPS
14
3b9ed9cd63ef Add simple client and server
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 13
diff changeset
82 yield
13
94feb678de75 Adjust step order to walking direction
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
83 self.last_leg = self.step_order()[self.last_leg]