Hrvatsko društvo za robotiku - Uvod u robotiku - vježbe

void RobotMaze::moveAhead() {
	bool encodersOver = false; // One tile's length covered. 
		Used only when encoders available.
	// When encoders available, timeout is only a safety measure, 
		to avoid possible endless loop. When not, it marks end of tile.
	static uint32_t startedAtMs;
	if (setup())
		startedAtMs = millis();
	bool timeOver = millis() - startedAtMs > MOVE_AHEAD_TIMEOUT_MS;

	...
}

void RobotMaze::moveAhead() {
	...

	// If any of the 3 conditions satisfied, break the movement: encoder, timeout, 
		or a wall to close ahead.
	if (encodersOver || timeOver || 
			distance(directionCurrent, false) < WALL_FOLLOW_DISTANCE || 
			distance(directionCurrent, true) < WALL_FOLLOW_DISTANCE) {
		if (testMode)
			motorGroup->go(0, 0),end();
		else{
			...
		}
	}
	...
}

void RobotMaze::moveAhead() {
	...
	if (encodersOver || timeOver || 
			distance(directionCurrent, false) < WALL_FOLLOW_DISTANCE 
			|| distance(directionCurrent, true) < WALL_FOLLOW_DISTANCE) {
		if (testMode)
			motorGroup->go(0, 0),end();
		else{
			// The robot's position changed so we need to 
				calculate new coordinates.
			int8_t newX = tileCurrent->x;
			int8_t newY = tileCurrent->y;
			if (directionCurrent == UP)
				newY++;
			else if (directionCurrent == LEFT)
				newX--;
			else if (directionCurrent == DOWN)
				newY--;
			else
				newX++;
			print("Coord: (%i, %i) ", newX, newY); // Debug.

			Tile* targetTile = tileContaining(newX, newY); //Search chain for 
				a tile already containing (x, y) - new robot's coordinates.
			...
		}
	}
	...
}

void RobotMaze::moveAhead() {
	...
	if (encodersOver || timeOver || 
			distance(directionCurrent, false) < WALL_FOLLOW_DISTANCE || 
			distance(directionCurrent, true) < WALL_FOLLOW_DISTANCE) {
		if (testMode)
			...
		else{
			...
			if (targetTile != NULL) { // A tile 
					found. That means we returned to the already 
					visited tile.
				print("tile exists\n\r"); // Debug.
				tileCurrent = targetTile; // Set the position to 
					the found tile.
				actionSet(actionDecide); // Movement over. The next 
					action will be decision what to do next. 
					No mapping is needed here as the 
					tile has already been mapped.
			}
			else { // No such a tile. Therefore, this coordinate has 
					not been visited yet. 
					We have to create a new Tile object.
				print("new tile\n\r");
				// Set the position to a newly created tile, with 
					new (x, y) coordinates. Breadcrumb direction 
					will be the opposite direction to the current 
					one.
				tileCurrent = new Tile(newX, newY, 
					(Direction)((directionCurrent + 2) % 4));
				actionSet(actionMap); // The next action will be tile
					mapping as this is a new tile and its walls 
					are not mapped yet.
			}
		}
	}
	...
}

void RobotMaze::moveAhead() {
	
	if (encodersOver || timeOver || 
		distance(directionCurrent, false) < WALL_FOLLOW_DISTANCE || 
		distance(directionCurrent, true) < WALL_FOLLOW_DISTANCE) {
		...
	}
	else { // End of tile not reached yet. Note that this part will 
			execute many times during one-tile trip.
		Direction closestWall = wallClosest(); // Find the wall 
			most appropriate to align with.
		if (closestWall == NOWHERE) // No alignable wall, the 
			movement can be corrected only by compass (IMU).
			imuFollow();
		else // Yes, a wall found in one direction. Follow it.
			wallFollow(closestWall);
	}
}

void RobotMaze::moveAhead1TileTest(){
	testMode = true; 
	directionCurrent = Direction::UP;
	actionSet(actionMoveAhead);
}