Box Ramming

void boxram() {

  Stop(0);

  delay(200);

  //here we locate the line and then do a simple line follow

  long imm = millis();

  followCurvyLine(1); //this initial call clears the adjustmnet data

  while(millis()-imm<2000) followCurvyLine(0);

  Stop(0);

  delay(1000);

  leftMotorSpeed=-130;

  rightMotorSpeed=130;

  Forward(0);

  while (digitalRead(PaperSensor)==HIGH && digitalRead(LineSensor)==HIGH && !lost);

  if (lost) return;

  Stop(0);

  delay(1000);

  if (digitalRead(PaperSensor)==LOW && !lost) {

    leftMotorSpeed=130;

    rightMotorSpeed=-130;

    Forward(0);

    while (digitalRead(LineSensor)==HIGH && !lost);

    if (lost) return;

    Stop(0);

    delay(1000);

  }

  if (lost) return;

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

  myservo.write(180);

  long now=millis();

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

  servoOff();

  long atleast=millis();

  int dist=151;

  long pingTime=0;

  followBasicLine(1);

  //follow line until box is found

  while ((dist>120 || dist==0 || millis()-atleast<100) && !lost) {

  //  followWallBasic(dist,initialDistance);

    followBasicLine(0);

    pingTime=ping();

    dist=currentPingDistance=microsecondsToCentimeters(pingTime);

    delay(10);  //so ping doesn't here its own echos

    if (useSave) saveData(useSave);

  }

  if (lost) return;

  atleast=millis();

  leftTick=0;

  Stop(0);

  delay(1000);

  //point servo forward;

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

  myservo.write(90);

  now=millis();

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

  servoOff();

  //turn to face box

  leftMotorSpeed=leftMotorDefaultSpeed/1.3;///1.3;//*0.8;

  rightMotorSpeed=-rightMotorDefaultSpeed/1.3;///1.3;//*0.9;

  Forward(0);

  atleast=millis();

  rightTick=0;

  dist=currentPingDistance=microsecondsToCentimeters(pingTime);

  delay(10);

  while ( (dist>120 || dist==0 || millis()-atleast<1000) && !lost) {

    dist=currentPingDistance=microsecondsToCentimeters(ping());

    if (useSave) saveData(useSave);

  }

  //once we see the box we go three extra ticks

  //trying to detect both edges of the box with the ping didn't work

  //i assumed that the gp2d12 was not good enough to detect the box at its farthest distance, though we did not try. 

  leftTick=0;

  while(leftTick<3);

  Stop(0);

  //light laser

  digitalWrite(laserPin, HIGH);

  now=millis();

  while(millis()-now<5000 && !lost);

  digitalWrite(laserPin, LOW);

  if (lost) return;

  //drive forward until out of zone

  leftMotorSpeed=leftMotorDefaultSpeed*0.7;//*0.64;

  rightMotorSpeed=rightMotorDefaultSpeed*0.8;//*0.8;

  Forward(0);

  atleast=millis();

  while ((dist>20 || dist==0 || millis()-atleast<100) && digitalRead(LineSensor)==HIGH && !lost) {

   // pingTime=ping();

    dist=currentPingDistance=microsecondsToCentimeters(pingTime);

    long now=millis();

    while(millis()-now<100);

    if (useSave) saveData(useSave);

  }

  if (lost) return;

  leftMotorSpeed=leftMotorDefaultSpeed*1.1;

  rightMotorSpeed=rightMotorDefaultSpeed*1.3;

  Forward(0);

  while(digitalRead(LineSensor)==HIGH && !lost);

  if (lost) return;

  Stop(0);

  delay(300);

  //backup a bit

  leftMotorSpeed=-leftMotorDefaultSpeed;

  rightMotorSpeed=-rightMotorDefaultSpeed;

  leftTick=0;

  Forward(0);

  while(leftTick<10 && !lost);

  if (lost) return;

  Stop(0);

  delay(300);

  //turn around

  leftTick=0;

  leftMotorSpeed=-leftMotorDefaultSpeed/1.3;//*0.9;

  rightMotorSpeed=rightMotorDefaultSpeed/1.3;//*0.9;

  Forward(0);

  while(leftTick<28 && !lost);

  if (lost) return;

  Stop(0);

  findLineAgain();

}