/**
* Asks the robot for its position
*
* @return The position of the robot
*/
public Point3Dim getRobotPos() {
// Request calibration point
String t = new String("REQ_CAL");
serialCom.writeString(t);
// Wait for response from robot, time out if no response
if (!waitForResponse()) {
return null;
}
String stringData = new String(inData);
/* Is it calibration value? */
if (stringData.startsWith("CAL")) {
String[] data = stringData.split(";");
Double d1 = new Double(data[1]);
Double d2 = new Double(data[2]);
Double d3 = new Double(data[3]);
Point3Dim point = new Point3Dim(d1, d2, d3);
inData = null; // reset indata after having read it
System.out.println("Read robotvalue: " + point.toString());
return point;
} else return null;
}
/**
* Commands the robot to go to a position and read a value.
*
* @param point The position to go to.
* @param zNormal
* @param yNormal
* @param xNormal
*/
public void robotGoTo(Point3Dim point, double angle) {
String t = new String("GOTO_POS;" + point.x + ";" + point.y + ";" + point.z + ";" + angle);
serialCom.writeString(t);
}