void Labyrinth::goSouth() {
if (hasSouth()) {
m_precedent = m_current;
m_current.y++;
newPosition();
}
}
bool hasFrontWallInMem(void) {
int curBlock = block[yPos][xPos];
if ( (orientation == 'N' && hasNorth(curBlock)) || (orientation == 'E' && hasEast(curBlock)) ||
(orientation == 'S' && hasSouth(curBlock)) || (orientation == 'W' && hasWest(curBlock)) )
return 1;
else
return 0;
}
//Returns which direction to move in and sets the orientation to next move
int getNextDirection(void)
{
int currDistance = distance[yPos][xPos];
int distN = MAX_DIST;
int distE = MAX_DIST;
int distS = MAX_DIST;
int distW = MAX_DIST;
if(yPos < SIZE - 1)
distN = distance[yPos+1][xPos];
if(xPos < SIZE - 1)
distE = distance[yPos][xPos+1];
if(xPos > 0)
distS = distance[yPos-1][xPos];
if(yPos > 0)
distW = distance[yPos][xPos-1];
if(!hasNorth(block[yPos][xPos]) && (distN == currDistance - 1))
{
orientation = 'N';
return MOVEN;
}
if(!hasEast(block[yPos][xPos]) && (distE == currDistance - 1))
{
orientation = 'E';
return MOVEE;
}
if(!hasSouth(block[yPos][xPos]) && (distS == currDistance - 1))
{
orientation = 'S';
return MOVES;
}
if(!hasWest(block[yPos][xPos]) && (distW == currDistance - 1))
{
orientation = 'W';
return MOVEW;
}
return 0;
}
void speedRun(void)
{
resetSpeedProfile();
useIRSensors = 1;
useSpeedProfile = 1;
int nextDir[100] = {0};
int length = 0;
xPos = 0;
yPos = 0;
orientation = 'N';
// Close off untraced routes
closeUntracedCells();
updateDistance();
visualizeGrid();
// Simulate path
for (int i = 0; !atCenter(); i++) {
if (orientation == 'N') {
while (!hasNorth(block[yPos][xPos]) && (distance[yPos + 1][xPos] == distance[yPos][xPos] - 1)) {
length++;
yPos++;
}
}
else if (orientation == 'E') {
while (!hasEast(block[yPos][xPos]) && (distance[yPos][xPos + 1] == distance[yPos][xPos] - 1)) {
length++;
xPos++;
}
}
else if (orientation == 'S') {
while (!hasSouth(block[yPos][xPos]) && (distance[yPos - 1][xPos] == distance[yPos][xPos] - 1)) {
length++;
yPos--;
}
}
else if (orientation == 'W') {
while (!hasWest(block[yPos][xPos]) && (distance[yPos][xPos - 1] == distance[yPos][xPos] - 1)) {
length++;
xPos--;
}
}
distances[i] = length;
nextDir[i] = getNextDirection();
length = 0;
}
/* Print values
for (int i = 0; distances[i]; i++)
printf("distances[%d] = %d | nextDir[%d] = %d\n\r", i, distances[i], i, nextDir[i]);
*/
orientation = 'N';
// Run path
for (int i = 0; distances[i] != 0; i++) {
moveForward(distances[i]);
if (nextDir[i] == MOVEN) {
moveN();
}
else if (nextDir[i] == MOVEE) {
moveE();
}
else if (nextDir[i] == MOVES) {
moveS();
}
else if (nextDir[i] == MOVEW) {
moveW();
}
}
useSpeedProfile = 0;
turnMotorOff;
}
