Mercurial > Kubik
changeset 152:7504bb6ef6cc
joy
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Thu, 07 Aug 2014 17:50:34 +0200 |
| parents | af2b8a1fa8f3 |
| children | 6d63e4e4542d |
| files | joy.py kubik.py |
| diffstat | 2 files changed, 75 insertions(+), 39 deletions(-) [+] |
line wrap: on
line diff
--- a/joy.py Wed Aug 06 18:51:39 2014 +0200 +++ b/joy.py Thu Aug 07 17:50:34 2014 +0200 @@ -1,40 +1,76 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + import pygame +import serial import kubik - +import creep -FPS = 12 +FPS = 24 -pygame.display.init() -pygame.joystick.init() -pygame.mixer.init() - -clock = pygame.time.Clock() -hello = pygame.mixer.Sound('~/sound/Turret_turret_active_5.wav') -robot = kubik.Robot() - -for joy in range(pygame.joystick.get_count()): - pygame.joystick.Joystick(joy).init() +class Walk(creep.Creep): + def __init__(self, robot): + super(Walk, self).__init__(robot) + self.speed = [0.0, 0.0] + self.rotation = 0.0 + self.gait = '' -robot.home() -while True: - for event in pygame.event.get(): - if event.type == pygame.JOYBUTTONDOWN: - hello.play() - print "Hello!" - elif event.type == pygame.JOYAXISMOTION: - print event.axis, event.value - if event.axis == 0: - for leg in robot.legs[:2]: - leg.move_to((30 + 20 * event.value, None, None)) - for leg in robot.legs[2:]: - leg.move_to((30 - 20 * event.value, None, None)) - elif event.axis == 1: - for leg in robot.legs: - leg.move_to((None, 40 * event.value, None)) - elif event.axis == 2: - for leg in robot.legs: - leg.move_to((None, None, -30 - 30 * event.value)) - robot.update() - clock.tick(FPS) + def joy_anim(self): + """This animation is for reading user input.""" + while True: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + raise SystemExit() + elif event.type == pygame.JOYBUTTONDOWN: + if event.button == 10: + self.gait = 'forward' + elif event.button == 11: + if self.rotation > 0: + self.gait = 'rotate_left' + else: + self.gait = 'rotate_right' + elif event.button == 0: + serial = self.robot.legs[0].hip.serial + serial.write('0xfd') + sensor = ord(serial.read(1)) + print "Sensor value: %d" % sensor + else: + print "Button %d" % event.button + elif event.type == pygame.JOYBUTTONUP: + if event.button in (10, 11): + self.gait = '' + elif event.type == pygame.JOYAXISMOTION: + if event.axis == 0: + self.speed[0] = event.value + self.rotation = 0.0 + elif event.axis == 1: + self.speed[1] = -event.value + self.rotation = 0.0 + elif event.axis == 2: + self.robot.shift[2] = event.value * 12 + elif event.axis == 3: + self.rotation = event.value + self.speed = [0.0, 0.0] + yield + + + +if __name__ == '__main__': + # Initialize PyGame. + pygame.display.init() + pygame.joystick.init() + for joy in range(pygame.joystick.get_count()): + pygame.joystick.Joystick(joy).init() + + # Initialize the robot. + robot = kubik.Robot(serial.Serial('/dev/rfcomm0', 9600)) + robot.home() + walk = Walk(robot) + clock = pygame.time.Clock() + robot.run_animations([ + walk.stomp_anim(), + walk.retract_anim(), + walk.joy_anim(), + ], FPS)
--- a/kubik.py Wed Aug 06 18:51:39 2014 +0200 +++ b/kubik.py Thu Aug 07 17:50:34 2014 +0200 @@ -48,19 +48,19 @@ self.legs = ( Leg(HipServo(m, 1, reverse=True, trim=250), KneeServo(m, 2, reverse=True, trim=-200), - AnkleServo1(m, 0, reverse=True, trim=0, center=1500 * 4), - (-1, 1)), + AnkleServo1(m, 0, reverse=False, trim=0, center=1500 * 4), + (1, -1)), Leg(HipServo(m, 4, reverse=True, trim=-100), KneeServo(m, 5, reverse=False, trim=-200), - AnkleServo2(m, 3, reverse=False, trim=-240, center=1500 * 4), + AnkleServo2(m, 3, reverse=True, trim=-240, center=1500 * 4), (1, 1)), Leg(HipServo(m, 7, reverse=False, trim=-150), KneeServo(m, 8, reverse=True, trim=100), - AnkleServo1(m, 6, reverse=True, trim=0, center=1500 * 4), - (1, -1)), + AnkleServo1(m, 6, reverse=False, trim=0, center=1500 * 4), + (-1, 1)), Leg(HipServo(m, 10, reverse=False, trim=-250), KneeServo(m, 11, reverse=False, trim=-100), - AnkleServo2(m, 9, reverse=False, trim=100, center=1500 * 4), + AnkleServo2(m, 9, reverse=True, trim=100, center=1500 * 4), (-1, -1)), )