void CalculationThread::check(double &dx, double &dy, double &dh) {
double k = 1 / 10.0;
if (main->h == 1)
dh = 0;
for (int i = 0; i < main->m; i++)
if (main->walls[i][3] == 0) {
if (heightEqualToMe(main->walls[i][2]) || (main->h == 1)) {
checkForWall(dx, dy, main->walls[i][0], main->walls[i][1], main->walls[i][0] + 1, main->walls[i][1]);
checkForWall(dx, dy, main->walls[i][0], main->walls[i][1] - k, main->walls[i][0], main->walls[i][1] + k);
checkForWall(dx, dy, main->walls[i][0] + 1, main->walls[i][1] - k, main->walls[i][0] + 1, main->walls[i][1] + k);
}
checkForDhWall(dh, main->walls[i][0], main->walls[i][1] - k * 1.5, main->walls[i][2], main->walls[i][0] + 1, main->walls[i][1] + k * 1.5);
checkForDhWall(dh, main->walls[i][0], main->walls[i][1] - k * 1.5, main->walls[i][2] + 1, main->walls[i][0] + 1, main->walls[i][1] + k * 1.5);
} else if (main->walls[i][3] == 1) {
if ((heightEqualToMe(main->walls[i][2]) || (main->h == 1))) {
checkForWall(dx, dy, main->walls[i][0], main->walls[i][1], main->walls[i][0], main->walls[i][1] + 1);
checkForWall(dx, dy, main->walls[i][0] - k, main->walls[i][1], main->walls[i][0] + k, main->walls[i][1]);
checkForWall(dx, dy, main->walls[i][0] - k, main->walls[i][1] + 1, main->walls[i][0] + k, main->walls[i][1] + 1);
}
checkForDhWall(dh, main->walls[i][0] - k * 1.5, main->walls[i][1], main->walls[i][2], main->walls[i][0] + k * 1.5, main->walls[i][1] + 1);
checkForDhWall(dh, main->walls[i][0] - k * 1.5, main->walls[i][1], main->walls[i][2] + 1, main->walls[i][0] + k * 1.5, main->walls[i][1] + 1);
} else if (main->walls[i][3] == 2) {
checkForDhWall(dh, main->walls[i][0], main->walls[i][1], main->walls[i][2], main->walls[i][0] + 1, main->walls[i][1] + 1);
if ((main->walls[i][2] - k < main->coord.h) && (main->coord.h < main->walls[i][2] + k)) {
checkForWall(dx, dy, main->walls[i][0], main->walls[i][1], main->walls[i][0] + 1, main->walls[i][1]);
checkForWall(dx, dy, main->walls[i][0], main->walls[i][1], main->walls[i][0], main->walls[i][1] + 1);
checkForWall(dx, dy, main->walls[i][0] + 1, main->walls[i][1], main->walls[i][0] + 1, main->walls[i][1] + 1);
checkForWall(dx, dy, main->walls[i][0], main->walls[i][1] + 1, main->walls[i][0] + 1, main->walls[i][1] + 1);
}
}
}
void doTheDemo() {
cmdDoPlay(">cc");
_DBG_("State 0");
forwards(20);
cmdDoPlay(">dd");
_DBG_("State 1");
while (sensorSide != 1) {
//Forwards until we find a left wall
checkForWall();
_DBD(sensorSide);_DBG_(" sens");
}
cmdDoPlay(">ee");
_DBG_("State 2");
//Follow wall until it's ended
int wallState = -1;
while (wallState != 3 && wallState != 0) {
wallState = checkForWall();
correctForwardMotion();
}
if (courseType == 0) {
cmdDoPlay(">aa");
_DBG_("State 5");
while(!checkForLine()) {
}
followLine();
while(!checkForNoLine()) {
}
brake();
}
else {
//Bear right to head for other wall
cmdDoPlay(">ff");
_DBG_("State 3");
right();
delay(1000);
_DBG_("State 3.1");
forwards(20);
//Wait until right wall is trackable
while (sensorSide != 2) {
checkForWall();
_DBD(sensorSide);_DBG_(" sens");
}
//Track right wall
cmdDoPlay(">gg");
_DBG_("State 4");
wallState = -1;
while (wallState != 4 && wallState != 0) {
wallState = checkForWall();
correctForwardMotion();
}
//Find the line
cmdDoPlay(">aa");
_DBG_("State 5");
forwards(20);
while(!checkForLine()) {
}
followLine();
while(!checkForNoLine()) {
}
brake();
}
}
