private void run() throws Exception {
System.out.println("Creating response");
LocalizationResponse lr = new LocalizationResponse();
System.out.println("Creating request");
DifferentialDriveRequest dr = new DifferentialDriveRequest();
// set up the request to move in a circle
dr.setAngularSpeed(Math.PI * 1.0);
dr.setLinearSpeed(0.8);
System.out.println("Start to move robot");
int rc = robotcomm.putRequest(dr);
System.out.println("Response code " + rc);
for (int i = 0; i < 250; i++) {
try {
Thread.sleep(1000);
} catch (InterruptedException ex) {
}
// ask the robot about its position and angle
robotcomm.getResponse(lr);
// double angle = getBearingAngle(lr);
double angle = lr.getHeadingAngle();
System.out.println("bearing = " + angle * 180 / 3.1415926);
double[] position = getPosition(lr);
System.out.println("position = " + position[0] + ", " + position[1]);
}
// set up request to stop the robot
dr.setLinearSpeed(0);
dr.setAngularSpeed(0);
System.out.println("Stop robot");
rc = robotcomm.putRequest(dr);
System.out.println("Response code " + rc);
}
/**
* Extract the position
*
* @param lr
* @return coordinates
*/
double[] getPosition(LocalizationResponse lr) {
return lr.getPosition();
}
/**
* Extract the robot bearing from the response
*
* @param lr
* @return angle in degrees
*/
double getBearingAngle(LocalizationResponse lr) {
double e[] = lr.getOrientation();
double angle = 2 * Math.atan2(e[3], e[0]);
return angle * 180 / Math.PI;
}
