Mercurial > circuitpython-spiderwing

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