Page 274 - The Definitive Guide to Building Java Robots

Preston_5564C07.fm Page 255 Monday, September 26, 2005 5:38 AM

CHAPTER 7 ■ NAVIGATION 255

For this adjustment, let’s say the robot is facing 30 degrees: westPos = 270 – 30 or 240 degrees,
and the southPos = 180 – 30 or 150 degrees. The left sonar moves counterclockwise a little, while the
right sonar moves clockwise a little. The best readings are one west of 48 and one south of 36.
Because the radius of the robot is 12 inches, the x measurement is cos(30) * radius or about
10 inches, while the y measurement is sin(30) or 6 inches. The final coordinates are 48 + 10, 36 + 6,
or 58,42.
In another example, if the robot turns to, say, 225 degrees or southwest, the readings will
be for the north and the east. For this angle, cos(225) and sin(225) = –8, so the readings will be
subtracted by 8, which makes sense because the robot’s center is away from the readings. See
Example 7-14.

Example 7-14. Localization.java

package com.scottpreston.javarobot.chapter7;

import java.awt.Point;

import com.scottpreston.javarobot.chapter2.JSerialPort;
import com.scottpreston.javarobot.chapter2.Utils;
import com.scottpreston.javarobot.chapter2.WebSerialClient;

public class Localization extends Navigation {

private SonarServos sonarServos;

public static final int ROBOT_RADIUS = 12;

public Localization(JSerialPort serialPort) throws Exception {
super(serialPort);
sonarServos = new SonarServos(serialPort);
}

// calculate vector from 2 points.
public static DistanceVector getDistanceVector(Point a, Point b) ➥
throws Exception {

int d;
int dx = a.x - b.x;
int dy = a.y - b.y;
// get distance
double mag = Math.sqrt(dx * dx + dy * dy);
// get angle
if ((dx) == 0) {
d = 90;
} else {
double slope = (double) (dy) / (double) (dx);
d = (int) Math.toDegrees(Math.atan(slope));
}

97022d2480fe4a63cfdfa123a6e70098