Obs_Avoid

#include <Servo.h>
#include <NewPing.h>
#include <ZumoMotors.h>
#include <ZumoBuzzer.h>
#include <Pushbutton.h>

#define TRIGGER_PIN  5
#define ECHO_PIN     4
#define MAX_DISTANCE 200

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
ZumoBuzzer buzzer;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);

Servo myservo;

int pos = 90; // pos for position and set a 0 degrees
int lastError1 = 0; // Used for Kd
int lastError2 = 0;
const int MAX_SPEED = 200; // Max speed up to 400
int m1Speed = 0;
int m2Speed = 0;

void setup()
{
  Serial.begin(9600);
    // Play a little welcome song
  buzzer.play(">g32>>c32");
  // Wait for the user button to be pressed and released
   button.waitForButton();

   // Turn on LED to indicate we are in calibration mode
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);

  // Serial.begin (9600); Used for troubleshooting
  myservo.attach(11);
  myservo.write(89);

    // Turn off LED to indicate we are through with calibration
  digitalWrite(13, LOW);
  buzzer.play(">g32>>c32");

  // Wait for the user button to be pressed and released
  button.waitForButton();

  // Play music and wait for it to finish before we start driving.
  buzzer.play("L16 cdegreg4");
  while(buzzer.isPlaying());

  delay(500);
}

void loop()
{
    myservo.write(36);  // check distance to the right
    delay(400);
    int rightDistance = sonar.ping_cm(); //set right distance in cm
    delay(100);
    myservo.write(144);  // check distace to the left
    delay(400);
    int leftDistance = sonar.ping_cm(); //set left distance in cm
    delay(100);
    myservo.write(90); //return to center
    delay(100);

  Serial.print("Right: ");
  Serial.print(rightDistance);
  Serial.print(" cm  ");
  Serial.print("Left: ");
  Serial.print(leftDistance);
  Serial.println(" cm");

    int error1 = 15 - rightDistance; //distance is in cm
    int error2 = 15 - leftDistance;

  // Get motor speed difference using proportional and derivative PID terms
  // (the integral term is generally not very useful for line following).
  // Here we are using a proportional constant of 1/4 and a derivative
  // constant of 6, which should work decently for many Zumo motor choices.
  // You probably want to use trial and error to tune these constants for
  // your particular Zumo and line course.

  // Error 1 is from the right distance reading feeding into the
  // left motor control
  // Error 2 is from the left distance reading feeding into the
  // right motor control


    int speedDifference = error1 / 4 + 6 * (error1 - lastError1);
    int speedDifference2 = error2 / 4 + 6 * (error2 - lastError2);

    // Defining the difference between the two reading for the
    // differential controller

    lastError1 = error1;
    lastError2 = error2;

    // Uses the right error to drive the left wheel
    // Uses the left error to drive the right wheel
    m1Speed = MAX_SPEED - speedDifference;
    m2Speed = MAX_SPEED - speedDifference2;

    // Limits the wheel speed so that if theres a negative
    // speed difference the wheel speed does go over max

  if (m1Speed > MAX_SPEED)
    m1Speed = MAX_SPEED;
  if (m2Speed > MAX_SPEED)
    m2Speed = MAX_SPEED;

  motors.setSpeeds(m1Speed, m2Speed);

}