Advertisement
Guest User

Code

a guest
Jun 22nd, 2018
143
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.19 KB | None | 0 0
  1. #import subprocess
  2. #subprocess.check_call(["python", '-m', 'pip', 'install', 'numpy'])
  3. import pygame
  4. import numpy as np
  5. import socket
  6. import sys
  7.  
  8.  
  9.  
  10. import paramiko
  11. ssh_client = paramiko.SSHClient()
  12. ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
  13. ssh_client.connect(hostname='169.254.222.19',username='pi',password='raspberry')
  14. stdin,stdout,stderr=ssh_client.exec_command("sudo python3 server.py")
  15. ssh_client.close()
  16.  
  17. client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  18. client.connect(('169.254.222.19',5555))
  19.  
  20. pygame.init()
  21.  
  22. display_width = 800
  23. display_height = 600
  24. black = (0,0,0)
  25. white = (255,255,255)
  26. red = (255,0,0)
  27. green = (0,255,0)
  28. blue = (0,0,255)
  29.  
  30. logoImg = pygame.image.load(r'logo.png')
  31. gameDisplay = pygame.display.set_mode((display_width,display_height))
  32. pygame.display.set_caption('Control Module')
  33. clock = pygame.time.Clock()
  34.  
  35. gameDisplay.fill(white)
  36. gameDisplay.blit(logoImg,(0,0))
  37. pygame.draw.rect(gameDisplay, red, [100, 500, 100, 100])
  38. pygame.draw.rect(gameDisplay, red, [300, 500, 100, 100])
  39.  
  40. myfont = pygame.font.SysFont("monospace", 15)
  41. leftEngineLabel = myfont.render("Left Engine", 1, blue)
  42. rightEngineLabel = myfont.render("Right Engine", 1, blue)
  43.  
  44.  
  45. joysticks = []
  46. for i in range(0, pygame.joystick.get_count()):
  47. joysticks.append(pygame.joystick.Joystick(i))
  48. joysticks[-1].init()
  49. print(joysticks[-1].get_name())
  50.  
  51. crashed = False
  52. #left engine bar based at (100,500)
  53. #right enggine bar based at (300,500)
  54. valaxis0 = 0
  55. valaxis1 = 0
  56. right_engine = valaxis1
  57. left_engine = valaxis1
  58. while not crashed:
  59. gameDisplay.fill(white)
  60. gameDisplay.blit(logoImg,(0,0))
  61. AR = 0
  62. AL = 0
  63. if valaxis1>0:
  64. if valaxis0>0:
  65. AL = 1-valaxis0
  66. AR = 1
  67. else:
  68. AL = 1
  69. AR = 1+valaxis0
  70. else:
  71. if valaxis0>0:
  72. AL = 1
  73. AR = 1-valaxis0
  74. else:
  75. AL = 1+valaxis0
  76. AR = 1
  77. AR = AR*valaxis1
  78. AL = AL*valaxis1
  79. PV = valaxis0
  80. PVS = 1-abs(valaxis1)*1
  81. AL = (1-PVS)*AL - PVS*PV
  82. AR = (1-PVS)*AR + PVS*PV
  83.  
  84. gameDisplay.blit(leftEngineLabel, (100, 80))
  85. gameDisplay.blit(rightEngineLabel, (300, 80))
  86. pygame.draw.rect(gameDisplay, red, [100, 350, 50, int(200*AL)])
  87. pygame.draw.rect(gameDisplay, red, [300, 350, 50, int(200*AR)])
  88. #displaying engines PWM duty cycle
  89. leftEnginePWM = myfont.render("PWM:"+str("{0:.2f}".format(100*abs(AL)))+"%", 1, blue)
  90. rightEnginePWM = myfont.render("PWM:"+str("{0:.2f}".format(100*abs(AR)))+"%", 1, blue)
  91. gameDisplay.blit(leftEnginePWM, (100, 100))
  92. gameDisplay.blit(rightEnginePWM, (300, 100))
  93. #displaying engines direction
  94. leftEnginePWM = myfont.render("Direction:"+str(-np.sign(AL)), 1, blue)
  95. rightEnginePWM = myfont.render("Direction:"+str(-np.sign(AR)), 1, blue)
  96. gameDisplay.blit(leftEnginePWM, (100, 120))
  97. gameDisplay.blit(rightEnginePWM, (300, 120))
  98.  
  99. clockR = int((-np.sign(AR))>0)
  100. clockL = int((-np.sign(AL))>0)
  101. counterclockR = int((-np.sign(AR))<0)
  102. counterclockL = int((-np.sign(AL))<0)
  103. pwmR = int(100*abs(AR))
  104. pwmL = int(100*abs(AL))
  105.  
  106. for event in pygame.event.get():
  107. #print(event)
  108. if event.type == pygame.QUIT:
  109. crashed = True
  110. elif event.type == pygame.JOYAXISMOTION:
  111. if event.axis == 0:
  112. valaxis0 = event.value
  113. if event.axis == 1:
  114. valaxis1 = event.value
  115. elif event.type == pygame.JOYBUTTONDOWN:
  116. if event.button == 7:
  117. counterclockR = 2
  118. counterclockL = 2
  119. pwmR = 2
  120. pwmL = 2
  121. clockR = 2
  122. clockL = 2
  123. crashed = True
  124. #kill the engine
  125. #print(event)
  126. #sending data to the server on the raspberry
  127. send_bytes = str(clockR)+','+str(counterclockR)+','+str(pwmR)+','+str(clockL)+','+str(counterclockL)+','+str(pwmL)
  128. send_bytes = send_bytes.encode()
  129. send_bytes += b'\x00'*(256-len(send_bytes))
  130. client.sendall(send_bytes)
  131. pygame.display.update()
  132. clock.tick(30)
  133.  
  134. pygame.quit()
  135. quit()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement