From 25bc1ebd4482ca6e15b86af40500a5330025f78b Mon Sep 17 00:00:00 2001 From: Marvin Martinson Date: Mon, 8 Apr 2019 00:58:35 +0300 Subject: [PATCH] Files --- client.py | 158 ++++++++++++++++++++++++++++++++++++++++++++ config.ini | 4 ++ robot.py | 191 +++++++++++++++++++++++++++++++++++++++++++++++++++++ tank | 16 +++++ 4 files changed, 369 insertions(+) create mode 100644 client.py create mode 100644 config.ini create mode 100644 robot.py create mode 100644 tank diff --git a/client.py b/client.py new file mode 100644 index 0000000..0a5e673 --- /dev/null +++ b/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") diff --git a/config.ini b/config.ini new file mode 100644 index 0000000..6014c09 --- /dev/null +++ b/config.ini @@ -0,0 +1,4 @@ +[tank] +hit_delay = 5 +robot_id = 2 +enemy_bytes = 0x1,0x2,0x3,0x4,0x5,0x6,0x7,0x20df4eb1 diff --git a/robot.py b/robot.py new file mode 100644 index 0000000..6108465 --- /dev/null +++ b/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)) diff --git a/tank b/tank new file mode 100644 index 0000000..fd2dfc2 --- /dev/null +++ b/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 +}