Mercurial > Kubik
changeset 138:49d2e68a50b3
control.py
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Fri, 18 Jul 2014 08:34:27 +0200 |
| parents | 62af0b5c0e8e |
| children | 03e30dd7c34e |
| files | control.py kubik.py walk.py wave.py |
| diffstat | 4 files changed, 145 insertions(+), 13 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/control.py Fri Jul 18 08:34:27 2014 +0200 @@ -0,0 +1,126 @@ +import pygame + +import kubik + + +FPS = 12 + + +class Walk(object): + def __init__(self, robot): + self.robot = robot + self.speed = [0, 0] + self.rotation = 0.0 + self.stride = 35 + self.shift_speed = 3 + self.shift_steps = 4 + self.down = -55 + self.up = -20 + self.legs = [ + (self.robot.legs[0], (1, -1), 1, (55, -55)), + (self.robot.legs[1], (1, 1), 1, (55, 55)), + (self.robot.legs[3], (-1, -1), -1, (55, -55)), + (self.robot.legs[2], (-1, 1), -1, (55, 55)), + ] + self.on_floor = set((leg, side) + for leg, shift, side, home in self.legs) + self.blip = pygame.mixer.Sound('sound/blip.wav') + + def _retract_leg(self, step): + dx, dy = self.speed + leg, side = step + leg.move_by((-dx * side, -dy, 0)) + leg.rotate(self.rotation * side / 3) + + def _retract(self): + while True: + for step in self.on_floor: + self._retract_leg(step) + yield + + def _raise_leg(self, leg, side): + leg.move_to((None, None, self.up)) + self.on_floor.remove((leg, side)) + self.blip.play() + + def _advance_leg(self, leg, side, home): + dx, dy = self.speed + leg.move_to(( + home[0] + dx * self.stride * side - (abs(dx) + abs(dy)) * 15, + home[1] + dy * self.stride - ( + abs(dy) + abs(dx)) * 15 * leg.y_direction, + None, + )) + leg.rotate(-self.rotation * self.stride * side / 3) + + def _lower_leg(self, leg): + leg.move_to((None, None, self.down + 1)) + + def _ground_leg(self, leg, side): + leg.move_to((None, None, self.down)) + self.on_floor.add((leg, side)) + + def _stomp(self): + shifted = 0, 0 + while True: + for leg, shift, side, home in self.legs: + for frame in range(self.shift_steps): + if shift[0] != shifted[0]: + self.robot.shift[0] += shift[0] * self.shift_speed + if shift[1] != shifted[1]: + self.robot.shift[1] += shift[1] * self.shift_speed + yield + self._raise_leg(leg, side) + for frame in range(2): + yield + self._advance_leg(leg, side, home) + for frame in range(2): + yield + self._lower_leg(leg) + for frame in range(2): + yield + self._ground_leg(leg, side) + for frame in range(self.shift_steps): + if shift[0] != shifted[0]: + self.robot.shift[0] -= shift[0] * self.shift_speed + if shift[1] != shifted[1]: + self.robot.shift[1] -= shift[1] * self.shift_speed + yield + shifted = shift + + def run(self): + clock = pygame.time.Clock() + stomp = self._stomp() + retract = self._retract() + try: + while True: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + raise SystemExit() + elif event.type == pygame.JOYBUTTONDOWN: + if event.button == 1: + self.blip.play() + elif event.type == pygame.JOYAXISMOTION: + if event.axis == 0: + event.speed[1] = event.value + elif event.axis == 1: + event.speed[0] = event.value + elif event.axis == 3: + event.rotation = event.value + stomp.next() + retract.next() + robot.update() + clock.tick(FPS) + finally: + self.robot.home() + + +if __name__ == '__main__': + pygame.jostick.init() + pygame.mixer.init() + for joy in range(pygame.joystick.get_count()): + pygame.joystick.Joystick(joy).init() + robot = kubik.Robot() + robot.home() + walk = Walk(robot) + walk.run()
--- a/kubik.py Tue Jul 15 22:56:33 2014 +0200 +++ b/kubik.py Fri Jul 18 08:34:27 2014 +0200 @@ -149,11 +149,11 @@ KneeServo(m, 2, trim=-200), AnkleServo1(m, 0, trim=0, center=1500 * 4), False), - Leg(HipServo(m, 4, reverse=True, trim=-100), + Leg(HipServo(m, 4, trim=-100), KneeServo(m, 5, reverse=True, trim=-200), - AnkleServo2(m, 3, reverse=True, trim=-400, center=1500 * 4), + AnkleServo2(m, 3, reverse=True, trim=-240, center=1500 * 4), True), - Leg(HipServo(m, 7, reverse=False, trim=-150), + Leg(HipServo(m, 7, reverse=True, trim=-150), KneeServo(m, 8, trim=100), AnkleServo1(m, 6, trim=0, center=1500 * 4), True),
--- a/walk.py Tue Jul 15 22:56:33 2014 +0200 +++ b/walk.py Fri Jul 18 08:34:27 2014 +0200 @@ -9,21 +9,22 @@ class Walk(object): def __init__(self, robot): self.robot = robot - self.speed = [0, 0] - self.rotation = 1.0 + self.speed = [0, 1] + self.rotation = 0.0 self.stride = 35 self.shift_speed = 3 self.shift_steps = 4 - self.down = -40 + self.down = -55 self.up = -20 self.legs = [ - (self.robot.legs[0], (1, -1), 1, (35, -35)), - (self.robot.legs[1], (1, 1), 1, (35, 35)), - (self.robot.legs[3], (-1, -1), -1, (35, -35)), - (self.robot.legs[2], (-1, 1), -1, (35, 35)), + (self.robot.legs[0], (1, -1), 1, (55, -55)), + (self.robot.legs[1], (1, 1), 1, (55, 55)), + (self.robot.legs[3], (-1, -1), -1, (55, -55)), + (self.robot.legs[2], (-1, 1), -1, (55, 55)), ] self.on_floor = set((leg, side) for leg, shift, side, home in self.legs) + self.blip = pygame.mixer.Sound('sound/blip.wav') def _retract_leg(self, step): dx, dy = self.speed @@ -40,6 +41,7 @@ def _raise_leg(self, leg, side): leg.move_to((None, None, self.up)) self.on_floor.remove((leg, side)) + self.blip.play() def _advance_leg(self, leg, side, home): dx, dy = self.speed @@ -72,8 +74,10 @@ for frame in range(2): yield self._advance_leg(leg, side, home) + for frame in range(2): + yield self._lower_leg(leg) - for frame in range(4): + for frame in range(2): yield self._ground_leg(leg, side) for frame in range(self.shift_steps): @@ -99,6 +103,8 @@ if __name__ == '__main__': + # pygame.jostick.init() + pygame.mixer.init() robot = kubik.Robot() walk = Walk(robot) walk.run()
--- a/wave.py Tue Jul 15 22:56:33 2014 +0200 +++ b/wave.py Fri Jul 18 08:34:27 2014 +0200 @@ -10,10 +10,10 @@ robot.legs[2].move_to((30, 55, 20)) for i in range(5): - for y in range(20, 40, 3): + for y in range(40, 60, 3): robot.legs[2].move_to((None, None, y)) yield - for y in range(40, 20, -3): + for y in range(60, 40, -3): robot.legs[2].move_to((None, None, y)) yield