IR Light Targeting

void seekLight() {

   int servoForwardDegree = 90;   //this value makes the servo point forward. 

   int lightSensorStopThreshold = 700;

  int iravg[4] = {0,0,0,0};

  int total=0;

  for (int i=0; i<4;i++) total+=iravg[i]=analogRead(analogIRPin);

  int iravgindex=0;

  int lowest = analogRead(analogIRPin);

  leftMotorSpeed = -leftMotorDefaultSpeed*0.9;    //sets motors to eighty percent

  rightMotorSpeed = rightMotorDefaultSpeed;

  leftTick=0;

  enableRotaryInterrupts();

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

  myservo.write(servoForwardDegree);                            // sets the servo position according to the current degree

  long now = millis();

  while (millis()-now<1000);

  //scan for light source

  Forward(0);                                  //sends motor speeds to motor

  while (leftTick<50) {                        //approx one revolution

    total=total-iravg[iravgindex]+(iravg[iravgindex]=analogRead(analogIRPin));

    if (lowest>iravg[iravgindex]) lowest=iravg[iravgindex];

    iravgindex=(iravgindex+1)%4;               //saves lowest value

    analogIRSensor=total/4;

    if (useSave) saveData(useSave);

    updateMenu();

  }

  Stop(0);

  delay(200); //wait for moemntum.

  Forward(0);

  //turn until we find the low range again

  while (analogIRSensor>(lowest+30)) {

    total=total-iravg[iravgindex]+(iravg[iravgindex]=analogRead(analogIRPin));

    iravgindex=(iravgindex+1)%4;              

    analogIRSensor=total/4;;

  }

  leftTick=0;

  //turn until we come back out of the low range

  while (analogIRSensor<(lowest+35)){

    total=total-iravg[iravgindex]+(iravg[iravgindex]=analogRead(analogIRPin));

    iravgindex=(iravgindex+1)%4;              

    analogIRSensor=total/4;;;

  }

  Stop(0);

  delay(200);

  //find middle of light range

  int currentTickCount=leftTick;        //current ticks counted through low range

  leftMotorSpeed=-leftMotorSpeed;

  rightMotorSpeed=-rightMotorSpeed;

  leftTick=0;

  Forward(0);

  while (leftTick<currentTickCount/2);  //go back half the ticks in the low range

  Stop(0);

  delay(200);

  //go toward light

  leftMotorSpeed=leftMotorDefaultSpeed;

  rightMotorSpeed=rightMotorDefaultSpeed*1.1;

  Forward(0);

  bool clockwise=true; //initial direction of servo

  int degree;

  myservo.write(degree=servoForwardDegree-20);

  now = millis();

  while (millis()-now<1000);

  total=0;

  for (int i=0; i<4;i++) total+=iravg[i]=analogRead(analogIRPin);

  iravgindex=0;

  int timeStopSensorActivated=0;

  while (1) { 

    Forward(0);

    int minDegree;

    int minimum=1024;

    int d=0;

    for (degree; (clockwise) ? degree<=servoForwardDegree+20 : degree>=servoForwardDegree-20; (clockwise) ? degree++ : degree--) {

      //move servo

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

      myservo.write(degree);                            // sets the servo position according to the current degree

      now = millis();

      while(millis()-now<10); //wait for sensor to get there. 

      //servoOff();

      if (degree==servoForwardDegree) {

        servoOff();

        delay(100);

        checkForObstacle(1);

        if (timeToStop) break;

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

      }

      //calculating moving average

      total=total-iravg[iravgindex]+(iravg[iravgindex]=analogRead(analogIRPin));

      iravgindex=(iravgindex+1)%4;              

      analogIRSensor=total/4;;

      //if minimum is found, add as minDegree from 2 degrees before/after depending on direction of servo

      if (analogIRSensor<minimum) { minimum=analogIRSensor; (clockwise) ? minDegree=degree-2 : minDegree=degree+2; }  

      //continually check for STOP line. 

      //if (analogRead(A1)>lightSensorStopThreshold && timeStopSensorActivated==0) timeStopSensorActivated=millis();

      //else if (analogRead(A1)<lightSensorStopThreshold) timeStopSensorActivated=0;

      //if ((millis()-timeStopSensorActivated>50 && timeStopSensorActivated>0) || ((d=microsecondsToCentimeters(ping()))<10 && d!=0)) { timeToStop=true; break; }

      currentPingDistance=microsecondsToCentimeters(ping());

      if (analogRead(A1)>lightSensorStopThreshold) { timeToStop=true; break; }

      //log the data

      if (useSave) saveData(useSave);

    }

    if (timeToStop) break;

    clockwise=!clockwise;   

    leftMotorSpeed=leftMotorDefaultSpeed*0.95*0.85-17*(servoForwardDegree-minDegree)/20;

    rightMotorSpeed=rightMotorDefaultSpeed*1.05*0.85+17*(servoForwardDegree-minDegree)/20;

  }

  servoOff();

  Stop(0);

  currentMenu=draw;

}