Lesson 25: Autonomous Navigation and Path Planning

// Simple obstacle avoidance navigation
void autonomousNavigation() {
  float frontDistance = getUltrasonicDistance();
  float leftDistance = getLeftSensorDistance();
  float rightDistance = getRightSensorDistance();
  
  const float SAFE_DISTANCE = 30; // cm
  const int MOVE_SPEED = 100;
  const int TURN_SPEED = 80;
  
  if (frontDistance > SAFE_DISTANCE) {
    // Path is clear - move forward
    moveForward(MOVE_SPEED);
    Serial.println("Moving forward");
  } else {
    // Obstacle detected - choose best direction
    stopMoving();
    delay(100);
    
    if (leftDistance > rightDistance && leftDistance > SAFE_DISTANCE) {
      // Turn left
      turnLeft(90);
      Serial.println("Turning left to avoid obstacle");
    } else if (rightDistance > SAFE_DISTANCE) {
      // Turn right
      turnRight(90);
      Serial.println("Turning right to avoid obstacle");
    } else {
      // Both sides blocked - turn around
      turnRight(180);
      Serial.println("Turning around - path blocked");
    }
    
    delay(500); // Allow turn to complete
  }
}

// Wall following navigation
enum WallFollowState {
  SEEKING_WALL,
  FOLLOWING_WALL,
  TURNING_CORNER
};

WallFollowState currentState = SEEKING_WALL;
const float WALL_DISTANCE = 20; // Desired distance from wall (cm)
const float WALL_TOLERANCE = 5;  // Acceptable distance variation

void wallFollowingNavigation() {
  float frontDistance = getUltrasonicDistance();
  float rightDistance = getRightSensorDistance();
  
  switch (currentState) {
    case SEEKING_WALL:
      if (frontDistance < 30) {
        // Found obstacle - start following
        currentState = FOLLOWING_WALL;
        turnRight(90); // Orient to follow wall
      } else {
        moveForward(100);
      }
      break;
      
    case FOLLOWING_WALL:
      if (frontDistance < 25) {
        // Corner or obstacle ahead
        currentState = TURNING_CORNER;
        stopMoving();
      } else if (rightDistance < WALL_DISTANCE - WALL_TOLERANCE) {
        // Too close to wall - turn left slightly
        turnLeft(15);
        moveForward(80);
      } else if (rightDistance > WALL_DISTANCE + WALL_TOLERANCE) {
        // Too far from wall - turn right slightly
        turnRight(15);
        moveForward(80);
      } else {
        // Good distance - move forward
        moveForward(100);
      }
      break;
      
    case TURNING_CORNER:
      turnLeft(90); // Turn around corner
      delay(1000);
      currentState = FOLLOWING_WALL;
      break;
  }
}

// Simple grid-based navigation
const int GRID_SIZE = 10;
int environmentGrid[GRID_SIZE][GRID_SIZE];
int robotX = 0, robotY = 0; // Current position
int goalX = 8, goalY = 8;   // Destination

void initializeGrid() {
  // 0 = free space, 1 = obstacle
  for (int x = 0; x < GRID_SIZE; x++) {
    for (int y = 0; y < GRID_SIZE; y++) {
      environmentGrid[x][y] = 0; // Start with empty grid
    }
  }
  
  // Add some obstacles
  environmentGrid[3][3] = 1;
  environmentGrid[3][4] = 1;
  environmentGrid[4][3] = 1;
  environmentGrid[6][7] = 1;
}

void moveToGoal() {
  // Simple greedy approach - move toward goal if possible
  int deltaX = goalX - robotX;
  int deltaY = goalY - robotY;
  
  if (abs(deltaX) > abs(deltaY)) {
    // Move in X direction
    if (deltaX > 0 && canMoveTo(robotX + 1, robotY)) {
      moveEast();
      robotX++;
    } else if (deltaX < 0 && canMoveTo(robotX - 1, robotY)) {
      moveWest();
      robotX--;
    } else {
      // X blocked, try Y
      if (deltaY > 0 && canMoveTo(robotX, robotY + 1)) {
        moveNorth();
        robotY++;
      } else if (deltaY < 0 && canMoveTo(robotX, robotY - 1)) {
        moveSouth();
        robotY--;
      }
    }
  } else {
    // Move in Y direction
    if (deltaY > 0 && canMoveTo(robotX, robotY + 1)) {
      moveNorth();
      robotY++;
    } else if (deltaY < 0 && canMoveTo(robotX, robotY - 1)) {
      moveSouth();
      robotY--;
    } else {
      // Y blocked, try X
      if (deltaX > 0 && canMoveTo(robotX + 1, robotY)) {
        moveEast();
        robotX++;
      } else if (deltaX < 0 && canMoveTo(robotX - 1, robotY)) {
        moveWest();
        robotX--;
      }
    }
  }
  
  // Check if goal reached
  if (robotX == goalX && robotY == goalY) {
    Serial.println("Goal reached!");
    celebrateGoal();
  }
}

bool canMoveTo(int x, int y) {
  // Check if position is valid and not blocked
  if (x < 0 || x >= GRID_SIZE || y < 0 || y >= GRID_SIZE) {
    return false; // Out of bounds
  }
  return environmentGrid[x][y] == 0; // Not blocked
}

void moveEast() {
  turnToDirection(0);   // Face east
  moveForward(50);      // Move one grid cell
  delay(1000);
}

void moveNorth() {
  turnToDirection(90);  // Face north
  moveForward(50);
  delay(1000);
}

void moveWest() {
  turnToDirection(180); // Face west
  moveForward(50);
  delay(1000);
}

void moveSouth() {
  turnToDirection(270); // Face south
  moveForward(50);
  delay(1000);
}