Page 271 - The Definitive Guide to Building Java Robots

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

252 CHAPTER 7 ■ NAVIGATION

public static final int RIGHT_AFT = 200;
public static final int RIGHT_FORE = 20;

private int leftPos = 127;
private int rightPos = 127;
private MiniSsc ssc;

public SonarServos(JSerialPort serialPort) throws Exception {
ssc = new MiniSsc(serialPort);
}

public void move(int left, int right) throws Exception {
Utils.pause(250); // wait for servo settle
ssc.move(LEFT_SONAR, left, RIGHT_SONAR, right);
Utils.pause(250); // wait for servo settle
}
// this will be from 180 to 360 of the robot.
public void moveLeft(int angle) throws Exception {
if (angle > 360) {
angle = angle - 360;
}
if (angle < 0) {
angle = angle + 360;
}
double thirdQuad = (LEFT_FORE - LEFT_NEUTRAL); // > 127
double fourthQuad = (LEFT_NEUTRAL - LEFT_AFT); // < 127
int pos = LEFT_NEUTRAL;
if (angle < 270 && angle > 180) {
angle = 270 - angle;
pos = (int) ((angle / 90.0) * thirdQuad) + LEFT_NEUTRAL;
} else if (angle > 270) {
angle = 360 - angle;
pos = LEFT_NEUTRAL - (int) ((angle / 90.0) * fourthQuad);
} else if (angle < 180) {
pos = LEFT_AFT;
}
ssc.move(LEFT_SONAR, pos);
}

// this will be from 0 to 180 of the robot.
public void moveRight(int angle) throws Exception {
if (angle > 360) {
angle = angle - 360;
}