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.
 
 
 

192 lines
5.7 KiB

  1. from glob import glob
  2. import os
  3. from threading import Thread
  4. from time import sleep
  5. from smbus import SMBus
  6. from time import sleep
  7. import sys
  8. import socket
  9. import time
  10. from configparser import ConfigParser
  11. """
  12. scp tank.py root@tank-465ce7: && ssh root@tank-465ce7 "python3 tank.py"
  13. """
  14. os.chdir("/root/")
  15. config = ConfigParser()
  16. config.read("config.ini")
  17. enemy_bytes = config.get('tank','enemy_bytes').split(",")
  18. robot_id = hex(int(config.get('tank','robot_id'),10))
  19. hit_delay = int(config.get('tank','hit_delay'),10)
  20. hit_delay = int(config.get('tank','hit_delay'),10)
  21. #robot_id = socket.gethostname().split("-")[-1]
  22. robot_shot_time = 0
  23. robot_hit_timer = 0
  24. bus = SMBus(0)
  25. hit = False
  26. #os.system("ifconfig wlan0 mtu 2304")
  27. GSTREAM = """gst-launch-1.0 -v v4l2src device=%(cam)s ! capsfilter caps="video/x-h264, width=%(width)d, height=%(height)s, framerate=20/1" ! rtph264pay config-interval=10 pt=96 ! udpsink host=%(remote)s port=5000 &"""
  28. sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
  29. sock.settimeout(0.2)
  30. sock.bind(("", 37020))
  31. print("Receiver started")
  32. #sockaddr = "192.168.2.255", 55555
  33. #sock.sendto(b"hello", sockaddr)
  34. def filter_scale(axis):
  35. tresh = 3000
  36. if abs(axis) < tresh:
  37. return 0
  38. if axis > 0:
  39. return (axis-tresh) * 255 / (32768-tresh)
  40. else:
  41. return (axis+tresh) * 255 / (32768-tresh)
  42. class MotorDriver(object):
  43. MotorSpeedSet = 0x82
  44. PWMFrequenceSet = 0x84
  45. DirectionSet = 0xaa
  46. MotorSetA = 0xa1
  47. MotorSetB = 0xa5
  48. Nothing = 0x01
  49. EnableStepper = 0x1a
  50. UnenableStepper = 0x1b
  51. Stepernu = 0x1c
  52. I2CMotorDriverAdd = 0x0f #Set the address of the I2CMotorDriver
  53. def __init__(self, bus, address=0x0f):
  54. self.I2CMotorDriverAdd=address
  55. #Maps speed from 0-100 to 0-255
  56. def map_vals(self,value, leftMin, leftMax, rightMin, rightMax):
  57. #http://stackoverflow.com/questions/1969240/mapping-a-range-of-values-to-another
  58. # Figure out how 'wide' each range is
  59. leftSpan = leftMax - leftMin
  60. rightSpan = rightMax - rightMin
  61. # Convert the left range into a 0-1 range (float)
  62. valueScaled = float(value - leftMin) / float(leftSpan)
  63. # Convert the 0-1 range into a value in the right range.
  64. return int(rightMin + (valueScaled * rightSpan))
  65. #Set motor speed
  66. def MotorSpeedSetAB(self,MotorSpeedA,MotorSpeedB):
  67. #MotorSpeedA=self.map_vals(MotorSpeedA,0,32767,0,255)
  68. #MotorSpeedB=self.map_vals(MotorSpeedB,0,32767,0,255)
  69. bus.write_i2c_block_data(self.I2CMotorDriverAdd, self.MotorSpeedSet, [MotorSpeedA,MotorSpeedB])
  70. sleep(.02)
  71. #Set motor direction
  72. def MotorDirectionSet(self,Direction):
  73. bus.write_i2c_block_data(self.I2CMotorDriverAdd, self.DirectionSet, [Direction,0])
  74. sleep(.02)
  75. bus = SMBus(0)
  76. m = MotorDriver(bus)
  77. oldDirection = None
  78. old_speed = 0, 0
  79. addr = None
  80. class irReciver(Thread):
  81. def run(self):
  82. global hit, addr
  83. global robot_hit_timer
  84. while True:
  85. try:
  86. bytes=bus.read_i2c_block_data(0x10,0x02,5)
  87. byte = hex(bytes[1] << 24 | bytes[2] << 16 | bytes[3] << 8 | bytes[4])
  88. if byte != "0x0":
  89. print(byte)
  90. #print(byte)
  91. # if code belong to enemy then hit
  92. if byte in enemy_bytes and byte !=robot_id and not hit:
  93. print("Cabuum")
  94. hit = True
  95. robot_hit_timer = time.time()
  96. bus.write_byte_data(0x10,0x06,0x02)
  97. except Exception as e:
  98. print(e)
  99. sleep(0.2)
  100. t = irReciver(daemon=True)
  101. t.start()
  102. while True:
  103. try:
  104. data, addr = sock.recvfrom(100)
  105. except OSError:
  106. m.MotorSpeedSetAB(0,0)
  107. continue
  108. buf = data.decode("ascii").split(":")
  109. cmd = buf[0]
  110. if hit:
  111. sock.sendto("hit".encode("ascii"),addr)
  112. m.MotorDirectionSet(0b0110)
  113. hit = False
  114. if time.time() - robot_hit_timer < 5:
  115. m.MotorDirectionSet(0b0110)
  116. m.MotorSpeedSetAB(250,250)
  117. continue
  118. else:
  119. #end led flashing
  120. bus.write_byte_data(0x10,0x06,0x01)
  121. if cmd == "play":
  122. remote, port = addr
  123. width, height = int(buf[1]), int(buf[2])
  124. os.system("killall gst-launch-1.0")
  125. cam = sorted(glob("/dev/video*"))[-1]
  126. cmd = GSTREAM % locals()
  127. print("Launching:", cmd)
  128. os.system(cmd)
  129. elif cmd == "shoot":
  130. if time.time() - robot_shot_time > 1:
  131. #bus.write_byte_data(0x20,0x01,socket.gethostname().split("-")[-1])
  132. bus.write_byte_data(0x20,0x01,int(robot_id,16))
  133. robot_shot_time = time.time()
  134. elif cmd == "speed":
  135. forward_backward, left_right = int(buf[1]), int(buf[2])
  136. axis1 = -filter_scale(forward_backward)
  137. axis2 = filter_scale(left_right)
  138. m1 = axis1 - axis2
  139. m2 = axis1 + axis2
  140. mx = max(abs(m1), abs(m2))
  141. if mx > 255:
  142. m1, m2 = m1 * 255 / mx , m2 * 255 / mx
  143. d = (0b1000 if m2 < 0 else 0b0100) | (0b10 if m1 < 0 else 0b01)
  144. #print("axis1 %03d axis2 %03d m1 %03d m2 %03d d %s" % ( axis1, axis2, m1, m2, bin(d)))
  145. if d != oldDirection:
  146. oldDirection = d
  147. m.MotorSpeedSetAB(0,0)
  148. m.MotorDirectionSet(d)
  149. m1 = abs(m1)
  150. m2 = abs(m2)
  151. if (m1, m2) != old_speed:
  152. old_speed = m1, m2
  153. m.MotorSpeedSetAB(int(m1),int(m2))