roverbot_obstacle_avoidance

// ---------------------------------------------------------------------------
// Example RoverBot with Basic Obstacle Avoidance
// ---------------------------------------------------------------------------

#include <NewPing.h>
#include <AFMotor.h>

#define TRIGGER_PIN  34  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     28  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

AF_DCMotor motor(1, MOTOR12_64KHZ); // create motor #2, 64KHz pwm 
AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm 
AF_DCMotor motor3(3, MOTOR12_64KHZ); // create motor #2, 64KHz pwm 
AF_DCMotor motor4(4, MOTOR12_64KHZ); // create motor #2, 64KHz pwm

void setup() {
  delay(2000);
  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.

  motor.setSpeed(200);     // set the speed to 200/255
  motor2.setSpeed(200);
  motor3.setSpeed(200);
  motor4.setSpeed(200);

  Serial.println("started");
}

void loop() {
  unsigned int distance = sense();
  Serial.println("loop hit");
  if (distance > 50) {
    Serial.println("far object");
    move_forward();
  } else {
    stop_motors();
    turn_right();
  }
}

void move_forward() {
  Serial.print("moving forward");
  motor.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
  delay(1000);
  return;
}

void turn_right() {
  Serial.println("turning right");
  motor.run(BACKWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(BACKWARD);
  delay(1000);
  return;
}

void stop_motors() {
  Serial.println("stopping");
  motor.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
  return;
}

int sense() {
  delay(50);
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  unsigned int dist = uS / US_ROUNDTRIP_CM;
  Serial.print(dist);
  return dist; // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
}