private boolean hitTheWall(Pose pose) { float poseAngle = (float) Math.toRadians(Const.angleToByte(pose.getHeading()) * 90); byte sin = (byte) Math.round(Math.sin(poseAngle)), cos = (byte) Math.round(Math.cos(poseAngle)); byte mapPoseX = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getX()), "X") + cos), mapPoseY = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getY()), "Y") + sin); // return true and update pose if it hit an obstacle if (mapPoseX < 0 || mapPoseY < 0 || mapPoseX > 8 || mapPoseY > 5 || wayClass.getOccupancyMap(mapPoseX, mapPoseY) > 0) { Rover.poseProvider.setPose( new Pose( cos != 0 ? (mapPoseX * Const.BLOCK_WIDTH + (cos > 0 ? -10 : 10 + Const.BLOCK_WIDTH)) : pose.getX(), sin != 0 ? (mapPoseY * Const.BLOCK_WIDTH + (sin > 0 ? -10 : 10 + Const.BLOCK_WIDTH)) : pose.getY(), cacheWayAngle)); return true; } return false; }
public void run() { while (true) { if (_suppress) { Thread.yield(); continue; } Pose oldPose = cachePose; cachePose = Rover.poseProvider.getPose(); // get angle and distance where to travel int[] way = wayClass.getPredefinedWay(oldPose, cachePose); cacheWayAngle = way[1]; cacheWayDistance = way[0] * Const.BLOCK_WIDTH; float angle = Const.normalizeAngle(cacheWayAngle - cachePose.getHeading()); if (Const.angleToByte(angle) == 2 && cacheWayDistance == Const.BLOCK_WIDTH) { // if the robot is supposed to rotate 180 deg and travel one block, back out Rover.pilot.travel(-cacheWayDistance, true); } else { // otherwise rotate and travel the distance if (Math.abs(angle) > 2) { // float cacheGyro = Rover.getGyro(); boolean touched = false; Rover.pilot.rotate(angle, true); while (Rover.pilot.isMoving()) { boolean[] touch = Rover.getTouch(); if (touch[0] || touch[1]) touched = true; Thread.yield(); } Pose newPose = Rover.poseProvider.getPose(); /* float angleGyro = Rover.getGyro() - cacheGyro; // if the turn is incomplete, rotate by the difference // TODO maybe turn only when the difference is bigger if (angle > angleGyro + 5 || angle < angleGyro - 5) { Rover.pilot.travel(touched ? -5 : 5); cacheWayDistance += touched ? 5 : -5; Rover.pilot.rotate(angle - angleGyro); newPose = Rover.poseProvider.getPose(); } */ // update pose with normalize heading Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle)); } float blockAngle = (float) Math.toRadians(cacheWayAngle); byte sin = (byte) Math.round(Math.sin(blockAngle)), cos = (byte) Math.round(Math.cos(blockAngle)); byte blockX = (byte) (Const.roundToMapArea(Const.middlePointToBlock(cachePose.getX()), "X") + cos * (way[0] + 1)), blockY = (byte) (Const.roundToMapArea(Const.middlePointToBlock(cachePose.getY()), "Y") + sin * (way[0] + 1)); // extend the distance to hit a wall if (blockX < 0 || blockY < 0 || blockX > 8 || blockY > 5 || wayClass.getOccupancyMap(blockX, blockY) > 0) { cacheWayDistance += Const.BLOCK_WIDTH; // TODO no way of updating pose if it doesn't hit wall } Rover.pilot.travel(cacheWayDistance, true); } // while the rover is traveling while (Rover.pilot.isMoving()) { boolean[] touch = Rover.getTouch(); if (touch[0] || touch[1]) { Rover.pilot.quickStop(); float sonic = Rover.getSonic(); Pose hitPose = Rover.poseProvider.getPose(); if (hitTheWall(hitPose)) { Rover.pilot.travel(-4); } else if (sonic > 15 && !(touch[0] && touch[1])) { // if the rover touches the obstacle by just one sensor // and distance is more than 15 cm float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled(); Rover.pilot.travel(-4); Rover.pilot.rotate(touch[0] ? -20 : 20); Pose newPose = Rover.poseProvider.getPose(); Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle)); cacheWayDistance -= distanceTraveled - 4; Rover.pilot.travel(cacheWayDistance, true); } else { // if the obstacle touched by both sensors // or the distance is less than 15 cm Pose p = Rover.poseProvider.getPose(); byte blockX = Const.roundToMapArea(Const.middlePointToBlock(p.getX()), "X"), blockY = Const.roundToMapArea(Const.middlePointToBlock(p.getY()), "Y"); float blockAngle = (float) Math.toRadians(Const.angleToByte(p.getHeading()) * 90); byte sin = (byte) Math.round(Math.sin(blockAngle)), cos = (byte) Math.round(Math.cos(blockAngle)); boolean isFree; try { isFree = wayClass.getOccupancyMap((byte) (blockX + cos), (byte) (blockY + sin)) < 0; } catch (ArrayIndexOutOfBoundsException e) { isFree = false; } if (!(touch[0] && touch[1]) && isFree) { Rover.pilot.rotate(touch[0] ? -90 : 90); Rover.pilot.travel(15); Rover.pilot.rotate(touch[0] ? 90 : -90); Rover.poseProvider.setPose( new Pose( (blockX + 1 / 2) * Const.BLOCK_WIDTH, (blockY + 1 / 2) * Const.BLOCK_WIDTH, Const.angleToByte(p.getHeading()) * 90)); float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled(); cacheWayDistance -= distanceTraveled; Rover.pilot.travel(cacheWayDistance, true); } else { System.out.println("ERROR: TRAVEL CORRECTION"); // This shouldn't happen any more Button.LEDPattern(5); } } } Thread.yield(); } } }