Autonomous Obstacle Avoidance

Code:

#include <Servo.h> // Import servo library

const int kLeftWheel_Backwards = 3; // Left wheel backwards pin

const int kLeftWheel_Forwards = 5; // Left wheel forwards pin

const int kRightWheel_Forwards = 6; // Right wheel forwards pin

const int kRightWheel_Backwards = 11; // Right wheel backwards pin

const int servoPin = 2; // Servo pin

const int trigPin = 9; // Trig of distance sensor pin

const int echoPin = 10; // Echo of distance sensor pin

const int scanSpeed = 30; // Amount servo rotates per timestep when scanning

const int motorSpeed = 200; // Speed of the driving motors

const int turnTime = 350; // Time spent when preforming a turn

const float distConversion = 0.16; // Conversion between distance signal and mm

const float detectDist = 350.0; // Distance to detect obsticals from

const float sideOffset = 70.0; // Distance from distance sensor to side of rover

Servo servo; // Servo object

float getDistance() { // Gets the distance

  digitalWrite(trigPin, LOW); // Write a low signal to trigPin

  delayMicroseconds(2); // Wait

  digitalWrite(trigPin, HIGH); // Write a high signal to trigPin

  delayMicroseconds(10); // Wait

  digitalWrite(trigPin, LOW); // Write a low signal to trigPin

  return pulseIn(echoPin, HIGH) * distConversion; // Get the signal from echoPin and convert to mm

}

void setLeftMotor(int speed) { // Set speed of left motor

  int s = constrain(speed, -255, 255); // Constrain speed to accepted range

  if (speed > 0) { // If speed is positive

    analogWrite(kLeftWheel_Forwards, s); // Spin forward

    analogWrite(kLeftWheel_Backwards, 0); // Dont spin backwards

  } else if (speed < 0) { // If speed is negative

    analogWrite(kLeftWheel_Forwards, 0); // Dont spin forward

    analogWrite(kLeftWheel_Backwards, -s); // Spin backward

  } else { // If speed is 0

    analogWrite(kLeftWheel_Forwards, 0); // Dont spin forward

    analogWrite(kLeftWheel_Backwards, 0); // Dont spin backward

  }

}

void setRightMotor(int speed) { // Set speed of right motor

  int s = constrain(speed, -255, 255); // Constrain speed to acceptable range

  if (speed > 0) { // If speed is positive

    analogWrite(kRightWheel_Forwards, s); // Spin forward

    analogWrite(kRightWheel_Backwards, 0); // Dont spin backward

  } else if (speed < 0) { // If speed is negative

    analogWrite(kRightWheel_Forwards, 0); // Dont spin forward

    analogWrite(kRightWheel_Backwards, -s); // Spin backward

  } else { // If speed is 0

    analogWrite(kRightWheel_Forwards, 0); // Dont spin forward

    analogWrite(kRightWheel_Backwards, 0); // Dont spin backward

  }

}

void drive() { // Drive

  setLeftMotor(motorSpeed); // Set left motor to motorSpeed

  setRightMotor(motorSpeed); // Set right motor to motorSpeed

}

void turnLeft() { // Turn left

  setLeftMotor(motorSpeed); // Set left motor to motorSpeed

  setRightMotor(-motorSpeed);  // Set right motor to negative motorSpeed

  delay(turnTime); // Wait

  drive(); // Drive forward

}

void turnRight() { // TurnRight

  setLeftMotor(-motorSpeed); // Set left motor to negative motorSpeed

  setRightMotor(motorSpeed); // Set right motor to motorSpeed

  delay(turnTime); // Wait

  drive(); // Drive forward

}

void setup() { // Setup

  pinMode(kRightWheel_Backwards, OUTPUT); // Set right wheel backward pin

  pinMode(kRightWheel_Forwards, OUTPUT); // Set right wheel forward pin

  pinMode(kLeftWheel_Forwards, OUTPUT); // Set left wheel forward pin

  pinMode(kLeftWheel_Backwards, OUTPUT); // Set left wheel backward pin

  servo.attach(servoPin); // Setup servo

  pinMode(trigPin, OUTPUT); // Set trig pin to output

  pinMode(echoPin, INPUT); // Set echo pin to input

  drive(); // Drive

  servo.write(0);

  delay(1000);

  servo.write(180); // Set servo to 180 degrees

  Serial.begin(9600); // Start serial

}

int angle = -90; // Angle of servo

bool increasing = true; // Is angle increasing

void loop() { // Loop

  double dist = getDistance(); // Get the distance

  if (angle < -45) { // If servo is facing left

    double leftDist = dist * cos((angle + 90) * PI / 180.0); // Get distance to left wall

    if (leftDist < detectDist + sideOffset) { // If distance in range

      angle = -90; // Set angle left

      increasing = true; // Increase angle

      servo.write(180); // Set servo left

      turnRight(); // Turn right

    }

  } else if (angle > 45) { // If servo is facing right

    double rightDist = dist * cos((angle - 90) * PI / 180.0); // Get distance to right wall

    if (rightDist < detectDist + sideOffset) { // If distance in range

      angle = 90; // Set angle right

      increasing = false; // Decrease angle

      servo.write(0); // Set servo right

      turnLeft(); // Turn left

    }

  } else { // If servo centered

    double forwardDist = dist * cos(angle * PI / 180.0); // Get distance to forward wall

    if (forwardDist < detectDist) { // If distance in range

      angle = -90; // Set angle left

      increasing = true; // Increase angle

      servo.write(180); // Set servo left

      turnRight(); // Turn right

    }

  }

  if (increasing) { // If angle is increasing

    angle += scanSpeed; // Increase angle

  } else { // If angle is decreasing

    angle -= scanSpeed; // Decrease angle

  }

  if (angle == -90 || angle == 90) { // If angle is -90 or 90

    increasing = !increasing; // Swap direction

  }

  servo.write(90 - angle); // Set servo to angle

  delay(100); // Wait

}