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 static double setDistanceToPoint(Pose actualPose, double destX, double destY) {
destX = -destX;
double d =
Math.sqrt(
(Math.pow((destX - actualPose.getLocation().getX()), 2.0d)
+ (Math.pow((destY - actualPose.getLocation().getY()), 2.0d))));
return d;
}
// ---- obie funkcje przyjmuja wartoœci dodatnie, zarówno dla x jak i y --//
// zwraca wartoœæ o jak¹ ma obróciæ siê robot aby staæ przodem do docelowych wspó³rzênych x,y
public static double setDirection(Pose actualPose, double destX, double destY) {
double newAngle =
(Math.atan2(
actualPose.getLocation().getY() - destY, actualPose.getLocation().getX() + destX));
System.out.println(
-(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle))));
return -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle)));
} // u¿ycie:
/**
* Returns a waypoint of the current location.
*
* @return Waypoint, current location
*/
@Override
public Waypoint getLocation() {
PoseProvider provider = nav.getPoseProvider();
Pose pose = provider.getPose();
return (Waypoint) pose.getLocation();
}
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();
}
}
}
