@Override
public void loop() throws ConnectionLostException {
try {
float raw = mTempInput.read();
float R = 10000 / raw - 10000; // R1 = (R2*Vi/Vo) - R2; Voltage Divider
float steinhart;
steinhart = R / Ro; // (R/Ro)
steinhart = (float) Math.log(steinhart); // ln(R/Ro)
steinhart /= B; // 1/B * ln(R/Ro)
steinhart += 1.0 / (TEMPERATURENOMINAL + 273.15); // + (1/ To)
steinhart = (float) (1.0 / steinhart); // Invert
steinhart -= 273.15; // convert to Cº | celsius = kelvin - 273.15
String tempString = String.format("%.2f", steinhart) + "ºC";
setText(tempString);
Thread.sleep(1000);
} catch (InterruptedException e) {
ioio_.disconnect();
setText(e.getMessage());
} catch (ConnectionLostException e) {
setText(e.getMessage());
throw e;
}
}
public static void searchVictims(MapController robot, BaseRescueBLooper ioio) {
hasVictimLeft = false;
hasVictimRight = false;
hasVictimFront = false;
hasVictimBack = false;
lastIoio = ioio;
try {
Thread.sleep(40);
} catch (InterruptedException e) {
e.printStackTrace();
}
hasVictimLeft |= lastIoio.temp1.victimExist();
hasVictimRight |= lastIoio.temp2.victimExist();
hasVictimFront |= lastIoio.temp3.victimExist();
hasVictimBack |= lastIoio.temp4.victimExist();
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();
}
}
}
}
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();
}
}
}
}