Parking

void park() {

  delay(200);

  if (!myservo.attached()) myservo.attach(servoPin);

  myservo.write(180);    

  long now=millis();

  while(millis()-now<1500); //wait for servo

  servoOff();

  int pingMovingAverage[4];

  int total;

  int index=0;

  for (int i=0;i<4;i++) { total+=pingMovingAverage[i]=microsecondsToCentimeters(ping()); delay(50);}

  int distance = total/4;

  int initialDistance = distance;

  int baseleftspeed = leftMotorSpeed=leftMotorDefaultSpeed*0.8;

  int baserightspeed = rightMotorSpeed=rightMotorDefaultSpeed*0.85;

  Forward(0);

  leftTick=0;

  int lasterror=0;

  while (leftTick<40) {

    total-=pingMovingAverage[index];

    total+=pingMovingAverage[index]=microsecondsToCentimeters(ping());

    Serial.println(pingMovingAverage[index]);

    index=(index+1)%4;

    distance=total/4;

    if (distance-initialDistance>15) {

      enableRotaryInterrupts();

      leftMotorSpeed=baseleftspeed;

      rightMotorSpeed=baserightspeed;

      Forward(0);

    }

    else if (distance-initialDistance<=15) {

      detatchrotaryInterrupts();

      leftMotorSpeed=constrain(baseleftspeed+2.7*(distance-initialDistance)+2.3*(lasterror), baseleftspeed-70,baseleftspeed+70);

      rightMotorSpeed=constrain(baserightspeed-2.7*(distance-initialDistance)-2.3*(lasterror), baserightspeed-70,baserightspeed+70);

      leftMotorSpeed=baseleftspeed;

      rightMotorSpeed=baserightspeed;

      lasterror=distance-initialDistance;

      Forward(0);

      leftTick=0;

    }

    delay(10);

  }

  leftMotorSpeed=leftMotorDefaultSpeed*0.64;

  rightMotorSpeed=rightMotorDefaultSpeed*0.64;

  Forward(0);

  while(leftTick<40+initialDistance);

  Stop(0);

  delay(100); //momentum

  enableRotaryInterrupts();

  //slow

  //turn to back in

  leftTick=0;

  //set motors to 70%

  leftMotorSpeed=-leftMotorDefaultSpeed*0.7;

  rightMotorSpeed=rightMotorDefaultSpeed*0.7;

  Forward(0);

  while (leftTick<7);

  Stop(0);

  delay(100);

  //back in

  rightMotorSpeed=(-rightMotorDefaultSpeed*0.7);;//rightMotorSpeed;

  leftTick=0;

  Forward(0);

  while (leftTick<(int)(sqrt(sq(35+initialDistance)*2)-30));

  Stop(0);

  delay(1000);

  if (!myservo.attached()) myservo.attach(servoPin);

  myservo.write(81);    

  now=millis();

  while(millis()-now<1500); //wait for servo

  servoOff();

  //turn to allign with road

  leftMotorSpeed=leftMotorDefaultSpeed*0.7;//leftMotorSpeed;

  leftTick=0;

  Forward(0);

  while(leftTick<7); //doing some trickery

  int lastRead = microsecondsToCentimeters(ping());

  int currentRead=lastRead;

  long lastReadTime = millis();

  while (lastRead>=currentRead && currentRead!=0) {

    if (millis()-lastReadTime>50) {

      lastRead=currentRead;

      currentRead=microsecondsToCentimeters(ping());

      lastReadTime=millis();

    }

  }

  Stop(0);

  detatchrotaryInterrupts();

  servoOff();

  currentMenu=draw;

}