/*
* Make a new step
*/
direction pathfinder_NextStep(direction currentDirection, uint8_t x, uint8_t y)
{
// first check if we have reached the target already
if ((x == targetX) && (y == targetY)) {
// we ain't going anywhere soon!
return NONE;
}
setCursorPosLCD(0, 2);
writeIntegerLCD(root->x, DEC);
setCursorPosLCD(0, 5);
writeIntegerLCD(root->y, DEC);
setCursorPosLCD(1, 0);
writeIntegerLCD(current->count, DEC);
setCursorPosLCD(1, 4);
writeIntegerLCD(nodeCount, DEC);
// now check if we can only go one direction
if (hal_hasWallLeft() && hal_hasWallRight() && !hal_hasWallFront()) {
return currentDirection;
} else if (hal_hasWallLeft() && !hal_hasWallRight() && hal_hasWallFront()) {
return turnRight(currentDirection);
} else if (!hal_hasWallLeft() && hal_hasWallRight() && hal_hasWallFront()) {
return turnLeft(currentDirection);
}
// we cannot keep cruising normally
// get the node or create a new one
struct node *node = getNode(current);
if (!node) {
node = createNode(current, currentDirection, x, y);
}
if (hal_hasWallLeft() && hal_hasWallRight() && hal_hasWallFront()) {
// dead end
current = node;
currentDirection = turnAround(currentDirection);
} else {
// ok, decide where we're gonna go
chooseDirection(currentDirection, node, x, y);
current = node;
}
lastDirection = currentDirection;
return currentDirection;
}
/*
* Create us a new node
*/
static struct node *createNode(struct node *current, direction currentDirection, uint8_t x, uint8_t y)
{
struct node *node;
node = malloc(sizeof(node));
node->count = ++nodeCount;
node->x = x;
node->y = y;
node->north = NULL;
node->west = NULL;
node->south = NULL;
node->east = NULL;
// link the available directions to an empty node
if (!hal_hasWallLeft()) {
setDirection(node, empty, turnLeft(currentDirection));
}
if (!hal_hasWallRight()) {
setDirection(node, empty, turnRight(currentDirection));
}
if (!hal_hasWallFront()) {
setDirection(node, empty, currentDirection);
}
// connect the current node to the new node
setDirection(node, current, currentDirection);
setDirection(current, node, lastDirection);
return node;
}
/*
* Initialize the pathfinder
*/
void pathfinder_init(uint8_t x, uint8_t y, direction currentDirection)
{
lastDirection = currentDirection;
clearLCD();
setCursorPosLCD(0, 2);
writeIntegerLCD(x, DEC);
setCursorPosLCD(0, 5);
writeIntegerLCD(y, DEC);
mSleep(5000);
// initialize root node
root = malloc(sizeof(root));
root->count = nodeCount;
root->x = x;
root->y = y;
root->north = NULL;
root->west = NULL;
root->south = NULL;
root->east = NULL;
// make the current node the root node
current = root;
// also, initialize the empty node
empty = malloc(sizeof(empty));
empty->count = 255;
empty->x = 255;
empty->y = 255;
empty->north = NULL;
empty->west = NULL;
empty->south = NULL;
empty->east = NULL;
// initialize the current node
if (!hal_hasWallLeft()) {
current->west = empty;
}
if (!hal_hasWallFront()) {
current->north = empty;
}
if (!hal_hasWallRight()) {
current->east = empty;
}
}
void ir_sendSituation(direction dir, uint8_t x, uint8_t y)
{
// hal_hasWallLeft() hal_hasWallRight() hal_hasWallFront()
receive_buffer[0] = x;
receive_buffer[1] = y+16;
// direction
if (dir == NORTH){
receive_buffer[2] = 32;
} else if (dir == EAST) {
receive_buffer[2] = 33;
} else if (dir == SOUTH) {
receive_buffer[2] = 34;
} else if (dir == WEST) {
receive_buffer[2] = 35;
}
// pro-tip: het maakt geen zak uit hoe veel lijnen een programma is
// wall
receive_buffer[3] = 40 + (hal_hasWallLeft() << 2) + (hal_hasWallFront() << 1) + hal_hasWallRight();
#if 0
L F R
0 0 0 0
0 0 1 1
0 1 0 2
0 1 1 3
1 0 0 4
1 0 1 5
1 1 0 6
1 1 1 7
#endif
SPI_EEPROM_writeByte(schrijf,receive_buffer[0]);
mSleep(10);
SPI_EEPROM_writeByte(schrijf+1,receive_buffer[1]);
mSleep(10);
SPI_EEPROM_writeByte(schrijf+2,receive_buffer[2]);
mSleep(10);
SPI_EEPROM_writeByte(schrijf+3,receive_buffer[3]);
mSleep(10);
schrijf = (schrijf + 8);
}
static void getAvailableDirections(direction currentDirection)
{
dirs[0] = NONE;
dirs[1] = NONE;
uint8_t num = 0;
if (!hal_hasWallRight()) {
dirs[num] = turnRight(currentDirection);
num++;
}
if (!hal_hasWallFront()) {
dirs[num] = currentDirection;
num++;
}
if (!hal_hasWallLeft()) {
dirs[num] = turnLeft(currentDirection);
}
return dirs;
}
