public static Map scanMap(RobotHead head) {
Map _map = new Map(3, 3, 1, 1);
// double prob_dist1 = 0.8;
// double prob_dist2 = 0.6;
double reads[][] =
head.performReads(new int[] {angle1 * -1, angle2 * -1, 0, angle2, angle1}, 500);
double read[] = reads[2];
if (!_map.getWall(Heading.North).exist(Util.range(wallMinFirst, wallMaxFirst, read[0]))) {
_map.getWall(0, 1, Heading.North).exist(Util.range(wallMinSecond, wallMaxSecond, read[0]));
if (!_map.getWall(0, 1, Heading.West)
.exist(Util.range(wallMinThird, wallMaxThird, reads[1][0])))
_map.getWall(-1, 1, Heading.North).exist(Util.range(wallMinFour, wallMaxFour, reads[0][0]));
if (!_map.getWall(0, 1, Heading.East)
.exist(Util.range(wallMinThird, wallMaxThird, reads[3][0])))
_map.getWall(1, 1, Heading.North).exist(Util.range(wallMinFour, wallMaxFour, reads[4][0]));
}
if (!_map.getWall(Heading.East).exist(Util.range(wallMinFirst, wallMaxFirst, read[1]))) {
_map.getWall(1, 0, Heading.East).exist(Util.range(wallMinSecond, wallMaxSecond, read[1]));
if (!_map.getWall(1, 0, Heading.North)
.exist(Util.range(wallMinThird, wallMaxThird, reads[1][1])))
_map.getWall(1, 1, Heading.East).exist(Util.range(wallMinFour, wallMaxFour, reads[0][1]));
if (!_map.getWall(1, 0, Heading.South)
.exist(Util.range(wallMinThird, wallMaxThird, reads[3][1])))
_map.getWall(1, -1, Heading.East).exist(Util.range(wallMinFour, wallMaxFour, reads[4][1]));
}
if (!_map.getWall(Heading.South).exist(Util.range(wallMinFirst, wallMaxFirst, read[2]))) {
_map.getWall(0, -1, Heading.South).exist(Util.range(wallMinSecond, wallMaxSecond, read[2]));
if (!_map.getWall(0, -1, Heading.East)
.exist(Util.range(wallMinThird, wallMaxThird, reads[1][2])))
_map.getWall(1, -1, Heading.South).exist(Util.range(wallMinFour, wallMaxFour, reads[0][2]));
if (!_map.getWall(0, -1, Heading.West)
.exist(Util.range(wallMinThird, wallMaxThird, reads[3][2])))
_map.getWall(-1, -1, Heading.South)
.exist(Util.range(wallMinFour, wallMaxFour, reads[4][2]));
}
if (!_map.getWall(Heading.West).exist(Util.range(wallMinFirst, wallMaxFirst, read[3]))) {
_map.getWall(-1, 0, Heading.West).exist(Util.range(wallMinSecond, wallMaxSecond, read[3]));
if (!_map.getWall(-1, 0, Heading.South)
.exist(Util.range(wallMinThird, wallMaxThird, reads[1][3])))
_map.getWall(-1, -1, Heading.West).exist(Util.range(wallMinFour, wallMaxFour, reads[0][3]));
if (!_map.getWall(-1, 0, Heading.North)
.exist(Util.range(wallMinThird, wallMaxThird, reads[3][3])))
_map.getWall(-1, 1, Heading.West).exist(Util.range(wallMinFour, wallMaxFour, reads[4][3]));
}
return _map;
}
public static void updateMapController(MapController robot, BaseRescueBLooper ioio) {
RobotHead head = ioio.Head;
hasVictimRight = false;
hasVictimLeft = false;
hasVictimFront = false;
hasVictimBack = false;
lastIoio = ioio;
double reads[][] =
head.performReads(
new int[] {angle1 * -1, angle2 * -1, 0, angle2, angle1},
50,
new Runnable() {
int pos = 0;
public void run() {
if (pos == 2) {
try {
Thread.sleep(60);
} catch (InterruptedException e) {
e.printStackTrace();
}
hasVictimLeft |= lastIoio.temp1.victimExist();
hasVictimRight |= lastIoio.temp2.victimExist();
hasVictimFront |= lastIoio.temp3.victimExist();
hasVictimBack |= lastIoio.temp4.victimExist();
// hasVictim |= lastIoio.temp3.victimExist();
}
pos++;
}
});
double read[] = reads[2];
Wall w = robot.getRelativeWall(Heading.North);
w.updateWall((Util.range(wallMinFirst, wallMaxFirst, read[0]) ? prob_dist1 : 1 - prob_dist1));
if (!w.exist()) {
w = robot.getRelativeWall(0, 1, Heading.North);
w.updateWall(
(Util.range(wallMinSecond, wallMaxSecond, read[0]) ? prob_dist2 : 1 - prob_dist2));
w = robot.getRelativeWall(0, 1, Heading.West);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[1][0]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(-1, 1, Heading.North);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[0][0]) ? prob_dist4 : 1 - prob_dist4));
}
w = robot.getRelativeWall(0, 1, Heading.East);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[3][0]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(1, 1, Heading.North);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[4][0]) ? prob_dist4 : 1 - prob_dist4));
}
}
w = robot.getRelativeWall(Heading.East);
w.updateWall((Util.range(wallMinFirst, wallMaxFirst, read[1]) ? prob_dist1 : 1 - prob_dist1));
if (!w.exist()) {
w = robot.getRelativeWall(1, 0, Heading.East);
w.updateWall(
(Util.range(wallMinSecond, wallMaxSecond, read[1]) ? prob_dist2 : 1 - prob_dist2));
w = robot.getRelativeWall(1, 0, Heading.North);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[1][1]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(1, 1, Heading.East);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[0][1]) ? prob_dist4 : 1 - prob_dist4));
}
w = robot.getRelativeWall(1, 0, Heading.South);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[3][1]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(1, -1, Heading.East);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[4][1]) ? prob_dist4 : 1 - prob_dist4));
}
}
w = robot.getRelativeWall(Heading.South);
w.updateWall((Util.range(wallMinFirst, wallMaxFirst, read[2]) ? prob_dist1 : 1 - prob_dist1));
if (!w.exist()) {
w = robot.getRelativeWall(0, -1, Heading.South);
w.updateWall(
(Util.range(wallMinSecond, wallMaxSecond, read[2]) ? prob_dist2 : 1 - prob_dist2));
w = robot.getRelativeWall(0, -1, Heading.East);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[1][2]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(1, -1, Heading.South);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[0][2]) ? prob_dist4 : 1 - prob_dist4));
}
w = robot.getRelativeWall(0, -1, Heading.West);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[3][2]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(-1, -1, Heading.South);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[4][2]) ? prob_dist4 : 1 - prob_dist4));
}
}
w = robot.getRelativeWall(Heading.West);
w.updateWall((Util.range(wallMinFirst, wallMaxFirst, read[3]) ? prob_dist1 : 1 - prob_dist1));
if (!w.exist()) {
w = robot.getRelativeWall(-1, 0, Heading.West);
w.updateWall(
(Util.range(wallMinSecond, wallMaxSecond, read[3]) ? prob_dist2 : 1 - prob_dist2));
w = robot.getRelativeWall(-1, 0, Heading.South);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[1][3]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(-1, -1, Heading.West);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[0][3]) ? prob_dist4 : 1 - prob_dist4));
}
w = robot.getRelativeWall(-1, 0, Heading.North);
w.updateWall(
(Util.range(wallMinThird, wallMaxThird, reads[3][3]) ? prob_dist3 : 1 - prob_dist3));
if (!w.exist()) {
w = robot.getRelativeWall(-1, 1, Heading.West);
w.updateWall(
(Util.range(wallMinFour, wallMaxFour, reads[4][3]) ? prob_dist4 : 1 - prob_dist4));
}
}
boolean hasVictim = false;
if (hasVictimLeft) {
if (robot.getRelativeWall(Heading.West).exist()) {
hasVictim = true;
}
}
if (hasVictimRight) {
if (robot.getRelativeWall(Heading.East).exist()) {
hasVictim = true;
}
}
if (hasVictimFront) {
if (robot.getRelativeWall(Heading.North).exist()) {
hasVictim = true;
}
}
if (hasVictimBack) {
if (robot.getRelativeWall(Heading.South).exist()) {
hasVictim = true;
}
}
if (hasVictim && !robot.getThisCell().hasVictim) {
robot.getThisCell().hasVictim = true;
for (int i = 0; i < 7; i++) {
try {
ioio.oLedVictim.write((i % 2) == 0);
Thread.sleep(200);
} catch (ConnectionLostException e) {
e.printStackTrace();
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
}