Page 107 - The Definitive Guide to Building Java Robots

Preston_5564C03.fm Page 88 Wednesday, October 5, 2005 7:21 AM

88 CHAPTER 3 ■ MOTION

/*
* gets maximum difference between current positions and new position
*/
int maxDiff = 0;
for (int i = 0; i < commands.size(); i++) {
ServoPosition newPos = (ServoPosition) commands.get(i);
ServoPosition curPos = (ServoPosition) servos.get(newPos.pin);
int tmpDiff = Math.abs(newPos.pos - curPos.pos);
if (tmpDiff > maxDiff) {
maxDiff = tmpDiff;
}
}
// total steps since 3 is min size.
double totalSteps = ((double) maxDiff / 3.0);
// calculate pause time
// total time of move divded by total steps
int pauseTime = (int) ((double) time / totalSteps);
// loop until total difference between all servos
// current position and goal position is zero
while (getTotalDiff() > 0) {
for (int i = 0; i < commands.size(); i++) {
ServoPosition newPos = (ServoPosition) commands.get(i);
ServoPosition curPos = (ServoPosition) servos.get(newPos.pin);
int tmpDiff = Math.abs(newPos.pos - curPos.pos);
if (newPos.pos > curPos.pos) {
if (tmpDiff > 2) {
curPos.pos = curPos.pos + 3;
} else {
curPos.pos = newPos.pos;
}
} else if (newPos.pos < curPos.pos) {
if (tmpDiff > 2) {
curPos.pos = curPos.pos - 3;
} else {
curPos.pos = newPos.pos;
}
}
// move current servo position plus or minus 3
move(curPos.pin, curPos.pos);
Utils.pause(pauseTime);
}
}
// resets commands list.
commands = new ArrayList();
}