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; }