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.

194 lines
4.4 KiB

6 years ago
  1. #!/usr/bin/env python3
  2. import pydle
  3. import socket
  4. import cmd
  5. import queue
  6. import threading
  7. import time
  8. import traceback
  9. from packet import Packet, CMD
  10. VOTE_TIME=0.5
  11. CHANNEL='#turtle_donatello'
  12. FB_SPEED = 0.2
  13. LR_SPEED = 1.0
  14. TURTLEBOT_ADDR = ('127.0.0.1', 13676)
  15. socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
  16. cmd_queue = queue.Queue(8)
  17. stop = Packet()
  18. stop.write_ubyte(CMD.MOTION)
  19. stop.write_double(0.0)
  20. stop.write_double(0.0)
  21. class KAThread(threading.Thread):
  22. def run(self):
  23. pkt = Packet()
  24. pkt.write_ubyte(CMD.KEEPALIVE)
  25. while True:
  26. socket.sendto(bytes(pkt), TURTLEBOT_ADDR)
  27. time.sleep(0.25)
  28. kathr = KAThread()
  29. kathr.setDaemon(True)
  30. kathr.start()
  31. def try_message(target, msg):
  32. return
  33. print('SENT', target, ':', msg)
  34. try:
  35. tirc.message(target, msg)
  36. except Exception:
  37. traceback.print_exc()
  38. class CmdThread(threading.Thread):
  39. def run(self):
  40. while True:
  41. try_message(CHANNEL, '--- {}s voting round begins ---'.format(VOTE_TIME))
  42. time.sleep(VOTE_TIME)
  43. print('--- Voting Round ---')
  44. major = tbcmds.get_majority()
  45. if not major:
  46. print('No commands.')
  47. continue
  48. print('Winner: {}'.format(tbcmds.pretty_cmd(major)))
  49. try_message(CHANNEL, 'Voted majority: {}'.format(tbcmds.pretty_cmd(major)))
  50. vfw, vlr, tm = major
  51. print('Starting motion:', vfw, vlr, tm)
  52. pkt = Packet()
  53. pkt.write_ubyte(CMD.MOTION)
  54. pkt.write_double(vfw)
  55. pkt.write_double(vlr)
  56. socket.sendto(bytes(pkt), TURTLEBOT_ADDR)
  57. time.sleep(tm)
  58. print('Stopping motion:', vfw, vlr, tm)
  59. socket.sendto(bytes(stop), TURTLEBOT_ADDR)
  60. cmdthr = CmdThread()
  61. cmdthr.setDaemon(True)
  62. cmdthr.start()
  63. class TBotCmds(cmd.Cmd):
  64. votes = {}
  65. votelock = threading.Lock()
  66. has_cmd = False
  67. def issue_cmd(self, cmd):
  68. with self.votelock:
  69. for k, v in self.votes.items():
  70. v.discard(self.context)
  71. self.votes.setdefault(cmd, set()).add(self.context)
  72. def get_majority(self):
  73. with self.votelock:
  74. if not self.votes:
  75. return None
  76. all_votes = sorted(self.votes.items(), key=lambda pair: len(pair[1]), reverse = True)
  77. self.votes.clear()
  78. return all_votes[0][0]
  79. def repr_votes(self):
  80. with self.votelock:
  81. if not self.votes:
  82. return '(None)'
  83. return ', '.join('{} for {}'.format(len(v), self.pretty_cmd(k)) for k, v in self.votes.items())
  84. def pretty_cmd(self, cmd):
  85. fb_name = 'In Place'
  86. if cmd[0] > 0:
  87. fb_name = 'Forward {}m/s'.format(cmd[0])
  88. if cmd[0] < 0:
  89. fb_name = 'Backward {}m/s'.format(cmd[0])
  90. lr_name = 'Straight'
  91. if cmd[1] > 0:
  92. lr_name = 'Left {}rad/s'.format(cmd[1])
  93. if cmd[1] < 0:
  94. lr_name = 'Right {}rad/s'.format(cmd[1])
  95. return '{} {} for {}s'.format(fb_name, lr_name, cmd[2])
  96. @staticmethod
  97. def clamp_time(ts):
  98. if not ts:
  99. ts = '1'
  100. t = float(ts)
  101. return max(min(1000000, t), 0.5)
  102. def do_forward(self, rest):
  103. t = self.clamp_time(rest)
  104. self.issue_cmd((FB_SPEED, 0.0, t))
  105. self.has_cmd = True
  106. do_fw = do_forward
  107. do_f = do_forward
  108. do_w = do_forward
  109. #def do_backward(self, rest):
  110. # t = self.clamp_time(rest)
  111. # self.issue_cmd((-FB_SPEED, 0.0, t))
  112. # self.has_cmd = True
  113. #do_back = do_backward
  114. #do_bw = do_backward
  115. #do_b = do_backward
  116. def do_stay(self, rest):
  117. t = self.clamp_time(rest)
  118. self.issue_cmd((0.0, 0.0, t))
  119. self.has_cmd = True
  120. do_stop = do_stay
  121. do_s = do_stay
  122. def do_left(self, rest):
  123. t = self.clamp_time(rest)
  124. self.issue_cmd((0.0, LR_SPEED, t))
  125. self.has_cmd = True
  126. do_lt = do_left
  127. do_l = do_left
  128. do_a = do_left
  129. def do_right(self, rest):
  130. t = self.clamp_time(rest)
  131. self.issue_cmd((0.0, -LR_SPEED, t))
  132. self.has_cmd = True
  133. do_rt = do_right
  134. do_r = do_right
  135. do_d = do_right
  136. def do_votes(self, rest):
  137. try_message(CHANNEL, 'Votes: {}'.format(tbcmds.repr_votes()))
  138. self.has_cmd = True
  139. do_v = do_votes
  140. tbcmds = TBotCmds()
  141. class TIRCBot(pydle.Client):
  142. def on_connect(self):
  143. print('Joining...')
  144. self.join(CHANNEL)
  145. def on_message(self, target, source, message):
  146. print('RECV:', target, ':', source, ':', message)
  147. try:
  148. tbcmds.context = source
  149. parts = message.split(';')
  150. for part in parts:
  151. try:
  152. tbcmds.onecmd(part)
  153. except Exception:
  154. traceback.print_exc()
  155. if tbcmds.has_cmd:
  156. tbcmds.has_cmd = False
  157. except Exception:
  158. traceback.print_exc()
  159. if __name__ == '__main__':
  160. tirc = TIRCBot('turtle_donatello')
  161. print('Connecting...')
  162. tirc.connect('irc.twitch.tv', 6667, password='nothing_to_see_here_move_on')
  163. try:
  164. tirc.handle_forever()
  165. except KeyboardInterrupt:
  166. tirc.message(CHANNEL, 'Software restarting, be back in a bit...')
  167. time.sleep(2)