Mercurial > circuitpython-spiderwing

comparison 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
comparison
equal deleted inserted replaced
-1:000000000000 0:cf2cd5a35d24
1 import utime
2
3
4 class Creep:
5 speed = 2
6 stride = 36
7 shift = 12
8 shift_steps = 2
9 raise_leg = 10
10 raise_steps = 2
11 next_leg = (
12 (2, 0, 3, 1), # forward
13 (1, 3, 0, 2), # backward
14 (1, 2, 3, 0), # clockwise
15 (3, 0, 1, 2), # counter-clockwise
16 (2, 3, 1, 0), # left
17 (3, 2, 0, 1), # right
18 )
19
20 def __init__(self, robot):
21 self.robot = robot
22 self.shift_x = 0
23 self.shift_y = 0
24 self.on_ground = list(robot.legs)
25 self.dx = 1
26 self.dy = 0
27 self.rotation = 0
28 self.spread = 20
29 leg = robot.legs[0]
30 self.height = 20
31 self.last_leg = 0
32
33 def move(self):
34 print("Moving legs backwards")
35 for leg in self.on_ground:
36 dx = self.dx * leg.x_dir * self.speed
37 dy = self.dy * leg.y_dir * self.speed
38 leg.move_to(leg.x - dx, leg.y - dy, self.height)
39 #leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
40 utime.sleep_ms(10)
41
42 def step(self, leg):
43 dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps
44 dy = (leg.y_dir * self.shift - self.shift_y) / self.shift_steps
45 print("Shifting body by (%d, %d) %d times" % (dx, dy, self.shift_steps))
46 for step in range(self.shift_steps):
47 self.robot.move_by(dx, dy, 0)
48 self.shift_x += dx
49 self.shift_y += dy
50 self.move()
51 self.on_ground.remove(leg)
52 print("Raising leg by %d %d times" % (self.raise_leg, self.raise_steps))
53 for step in range(self.raise_steps):
54 leg.move_by(0, 0, self.raise_leg)
55 self.move()
56 print("Moving leg forward")
57 leg.move_to(
58 (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread,
59 (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread,
60 self.raise_leg * self.raise_steps + self.height
61 )
62 self.move()
63 self.move()
64 print("Lowering leg")
65 for step in range(self.raise_steps - 1):
66 leg.move_by(0, 0, -self.raise_leg)
67 self.move()
68 self.on_ground.append(leg)
69 self.move()
70 print("Leg done")
71
72 def run(self, steps):
73 for step in range(steps):
74 print("Moving leg %d" % self.last_leg)
75 leg = self.robot.legs[self.last_leg]
76 self.step(leg)
77 self.last_leg = self.next_leg[0][self.last_leg]