We are trying to send information from a custom labview dashboard to the robot (runs java). We have a regular java applet that I can upload if you would like to see it, that works perfectly. When we try to enable the robot we get robots don't quit. Here is our code. Do you see any problems. Thanks.
Code:
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.*;
import java.io.IOException;
import javax.microedition.io.Connector;
import javax.microedition.io.Datagram;
import javax.microedition.io.DatagramConnection;
import javax.microedition.io.UDPDatagramConnection;
public class RobotTemplate extends SimpleRobot {
double targX = 0.0;
double targArea = 0.0;
boolean aHasValue = false;
boolean xHasValue = false;
double distanceFromTarg = 0.0;
static final double distanceConversionFactor = 0;
static final int IMAGE_WIDTH = 320;
int batman = 0;
Datagram data;
DatagramConnection receivePacket;
String rawUdpData;
Jaguar jagL1 = new Jaguar(3);
Jaguar jagL2 = new Jaguar(4);
Jaguar jagR1 = new Jaguar(1);
Jaguar jagR2 = new Jaguar(2);
Joystick stick3 = new Joystick(3);
public void autonomous() {
}
public void operatorControl() {
while (isOperatorControl()) {
udp();
if (stick3.getRawButton(2)) {
tracking();
} else {
jagL1.set(0.0);
jagR1.set(0.0);
jagL2.set(0.0);
jagR2.set(0.0);
}
}
}
public void udp() {
try {
receivePacket = (DatagramConnection)
Connector.open("data://:1130");
// = new DatagramConnection(5021); //put port in here
try{
receivePacket.receive(data);
}finally{
receivePacket.close();
}
//rawUdpData = data.readUTF();
rawUdpData = (String.valueOf(data.getData()));
} catch (IOException ex) {
ex.printStackTrace();
}
// byte[] receiveData = new byte[1024]; //1024 is time out
String udpArea = new String();
String udpX = new String();
//data recieving
//DatagramPacket receivePacket = new DatagramPacket(receiveData, receiveData.length);
// serverSocket.receive(receivePacket);
//String rawUdpData = new String(receivePacket.getData());
// System.out.println("RECEIVED: " + rawUdpData);
//string analizing
if (rawUdpData.startsWith("Area: ")) { // 1 spaces
//System.out.println("worked");
udpArea = rawUdpData.replace('A', ' ');
udpArea = udpArea.replace('r', ' ');
udpArea = udpArea.replace('e', ' ');
udpArea = udpArea.replace('a', ' ');
udpArea = udpArea.replace(':', ' ');
// udpArea = (rawUdpData.replace("Area: ", ""));
udpArea = udpArea.trim();
}
if (rawUdpData.startsWith("Bounding X: ")) { // 1 spaces
udpX = rawUdpData.replace('B', ' ');
udpX = udpX.replace('o', ' ');
udpX = udpX.replace('u', ' ');
udpX = udpX.replace('n', ' ');
udpX = udpX.replace('d', ' ');
udpX = udpX.replace('i', ' ');
udpX = udpX.replace('n', ' ');
udpX = udpX.replace('g', ' ');
udpX = udpX.replace('X', ' ');
udpX = udpX.replace(':', ' ');
// udpX = (rawUdpData.replace("Bounding X: ", "")); //removes lables
udpX = udpX.trim(); //removes whitespace in string
}
//safty for converting string to double
if (udpArea.length() > 0) {
aHasValue = true;
} else {
aHasValue = false;
targArea = 0.0;
}
if (udpX.length() > 0) {
xHasValue = true;
} else {
xHasValue = false;
targX = 0.0;
}
//converts string to double
if (aHasValue) {
targArea = Double.parseDouble(udpArea);
}
if (xHasValue) {
targX = Double.parseDouble(udpX);
}
System.out.println("Area string: " + udpArea);
System.out.println("X string: " + udpX);
System.out.println("a: " + targArea);
System.out.println("X: " + targX);
}
public void tracking() {
if (xHasValue) {
double distOffCenter = ((IMAGE_WIDTH / 2) - targX);
double ratioOffCenter = distOffCenter / (IMAGE_WIDTH / 2);
double speed = (0.3 * (ratioOffCenter * ratioOffCenter) + (distOffCenter > 0 ? 0.4 : -0.4)); //Left Right
if ((Math.abs(speed)) <= 0.41 && batman <= 15) {
batman++;
}
if (Math.abs(speed) <= 0.41) {
jagL1.set(0.0);
jagL2.set(0.0);
jagR1.set(0.0);
jagR2.set(0.0);
} else {
jagL1.set(speed);
jagL2.set(speed);
jagR1.set(speed);
jagR2.set(speed);
}
} else {
System.out.println("NOIMAGE");//Middle of target // myDrive.tankDrive(0, 0);
jagL1.set(0.0);
jagL2.set(0.0);
jagR1.set(0.0);
jagR2.set(0.0);
}
}
}
//end of class