Browse Source

Files

master
Marvin Martinson 1 year ago
commit
25bc1ebd44
4 changed files with 369 additions and 0 deletions
  1. +158
    -0
      client.py
  2. +4
    -0
      config.ini
  3. +191
    -0
      robot.py
  4. +16
    -0
      tank

+ 158
- 0
client.py View File

@@ -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")

+ 4
- 0
config.ini View File

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

+ 191
- 0
robot.py View File

@@ -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))

+ 16
- 0
tank View File

@@ -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
}

Loading…
Cancel
Save