Mercurial > circuitpython-spiderwing
diff python/gait.py @ 0:cf2cd5a35d24 draft
Initial commit
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Mon, 15 May 2017 16:36:08 +0200 |
| parents | |
| children | 8ed8f30b0568 |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/python/gait.py Mon May 15 16:36:08 2017 +0200 @@ -0,0 +1,77 @@ +import utime + + +class Creep: + speed = 2 + stride = 36 + shift = 12 + shift_steps = 2 + raise_leg = 10 + raise_steps = 2 + next_leg = ( + (2, 0, 3, 1), # forward + (1, 3, 0, 2), # backward + (1, 2, 3, 0), # clockwise + (3, 0, 1, 2), # counter-clockwise + (2, 3, 1, 0), # left + (3, 2, 0, 1), # right + ) + + def __init__(self, robot): + self.robot = robot + self.shift_x = 0 + self.shift_y = 0 + self.on_ground = list(robot.legs) + self.dx = 1 + self.dy = 0 + self.rotation = 0 + self.spread = 20 + leg = robot.legs[0] + self.height = 20 + self.last_leg = 0 + + def move(self): + print("Moving legs backwards") + for leg in self.on_ground: + dx = self.dx * leg.x_dir * self.speed + dy = self.dy * leg.y_dir * self.speed + leg.move_to(leg.x - dx, leg.y - dy, self.height) + #leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2) + utime.sleep_ms(10) + + def step(self, leg): + dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps + dy = (leg.y_dir * self.shift - self.shift_y) / self.shift_steps + print("Shifting body by (%d, %d) %d times" % (dx, dy, self.shift_steps)) + for step in range(self.shift_steps): + self.robot.move_by(dx, dy, 0) + self.shift_x += dx + self.shift_y += dy + self.move() + self.on_ground.remove(leg) + print("Raising leg by %d %d times" % (self.raise_leg, self.raise_steps)) + for step in range(self.raise_steps): + leg.move_by(0, 0, self.raise_leg) + self.move() + print("Moving leg forward") + leg.move_to( + (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread, + (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread, + self.raise_leg * self.raise_steps + self.height + ) + self.move() + self.move() + print("Lowering leg") + for step in range(self.raise_steps - 1): + leg.move_by(0, 0, -self.raise_leg) + self.move() + self.on_ground.append(leg) + self.move() + print("Leg done") + + def run(self, steps): + for step in range(steps): + print("Moving leg %d" % self.last_leg) + leg = self.robot.legs[self.last_leg] + self.step(leg) + self.last_leg = self.next_leg[0][self.last_leg]
