Votes Internally!
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

96 lines
2.9 KiB

6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
  1. import socket
  2. import pygame
  3. import sys
  4. import time
  5. from packet import Packet, CMD
  6. pygame.init()
  7. disp = pygame.display.set_mode((640, 480))
  8. font = pygame.font.SysFont(pygame.font.get_default_font(), 24)
  9. joys = [pygame.joystick.Joystick(i) for i in range(pygame.joystick.get_count())]
  10. for joy in joys:
  11. joy.init()
  12. LIN_AX = 1
  13. ANG_AX = 0
  14. LIN_V = 0.2
  15. ANG_V = 1.0
  16. PARMS = {
  17. 0: 'LIN_V',
  18. 1: 'ANG_V',
  19. }
  20. DOWN = 4
  21. UP = 5
  22. TIMER_KA = pygame.USEREVENT
  23. KA_PKT = Packet()
  24. KA_PKT.write_ubyte(CMD.KEEPALIVE)
  25. pygame.time.set_timer(TIMER_KA, 100)
  26. sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
  27. addr = (sys.argv[1], int(sys.argv[2]))
  28. lasthat = None
  29. vmap = [
  30. [7, 0, 1],
  31. [6, -1, 2],
  32. [5, 4, 3],
  33. ]
  34. clock = pygame.time.Clock()
  35. while True:
  36. for ev in pygame.event.get():
  37. if ev.type == pygame.KEYDOWN and ev.key == pygame.K_ESCAPE:
  38. exit()
  39. if ev.type == pygame.JOYHATMOTION and ev.joy == joy.get_id():
  40. lasthat = time.time()
  41. if ev.type == TIMER_KA:
  42. sock.sendto(bytes(KA_PKT), addr)
  43. if any(joy.get_button(i) for i in (UP, DOWN)):
  44. cnt = 0
  45. for but, var in PARMS.items():
  46. if joy.get_button(but):
  47. globals()[var] += (0.1 if joy.get_button(UP) else -0.1)
  48. print(var, ':', globals()[var])
  49. cnt += 1
  50. if not cnt:
  51. print('WARN: no parms modified, try pressing another button')
  52. if lasthat is not None and time.time() - lasthat > 0.05:
  53. hx, hy = joy.get_hat(0)
  54. val = vmap[1 - hy][1 + hx]
  55. print('snd', val)
  56. if val >= 0 and val <= 6:
  57. pkt = Packet()
  58. pkt.write_ubyte(CMD.SOUND)
  59. pkt.write_ubyte(val)
  60. sock.sendto(bytes(pkt), addr)
  61. lasthat = None
  62. lv, av = joy.get_axis(LIN_AX), joy.get_axis(ANG_AX)
  63. pkt = Packet()
  64. pkt.write_ubyte(CMD.MOTION)
  65. pkt.write_double(-LIN_V * lv)
  66. pkt.write_double(-ANG_V * av)
  67. sock.sendto(bytes(pkt), addr)
  68. w, h = disp.get_size()
  69. r = int(min(w / 2, h / 2))
  70. disp.fill((0, 0, 0))
  71. pygame.draw.circle(disp, (0, 0, 255), (int(w / 2), int(h / 2)), r, 1)
  72. pygame.draw.rect(disp, (127, 0, 255), (int(w / 2 - r), int(h / 2 - r), 2*r, 2*r), 1)
  73. pygame.draw.circle(disp, (255, 255, 255), (int(w / 2 + r * av), int(h / 2 + r * lv)), 5)
  74. bottom = h
  75. for but, var in PARMS.items():
  76. if joy.get_button(but):
  77. val = globals()[var]
  78. surf = font.render('%s: %0.1f'%(var, val), True, (255, 255, 255))
  79. disp.fill((0, 0, 255), (0, bottom - surf.get_height(), int(w * val / 5), surf.get_height()))
  80. disp.blit(surf, (int(w / 2 - surf.get_width() / 2), bottom - surf.get_height()))
  81. bottom -= surf.get_height()
  82. pygame.display.flip()
  83. clock.tick(30)