Page 297 - The Definitive Guide to Building Java Robots

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

278 CHAPTER 7 ■ NAVIGATION

public IndoorNavigation(JSerialPort serialPort, Room room) throws Exception {
super(serialPort);
currentRoom = room;
}

public void move(String end) throws Exception{
ArrayList path = new ArrayList();
getBestRegion();
NavPoint start = currentRegion.getPointByName(NavPoint.START_POINT);
NavPoint startCenter = currentRegion.getPointByName(NavPoint.CENTER_POINT);
// start vector will be in virtual points 100x100
DistanceVector startVector = getDistanceVector(start,startCenter);
// convert from 100x100 to scaled version
startVector.magnitude = currentRegion.getScaledMagnitude➥
(startVector.magnitude);
path.add(startVector);
// middle vectors
ArrayList regions = currentRoom.getRegions();
Region endRegion = null;
NavPoint endPoint = null;
for (int r=0;r<regions.size();r++) {
endRegion = (Region)regions.get(r);
if (endRegion.getPointByName(end) != null){
endPoint = endRegion.getPointByName(end);
break;
}
}
Dijkstra dijkstra = new Dijkstra();
dijkstra.setVertices(regions);
dijkstra.setEdges(currentRoom.getEdges());
path.addAll(dijkstra.getShortestPath(currentRegion,endRegion));
// end vector
NavPoint endCenterPoint = currentRegion.getPointByName➥
(NavPoint.CENTER_POINT);
DistanceVector endVector = getDistanceVector(endCenterPoint,endPoint);
endVector.magnitude = endRegion.getScaledMagnitude(endVector.magnitude);
path.add(endVector);
DistanceVector[] path2 = (DistanceVector[]) path.toArray();
// conversion will be made to seconds from Navigation
move(path2);
}