Test the simple packet coms Java API

SImplePacketComsJavaTest.groovy

@Grab(group='com.neuronrobotics', module='SimplePacketComsJava', version='0.0.4')

import edu.wpi.SimplePacketComs.bytepacket.BytePacketType;
import edu.wpi.SimplePacketComs.floatpacket.FloatPacketType;
import edu.wpi.SimplePacketComs.*;
import edu.wpi.SimplePacketComs.phy.UDPSimplePacketComs;
import java.net.InetAddress;
import java.util.HashSet;

class SwarmRobot extends NonBowlerDevice{
	UDPSimplePacketComs udpdevice;
	def packets = [new FloatPacketType(1871, 64),
				new FloatPacketType(1936, 64),
				new BytePacketType(2012, 64)]
	SwarmRobot(InetAddress add){

		udpdevice = new UDPSimplePacketComs(add);
		for(PacketType pt:packets)
			udpdevice.addPollingPacket(pt);
		/*
		udpdevice.addEvent(1871, {
			
			//System.out.print("\r\n\r\n1871 Packet updated: ");
			for (int i = 0; i < packetOne.upstream.length; i++) {
				//System.out.println(" " + packetOne.upstream[i] + " ");
	
			}
		});
		udpdevice.addEvent(1936, {
			//System.out.print("\r\n\r\n1936 Packet updated: ");
			for (int i = 0; i < packetTwo.upstream.length; i++) {
				//System.out.println(" " + packetTwo.upstream[i] + " ");
	
			}
		});
		udpdevice.addEvent(2012, {
			//System.out.print("\r\n\r\n2012 Packet updated: ");
			for (int i = 0; i < packetThree.upstream.length; i++) {
				//System.out.println(" " + packetThree.upstream[i] + " ");
	
			}
		});
		*/
		
	}
	ArrayList<Integer> getIDs(){
		return packets.collect{it.idOfCommand}
	}
	void writeFloats(int id,double [] values){
		udpdevice.writeFloats( id,  values)
	}
	void writeBytes(int id,byte [] values){
		udpdevice.writeBytes( id,  values)
	}
	void readFloats(int id,double [] values){
		udpdevice.readFloats( id,  values)
	}
	void readBytes(int id,byte [] values){
		udpdevice.readBytes( id,  values)
	}
	
	/**
	 * This method tells the connection object to disconnect its pipes and close out the connection. Once this is called, it is safe to remove your device.
	 */
	
	public void disconnectDeviceImp(){
		udpdevice.disconnect();
	}
	
	/**
	 * Connect device imp.
	 *
	 * @return true, if successful
	 */
	public  boolean connectDeviceImp(){
		udpdevice.connect();
	}
	
	/**
	 * Gets the namespaces imp.
	 *
	 * @return the namespaces imp
	 */
	public  ArrayList<String>  getNamespacesImp(){return []}
}

HashSet<InetAddress> addresses = UDPSimplePacketComs.getAllAddresses();
if (addresses.size() < 1) {
	println("No devices found");
	return
}
def robots = []
for (InetAddress add : addresses) {
	System.out.println("Got " + add);
	def robot =DeviceManager.getSpecificDevice( add.toString(),{
		//If the device does not exist, prompt for the connection
		def e = new SwarmRobot(add);
		e.connect()
		return e
	})	
	robots.add(robot)	
}

ThreadUtil.wait(1000)
def robot = robots.get(0)
double[] data = [0.0]*15
robot.readFloats(1871,data)
println data
robot.readFloats(1936,data)
robot.writeFloats(1936,data)
println data
byte[] bytes = [0]*60
robot.readBytes(2012,bytes)
robot.writeBytes(2012,bytes)
println bytes