analogReadClean

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"



Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(2);// port M1
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(1);// port M2
int motorSpeed[] = {100, 120};
int motorDirection[] = {FORWARD, BACKWARD};
boolean  Motor1Boolean ;
boolean Motor2Boolean ;
int Sensor0 ; // Sensor0 connected to this digital pin number
int Sensor1 ; // Sensor1 connected to this digital pin number
int Sensor2 ; // Sensor1 connected to this digital pin number
int PrevSensor0, PrevSensor1, PrevSensor2;
char inChar;
//initalSensorState1, initalSensorState2;
int initalSensorState1 = 5;
int initalSensorState2 = 5;

unsigned int timeMS;
int stopVarA = 0;
int stopVarB = 0;
int stopVarC = 0;
int holeNum = 0;
int holeGoNum = 0;


int low = 200;
int high = 350;
int up = 900 - high;

void setup() {
  // initialize things
  Motor1Boolean = false;
  Motor2Boolean = false;

  // set SENSOR pins as digital in
  Sensor0 = A0; // Sensor0 connected to this digital pin number
  Sensor1 = A1; // Sensor1 connected to this digital pin number
  Sensor2 = A2; // Sensor1 connected to this digital pin number



  Serial.begin(9600);



  //initial motor speed
  AFMS.begin();  // create with the default frequency 1.6KHz
  //AFMS.begin(1000);  // OR with a different frequency, say 1KHz

  // Set the speed to start, from 0 (off) to 255 (max speed)
  myMotor1->setSpeed(motorSpeed[0]);
  myMotor2->setSpeed(motorSpeed[1]);

}

// the loop function runs over and over again forever
void loop() {
  PrevSensor0 = readMyAnalog(Sensor0, PrevSensor0); //4;
  PrevSensor1 = readMyAnalog(Sensor1, PrevSensor1); //2;
  PrevSensor2 = readMyAnalog(Sensor2, PrevSensor2);

  delay(50);

  int rn = analogRead(Sensor0);
  if (PrevSensor0 < low && rn > high) // if hole
  {
    initalSensorState1 = readMyAnalog(Sensor1, initalSensorState1);
    initalSensorState2 = readMyAnalog(Sensor2, initalSensorState2);

    Motor1Boolean = true;
    Motor2Boolean = true;

    myMotor1->run(FORWARD);
    delay(1);
    myMotor2->run(FORWARD);
    
  }

  if ( Motor1Boolean == true)
  {
    rn = analogRead(Sensor1);

    if (PrevSensor1 < low && rn > high) // if from hole to paper
    {
      
      //if (initalSensorState1 > high) { // sensor was not in hole
        Motor1Boolean = false;
        myMotor1->run(RELEASE);

//      } else {
//        initalSensorState1 = high + up;
//
//      }



    }
  }
  
  delay(1);

  if ( Motor2Boolean == true)
  {
    rn = analogRead(Sensor2);
    if (PrevSensor2 < low && rn > high) // if from hole to paper
    {

      if (initalSensorState2 > high) { // sensor was not in a hole

        Motor2Boolean = false;
        myMotor2->run(RELEASE);
      } else {
        initalSensorState2 = high + up;

      }

    }
  }
  delay(1);
}


int readMyAnalog(int sensor, int prev) {
  int a = analogRead(sensor);
  if (!(a > low && a < high)) {
    return a;
  }
  return prev;
}