straightCode

#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[] = {200, 150};
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 = HIGH;
int initalSensorState2 = HIGH;
boolean ini = HIGH;

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

  // set SENSOR pins as digital in
  Sensor0 = 5; // Sensor0 connected to this digital pin number
  Sensor1 = 7; // Sensor1 connected to this digital pin number
  Sensor2 = 6; // 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 = digitalRead(Sensor0);//HIGH;
  PrevSensor1 = digitalRead(Sensor1);//LOW;
  PrevSensor2 = digitalRead(Sensor2);
  //
  //  Serial.print(PrevSensor0);
  //  Serial.print(", ");
  //  Serial.print (PrevSensor1);
  //  Serial.print(",");
  //  Serial.println (PrevSensor2);
  delay(1);

  if (PrevSensor0 == LOW && digitalRead(Sensor0) == HIGH) // if hole
  {

    //   PrevSensor0 = digitalRead(Sensor0);

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

    myMotor1->run(FORWARD);
    myMotor2->run(FORWARD);

  }

  if ( Motor1Boolean == true)
  {
    if (PrevSensor1 == LOW && digitalRead(Sensor1) == HIGH) // if from hole to paper
    {


      if (initalSensorState1 == HIGH) { // sensor was not in hole
        Motor1Boolean = false;
        myMotor1->run(RELEASE);


      } else {
        initalSensorState1 = HIGH;

      }

    }
  }

  delay(1);

  if ( Motor2Boolean == true)
  {
    if (PrevSensor2 == LOW && digitalRead(Sensor2) == HIGH) // if from hole to paper
    {

      if (initalSensorState2 == HIGH) { // sensor was not in a hole

        Motor2Boolean = false;
        myMotor2->run(RELEASE);

      } else {
        initalSensorState2 = HIGH;

      }

    }
  }

  //delay(5);
}