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

Direction RobotMaze:: wallClosest() {
	Direction closest = NOWHERE; // If no wall found, this will be the result.
	uint16_t leastDistance = 0xFFFF; // Set variable to a huge number so that any distance reading will override it.
	for (uint8_t i = 0; i < 4; i++) { // Iterate all the 4 directions.
		Direction dir = (Direction)i; // Cast integer to Direction.
		...
	}
	...
}

Direction RobotMaze:: wallClosest() {
	...
	for (uint8_t i = 0; i < 4; i++) { // Iterate all the 4 directions.
		Direction dir = (Direction)i; // Cast integer to Direction.
		uint16_t d0 = distance(dir, false); // Take measurements of the both sensors in a chosen direction dir.
		uint16_t d1 = distance(dir, true);
		...
	}
	...
}

Direction RobotMaze:: wallClosest() {
	...
	for (uint8_t i = 0; i < 4; i++) { // Iterate all the 4 directions.
		...
		//If both of the distances read walls and their difference is not too big, we conclude that there is a wall in this direction.
		if (d0 < NO_WALL_DISTANCE && d1 < NO_WALL_DISTANCE && abs(d0 - d1) < WALL_FOLLOW_ERROR_ALLOWED) {
			if (d0 < d1) // Find the smaller of 2 values.
				d0 = d1;
			if (d0 < leastDistance) { // If it small enough, change the running minimum.
				leastDistance = d0;
				closest = dir; // Remember the direction of the new minimum.
			}
		}
	}
	...
}

Direction RobotMaze:: wallClosest() {
	...
	return closest;
}