Files

client.py

@@ -0,0 +1,158 @@
+# On Ubuntu gstreamer interface is missing on OpenCV
+# On Fedora enable RPM fusion repo and install packages
+# dnf install python3-opencv gstreamer-plugins-bad-free gstreamer-plugins-ugly gstreamer1-plugins-bad-freeworld gstreamer1-plugins-good gstreamer1-libav python3-evdev
+
+from time import sleep
+import sys
+import socket
+import cv2
+import numpy as np
+import time
+from threading import Thread
+
+_, hostname = sys.argv
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sock.settimeout(0)
+sock.bind(("", 37021))
+
+#WIDTH, HEIGHT = 1920, 1080
+WIDTH, HEIGHT = 1280, 720
+WIDTH, HEIGHT = 640, 480
+#WIDTH, HEIGHT = 320, 240
+
+CROSSHAIR_THICKNESS = 3
+CROSSHAIR_SIZE = 50
+
+
+
+from glob import glob
+from evdev import InputDevice, categorize, ecodes
+state = dict()
+shoot = False
+robot_shoot_timestamp = 0
+
+
+class GamepadReader(Thread):
+    def run(self):
+        global shoot
+        global robot_shoot_timestamp
+        gamepad = InputDevice(glob("/dev/input/by-id/usb-Microsoft_Controller*event*")[0])
+        print("Opening:", gamepad)
+
+        for event in gamepad.read_loop():
+            if not event:
+                continue
+            if event.type == 3:
+                state["axis%d" % event.code] = event.value
+            elif event.type == 1:
+                state["button%d" % event.code] = event.value
+            elif event.type == 0:
+                pass # dno?
+            else:
+                print(event.type, event.value, event.code)
+
+            if state.get("button310", 0) or state.get("button311", 0):
+                shoot = True
+
+
+
+t = GamepadReader(daemon=True)
+
+t.start()
+
+
+
+print("Resolving hostname:", hostname)
+while True:
+    try:
+        sockaddr = socket.getaddrinfo(hostname, 37020)[0][-1]
+    except OSError:
+        sleep(0.5)
+    else:
+        break
+
+
+print("Sending let's play packet to", sockaddr)
+msg = "play:%d:%d" % (WIDTH, HEIGHT)
+sock.sendto(msg.encode("ascii"), sockaddr)
+
+
+pipeline = 'udpsrc port=5000 caps = "application/x-rtp, encoding-name=(string)H264" ! rtpjitterbuffer mode=3 ! rtph264depay ! decodebin ! videoconvert ! appsink'
+
+print("gst-launch-1.0 -v", pipeline.replace("appsink", "autovideosink"))
+sleep(5)
+cap = cv2.VideoCapture(pipeline)
+
+if not cap.isOpened():
+    print("Didn't receive video feed")
+    quit()
+
+
+red = np.zeros((HEIGHT,WIDTH,3), np.uint8)
+cv2.rectangle(red, (0, 0), (WIDTH, HEIGHT), (0, 0, 255), thickness=-1)
+cv2.namedWindow("window", cv2.WND_PROP_FULLSCREEN)
+cv2.setWindowProperty("window",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
+
+blood = 1.0
+shoot_box_width = 0
+
+
+while True:
+    ret, frame = cap.read()
+    if ret == False:
+        break
+
+    # cv2.putText(frame, "yoyo",
+    #     (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 3)
+
+    cv2.rectangle(frame, (WIDTH//2-CROSSHAIR_THICKNESS//2, HEIGHT//2-CROSSHAIR_SIZE//2), (WIDTH//2+CROSSHAIR_THICKNESS//2, HEIGHT//2+CROSSHAIR_SIZE//2), (0, 255, 0), thickness=-1)
+    cv2.rectangle(frame, (WIDTH//2-CROSSHAIR_SIZE//2, HEIGHT//2-CROSSHAIR_THICKNESS//2), (WIDTH//2+CROSSHAIR_SIZE//2, HEIGHT//2+CROSSHAIR_THICKNESS//2), (0, 255, 0), thickness=-1)
+    cv2.rectangle(frame, (10, HEIGHT-20), (int(WIDTH*0.30), HEIGHT-10),(0, 0, 0),thickness=-1)
+
+    if blood > 0:
+        blood -= 0.02
+        frame = cv2.addWeighted( red, blood, frame, (1-blood), 0.0);
+
+
+
+    msg =     "speed:%d:%d" % (state.get("axis1", 0), state.get("axis3", 0))
+    sock.sendto(msg.encode("ascii"), sockaddr)
+
+    if shoot and time.time() - robot_shoot_timestamp >=1:
+        msg = "shoot"
+        robot_shoot_timestamp = time.time()
+        shoot_box_width = 0
+        #cv2.rectangle(frame, (WIDTH-10, HEIGHT-10), (40, 100),(0, 0, 0),thickness=-1)
+        sock.sendto(msg.encode("ascii"), sockaddr)
+        shoot = False
+    else:
+        shoot = False
+
+    if time.time() - robot_shoot_timestamp <=1:
+        shoot_box_width += int((WIDTH*0.30)/22)
+    else:
+        shoot_box_width = int(WIDTH*0.30)
+    cv2.rectangle(frame, (10, HEIGHT-20), (shoot_box_width, HEIGHT-10),(0, 255, 0),thickness=-1)
+
+
+
+
+    cv2.imshow("window",frame)
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+    try:
+       #print("got:", sock.recv(100))
+        data = sock.recv(100)
+        cmd = data.decode("ascii")
+        #print(cmd)
+        if cmd == "hit":
+            robot_hit_timestamp = time.time()
+            blood = 2.2
+
+
+
+
+    except BlockingIOError:
+        pass
+       #print("nuffing")

config.ini

@@ -0,0 +1,4 @@
+[tank]
+hit_delay = 5
+robot_id = 2
+enemy_bytes = 0x1,0x2,0x3,0x4,0x5,0x6,0x7,0x20df4eb1

robot.py

@@ -0,0 +1,191 @@
+from glob import glob
+import os
+from threading import Thread
+from time import sleep
+from smbus import SMBus
+from time import sleep
+import sys
+import socket
+import time
+from configparser import ConfigParser
+"""
+scp tank.py root@tank-465ce7: && ssh root@tank-465ce7 "python3 tank.py"
+"""
+
+os.chdir("/root/")
+
+config = ConfigParser()
+config.read("config.ini")
+enemy_bytes = config.get('tank','enemy_bytes').split(",")
+robot_id = hex(int(config.get('tank','robot_id'),10))
+hit_delay = int(config.get('tank','hit_delay'),10)
+hit_delay = int(config.get('tank','hit_delay'),10)
+
+#robot_id = socket.gethostname().split("-")[-1]
+robot_shot_time = 0
+robot_hit_timer = 0
+
+bus = SMBus(0)
+hit = False
+
+
+#os.system("ifconfig wlan0 mtu 2304")
+
+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 &"""
+
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
+sock.settimeout(0.2)
+sock.bind(("", 37020))
+print("Receiver started")
+
+#sockaddr = "192.168.2.255", 55555
+#sock.sendto(b"hello", sockaddr)
+
+def filter_scale(axis):
+    tresh = 3000
+    if abs(axis) < tresh:
+        return 0
+    if axis > 0:
+        return (axis-tresh) * 255 / (32768-tresh)
+    else:
+        return (axis+tresh) * 255 / (32768-tresh)
+
+class MotorDriver(object):
+
+        MotorSpeedSet             = 0x82
+        PWMFrequenceSet           = 0x84
+        DirectionSet              = 0xaa
+        MotorSetA                 = 0xa1
+        MotorSetB                 = 0xa5
+        Nothing                   = 0x01
+        EnableStepper             = 0x1a
+        UnenableStepper           = 0x1b
+        Stepernu                  = 0x1c
+        I2CMotorDriverAdd         = 0x0f  #Set the address of the I2CMotorDriver
+
+        def __init__(self, bus, address=0x0f):
+                self.I2CMotorDriverAdd=address
+
+        #Maps speed from 0-100 to 0-255
+        def map_vals(self,value, leftMin, leftMax, rightMin, rightMax):
+                #http://stackoverflow.com/questions/1969240/mapping-a-range-of-values-to-another
+                # Figure out how 'wide' each range is
+                leftSpan = leftMax - leftMin
+                rightSpan = rightMax - rightMin
+
+                # Convert the left range into a 0-1 range (float)
+                valueScaled = float(value - leftMin) / float(leftSpan)
+
+                # Convert the 0-1 range into a value in the right range.
+                return int(rightMin + (valueScaled * rightSpan))
+
+        #Set motor speed
+        def MotorSpeedSetAB(self,MotorSpeedA,MotorSpeedB):
+                #MotorSpeedA=self.map_vals(MotorSpeedA,0,32767,0,255)
+                #MotorSpeedB=self.map_vals(MotorSpeedB,0,32767,0,255)
+                bus.write_i2c_block_data(self.I2CMotorDriverAdd, self.MotorSpeedSet, [MotorSpeedA,MotorSpeedB])
+                sleep(.02)
+
+        #Set motor direction
+        def MotorDirectionSet(self,Direction):
+                bus.write_i2c_block_data(self.I2CMotorDriverAdd, self.DirectionSet, [Direction,0])
+                sleep(.02)
+
+bus = SMBus(0)
+m = MotorDriver(bus)
+oldDirection = None
+old_speed = 0, 0
+addr = None
+
+class irReciver(Thread):
+    def run(self):
+        global hit, addr
+        global robot_hit_timer
+        while True:
+            try:
+                bytes=bus.read_i2c_block_data(0x10,0x02,5)
+                byte = hex(bytes[1] << 24 | bytes[2] << 16 | bytes[3] << 8 | bytes[4])
+                if byte != "0x0":
+                    print(byte)
+                #print(byte)
+                # if code belong to enemy then hit
+                if byte in enemy_bytes and byte !=robot_id and not hit:
+                    print("Cabuum")
+                    hit = True
+                    robot_hit_timer = time.time()
+                    bus.write_byte_data(0x10,0x06,0x02)
+            except Exception as e:
+                print(e)
+            sleep(0.2)
+
+
+
+t = irReciver(daemon=True)
+t.start()
+
+while True:
+    try:
+        data, addr = sock.recvfrom(100)
+    except OSError:
+        m.MotorSpeedSetAB(0,0)
+        continue
+    buf = data.decode("ascii").split(":")
+
+    cmd = buf[0]
+
+    if hit:
+        sock.sendto("hit".encode("ascii"),addr)
+        m.MotorDirectionSet(0b0110)
+        hit = False
+
+    if time.time() - robot_hit_timer < 5:
+        m.MotorDirectionSet(0b0110)
+        m.MotorSpeedSetAB(250,250)
+        continue
+    else:
+        #end led flashing
+        bus.write_byte_data(0x10,0x06,0x01)
+
+
+
+    if cmd == "play":
+        remote, port = addr
+        width, height = int(buf[1]), int(buf[2])
+
+
+        os.system("killall gst-launch-1.0")
+        cam = sorted(glob("/dev/video*"))[-1]
+        cmd = GSTREAM % locals()
+        print("Launching:", cmd)
+        os.system(cmd)
+    elif cmd == "shoot":
+        if time.time() - robot_shot_time > 1:
+            #bus.write_byte_data(0x20,0x01,socket.gethostname().split("-")[-1])
+            bus.write_byte_data(0x20,0x01,int(robot_id,16))
+            robot_shot_time = time.time()
+    elif cmd == "speed":
+        forward_backward, left_right = int(buf[1]), int(buf[2])
+        axis1 = -filter_scale(forward_backward)
+        axis2 = filter_scale(left_right)
+        m1 = axis1 -  axis2
+        m2 = axis1 +  axis2
+
+        mx = max(abs(m1), abs(m2))
+        if mx > 255:
+            m1, m2 = m1 * 255 / mx , m2 * 255 / mx
+
+        d = (0b1000 if m2 < 0 else 0b0100) | (0b10 if m1 < 0 else 0b01)
+        #print("axis1 %03d axis2 %03d m1 %03d m2 %03d d %s" % ( axis1, axis2, m1, m2, bin(d)))
+
+        if d != oldDirection:
+            oldDirection = d
+            m.MotorSpeedSetAB(0,0)
+            m.MotorDirectionSet(d)
+
+        m1 = abs(m1)
+        m2 = abs(m2)
+
+        if (m1, m2) != old_speed:
+            old_speed = m1, m2
+            m.MotorSpeedSetAB(int(m1),int(m2))

tank

@@ -0,0 +1,16 @@
+#!/bin/sh /etc/rc.common
+# Copyright (C) 2015 CZ.NIC z.s.p.o. (http://www.nic.cz/)
+
+START=81
+STOP=0
+
+USE_PROCD=1
+SCRIPT="/root/robot.py"
+
+start_service() {
+	procd_open_instance
+	procd_set_param command python3 "$SCRIPT"
+	#procd_set_param stdout 1
+	#procd_set_param stderr 1
+	procd_close_instance
+}