//HoldDistanceWithPing
//maintain a set distance using Ping
// cannot turn

#include <Servo.h>


const int pingPin = 10;
const int pingServoPin = 11;
int LservoPin = 13;
int RservoPin = 12;

unsigned int duration, dist_in;
Servo servoL, servoR;

const int setpoint = 5;      //desired distance = 5 in
const int kp = -20;
//const int kpr = -20;


void setup()
{
  tone(4, 3000, 500);
  delay(500);
  
  servoL.attach(LservoPin);
  servoR.attach(RservoPin);
}

void loop() // Main loop auto-repeats
{
 
  // do ultrasonic stuff
  pinMode(pingPin, OUTPUT);          // Set pin to OUTPUT
  digitalWrite(pingPin, LOW);        // Ensure pin is low
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);       // Start ranging
  delayMicroseconds(5);              //   with 5 microsecond burst
  digitalWrite(pingPin, LOW);        // End ranging
  pinMode(pingPin, INPUT);           // Set pin to INPUT
  duration = pulseIn(pingPin, HIGH); // Read echo pulse
  dist_in = duration / 74 / 2;        // Convert to inches
  //Serial.println(dist_in);            // Display result
  delay(200);		             // Short delay

  int driveLR = (setpoint - dist_in) * kp;    //proportional control
  maneuver(driveLR, driveLR, 20);     // pass -200 to 200
}


// maneuver
void maneuver (int speedL, int speedR, int msTime)
{
  servoL.writeMicroseconds(1500 + speedL);
  servoR.writeMicroseconds(1500 - speedR);
  if (msTime == -1)
  {
    servoL.detach();
    servoR.detach();
  }
}