analogReadCommentedNoInitialSensorState

#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
  //  pinMode(Sensor0, INPUT); // Define Sensor0 as a digital in pin
  //  pinMode(Sensor1, INPUT); // Define Sensor2 as a digital in pin
  //  pinMode(Sensor2, INPUT); // Define Sensor2 as a digital in pin



  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);


  Serial.print("________START___________");
  Serial.print("");
  Serial.print(PrevSensor0);
  Serial.print(", ");
  Serial.print (PrevSensor1);
  Serial.print(",");
  Serial.println (PrevSensor2);

  delay(50);
  //  timeMS=millis();
  int rn = analogRead(Sensor0);
  Serial.print("After delay: ");
  Serial.print(PrevSensor0);
  Serial.print(" , ");
  Serial.println(rn);
  if (PrevSensor0 < low && rn > high) // if hole
  {

    //   PrevSensor0 = analogRead(Sensor0);

    initalSensorState1 = readMyAnalog(Sensor1, initalSensorState1);
    initalSensorState2 = readMyAnalog(Sensor2, initalSensorState2);
    //Serial.print(initalSensorState1);
    //Serial.print(" ");
    //Serial.println(initalSensorState2);
    //delay(5);
    Motor1Boolean = true;
    Motor2Boolean = true;

    myMotor1->run(FORWARD);
    delay(1);
    myMotor2->run(FORWARD);
    stopVarC++;
    Serial.print("Motor Start");
    Serial.print(stopVarC);
    Serial.print("--(");
    Serial.print(PrevSensor0);
    Serial.print(", ");
    Serial.print(rn);
    Serial.print("), --");
    Serial.print (PrevSensor1);
    Serial.print(",");
    Serial.println (PrevSensor2);

    //    holeGoNum++;
    //        Serial.print("GOGO");
    //        Serial.println(holeGoNum);


  }

  //  Serial.print(millis()-timeMS);
  //  Serial.println(" <-- RUN");

  //  timeMS=millis();
  //delay(1);
  if ( Motor1Boolean == true)
  {
    rn = analogRead(Sensor1);
    Serial.print("before entering Sensor1:");
    Serial.print(PrevSensor1);
    Serial.print(" , ");
    Serial.println(rn);
    if (PrevSensor1 < low && rn > high) // if from hole to paper
    {
      //        holeNum++;
      //        Serial.print("Hole");
      //        Serial.println(holeNum);

      //if (initalSensorState1 > high) { // sensor was not in hole
        Motor1Boolean = false;
        myMotor1->run(RELEASE);
        stopVarA++;
        Serial.print("Stopped Motor One ");
        Serial.print(stopVarA);

        Serial.print(" times: ");
        Serial.print(PrevSensor0);
        Serial.print(",--( ");
        Serial.print (PrevSensor1);
        Serial.print(" , ");
        Serial.print(rn);
        Serial.print(",)--");
        Serial.println (PrevSensor2);

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



    }
  }
  //  Serial.print(millis()-timeMS);
  //  Serial.println(" <-- Motor1");
  delay(1);

  //  timeMS=millis();
  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);
        stopVarB++;
        Serial.print("Stopped Motor Two ");
        Serial.print(stopVarB);

        Serial.print(" times: ");
        Serial.print(PrevSensor0);
        Serial.print(", ");
        Serial.print (PrevSensor1);
        Serial.print(",--(");
        Serial.print(PrevSensor2);
        Serial.print(" , ");
        Serial.print(rn);
        Serial.println(",)--");

      } else {
        initalSensorState2 = high + up;

      }

    }
  }
  //  Serial.print(millis()-timeMS);
  //  Serial.println(" <-- Motor2");

  delay(1);
}


int readMyAnalog(int sensor, int prev) {
  int a = analogRead(sensor);
  //  Serial.print("readAnalog------>");
  //  Serial.print("Sensor:");
  //  Serial.print(sensor);
  //  Serial.print("=");
  //  Serial.println(a);
  if (!(a > low && a < high)) {
    return a;
  }
  //  if (abs(a - prev) > 100) {
  //    if (a < prev) {
  //      return low-10;
  //    } else {
  //      return high+10;
  //    }
  //  }
  return prev;
}