/* * 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; }