Page 272 - The Definitive Guide to Building Java Robots

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

CHAPTER 7 ■ NAVIGATION 253

if (angle < 0) {
angle = angle + 360;
}
double firstQuad = (RIGHT_NEUTRAL - RIGHT_FORE); // < 127
double secondQuad = (RIGHT_AFT - RIGHT_NEUTRAL); // > 127
int pos = RIGHT_NEUTRAL;
if (angle < 90) {
pos = RIGHT_NEUTRAL - (int) ((angle / 90.0) * firstQuad);
} else if (angle > 90 && angle > 180) {
angle = 180 - angle;
pos = (int) ((angle / 90.0) * secondQuad) + RIGHT_NEUTRAL;
} else if (angle > 180) {
pos = RIGHT_AFT;
}
ssc.move(RIGHT_SONAR, pos);
}

public void lookSide() throws Exception {
move(LEFT_NEUTRAL, RIGHT_NEUTRAL);
}

public void lookFore() throws Exception {
move(LEFT_FORE, RIGHT_FORE);
}

public void lookAft() throws Exception {
move(LEFT_AFT, RIGHT_AFT);
}

public static void main(String[] args) throws Exception {
try {
WebSerialClient com = new WebSerialClient("10.10.10.99", "8080", "1");
SonarServos ss = new SonarServos(com);
ss.lookFore();
Utils.pause(1000);
ss.lookAft();
Utils.pause(1000);
ss.lookSide();
// get 360 readings from sonar
for (int a = 0; a < 360; a = a + 10) {
ss.moveLeft(a);
ss.moveRight(a);
Utils.pause(1000);
}
com.close();