Testing the SimplePacketComms API for the Hephaestus Arm

HephaestusArmTest.groovy

@GrabResolver(name='sonatype', root='https://oss.sonatype.org/content/repositories/releases/')
@Grab(group='com.neuronrobotics', module='SimplePacketComsJava', version='0.1.1')

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.hid4java.HidDevice;
import org.hid4java.HidManager;
import org.hid4java.HidServices;

import edu.wpi.SimplePacketComs.*;

public class HIDSimplePacketComs extends AbstractSimpleComsDevice {
	private int vid = 0;
	private int pid = 0;
	private static HidServices hidServices = null;
	private HidDevice hidDevice = null;

	public HIDSimplePacketComs(int vidIn, int pidIn) {
		// constructor
		vid = vidIn;
		pid = pidIn;
	}

	@Override
	public int read(byte[] message, int howLongToWaitBeforeTimeout) {
		return hidDevice.read(message, howLongToWaitBeforeTimeout);
	}

	@Override
	public int write(byte[] message, int length, int howLongToWaitBeforeTimeout) {
		return hidDevice.write(message, length, (byte) 0);
	}

	@Override
	public boolean disconnectDeviceImp() {
		// TODO Auto-generated method stub
		if (hidDevice != null) {
			hidDevice.close();
		}
		if (hidServices != null) {
			// Clean shutdown
			hidServices.shutdown();
		}
		System.out.println("HID device clean shutdown");
		return false;
	}

	@Override
	public boolean connectDeviceImp() {
		// TODO Auto-generated method stub
		if (hidServices == null)
			hidServices = HidManager.getHidServices();
		// Provide a list of attached devices
		hidDevice = null;
		for (HidDevice h : hidServices.getAttachedHidDevices()) {
			if (h.isVidPidSerial(vid, pid, null)) {
				if (hidDevice == null) {
					hidDevice = h;

					hidDevice.open();
					System.out.println("Found! " + hidDevice);
					return true;
				} else {
					System.out.println("Already opened! this matches too.. " + h);
				}

			}
		}

		return false;
	}

}

public class HephaestusArm extends HIDSimplePacketComs{
	PacketType pollingPacket = new FloatPacketType(37,64);
	PacketType pidPacket = new FloatPacketType(65,64);
	PacketType PDVelPacket = new FloatPacketType(48,64);
	PacketType SetVelocity = new FloatPacketType(42,64);
	public HephaestusArm(int vidIn, int pidIn) {
		super(vidIn, pidIn);
		pidPacket.oneShotMode();
		pidPacket.sendOk();
		PDVelPacket.oneShotMode();		
		PDVelPacket.sendOk();
		SetVelocity.oneShotMode();
		SetVelocity.sendOk();
		for (PacketType pt : Arrays.asList(pollingPacket, pidPacket, PDVelPacket, SetVelocity)) {
			addPollingPacket(pt);
		}
	}
	public void addPollingPacketEvent(Runnable event) {
		addEvent(pollingPacket.idOfCommand, event);
	}
	public void setValuesevent(int index,float position, float velocity, float force){
		pollingPacket.downstream[(index*3)+0] = position;
		pollingPacket.downstream[(index*3)+1] = velocity;
		pollingPacket.downstream[(index*3)+2] = force;
		//println "Setting Downstream "+downstream
	}
	public void setPIDGains(int index,float kp, float ki, float kd){
		
		pidPacket.downstream[(index*3)+0] = kp;
		pidPacket.downstream[(index*3)+1] = ki;
		pidPacket.downstream[(index*3)+2] = kd;
		//println "Setting Downstream "+downstream
	}
	public void pushPIDGains(){
		pidPacket.oneShotMode();
	}
	public void setPDVelGains(int index,float kp, float kd){
		
		PDVelPacket.downstream[(index*2)+0] = kp;
		PDVelPacket.downstream[(index*2)+1] = kd;
		//println "Setting Downstream "+downstream
	}
	public void pushPDVelGains(){
		PDVelPacket.oneShotMode();
	}
	public void setVelocity(int index,float TPS){
		SetVelocity.downstream[index] = TPS;
		//println "Setting Downstream "+downstream
	}
	public void pushVelocity(){
		SetVelocity.oneShotMode();
	}
	public List<Double> getValues(int index){
		List<Double> back= new ArrayList<>();
	
		back.add(pollingPacket.upstream[(index*3)+0].doubleValue()) ;
		back.add( pollingPacket.upstream[(index*3)+1].doubleValue());
		back.add(pollingPacket.upstream[(index*3)+2].doubleValue());
		
		return back;
	}
	public double getPosition(int index) {
		return pollingPacket.upstream[(index*3)+0].doubleValue();
	}
	
	public Number[] getRawValues(){
		return pollingPacket.upstream;
	}
	public void setRawValues(Number[] set){
		for(int i=0;i<set.length&&i<pollingPacket.downstream.length;i++) {
			pollingPacket.downstream[i]=set[i];
		}
	}
}

HephaestusArm arm = new HephaestusArm(0x3742,0x7);
arm.connect();
HephaestusArm slave = new HephaestusArm(0x3743,0x8);
slave.connect();

try{
while(!Thread.interrupted()){
	Thread.sleep(10);
	slave.setRawValues(arm.getRawValues())
}
}catch(Exception e){}

slave.disconnect();
arm.disconnect();