StateMachine2

#include <Keypad.h>
#include <SoftwareSerial.h>
#include <Adafruit_RGBLCDShield.h>
#include <Wire.h>
#include <utility/Adafruit_MCP23017.h>

Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();

//Parallax RFID Reader
#define RFIDEnablePin 2 //Pin that enables reading. Set as OUTPUT and LOW to read an RFID tag
#define RFIDSerialRate 2400 //Parallax RFID Reader Serial Port Speed
#define SLEEP 200 //Default sleep duration for RF reader

//Using SoftwareSerial Library to locate the serial pins off the default set
//This allows the Arduino to be updated via USB with no conflict
#define RxPin 5 //Pin to read data from Reader
#define TxPin 4 //Pin to write data to the Reader NOTE: The reader doesn't get written to, don't connect this line.
SoftwareSerial RFIDReader(RxPin, TxPin);

#define Num_Chips 7 //Numer of RFID chips in array
#define ROWS  4 // Four rows
#define COLS  4 // Four columns
#define RFID_CODE 10 //size of RFID tag

#define PWM 3 //Pin for the motor


//If the RFID reader is currently reading

//The state of the machine, either WAIT waiting for a button press or 1 while looking for an RFID tag
int state;

//The RFID tag currently being searched for
String target = "";
String lastTag = "";

// Define the Keymap
char keys[ROWS][COLS] =
{
  {'1', '2', '3', 'A'},
  {'4', '5', '6', NO_KEY},
  {'7', NO_KEY, NO_KEY, NO_KEY},
  {NO_KEY, NO_KEY, NO_KEY, 'x'}
};

// Define Array of Strings for RFID tags
String chips[Num_Chips] = {"0000958C92", "0000934267", "0000955FD0", "0000928EB5", "0000934425", "000093242A", "000093815C"};


//Variables used in RFID scanning
int bytesRead, val;

//Storage for RFID codes during scanning
char code[RFID_CODE];

// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
byte rowPins[ROWS] = { 9, 8, 7, 6 };

// Connect keypad COL0, COL1 and COL2 to these Arduino pins.
byte colPins[COLS] = {13, 12, 11, 10 };

// Create the Keypad
Keypad kpd = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );


//Starts the motor forward and ramps the speed up to full
void startMotor()
{
  digitalWrite (PWM, HIGH);
  Serial.println("Starting motor RPM ramp-up");
  //Slowly ramp the motor up from WAIT to full speed
  for (int motorValue = 0 ; motorValue <= 255; motorValue += 5)
  {
    analogWrite(PWM, motorValue);
    delay(30);
  }
}

//Slows the motor and eventually disables it
void stopMotor()
{
  //Slowly ramp the motor down from full speed to 0
  Serial.println("Starting motor ramp-down");
  for (int motorValue = 255 ; motorValue >= 0; motorValue -= 5)
  {
    analogWrite(PWM, motorValue);
    delay(30);
  }
  Serial.println("Motor disabled");
  digitalWrite (PWM, LOW);
}

void resetLCD()
{
  lcd.print("Select Box #");
}

//Reads data from the RFID reader and if it is a valid tag assigns it to a string
void ReadSerial(String &ReadTagString)
{
  int bytesread = 0;
  int  val = 0;
  char code[10];
  String TagCode = "";

  if (RFIDReader.available() > 0) {         // If data available from reader
    if ((val = RFIDReader.read()) == 10) {  // Check for header
      bytesread = 0;
      while (bytesread < 10) {              // Read 10 digit code
        if ( RFIDReader.available() > 0) {
          val = RFIDReader.read();
          if ((val == 10) || (val == 13)) { // If header or stop bytes before the 10 digit reading
            break;                         // Stop reading
          }
          code[bytesread] = val;           // Add the digit
          bytesread++;                     // Ready to read next digit
        }
      }
      if (bytesread == 10) {               // If 10 digit read is complete

        for (int x = 0; x < 10; x++)       //Copy the Chars to a String
        {
          TagCode += code[x];
        }
        ReadTagString = TagCode;          //Update the caller
        while (RFIDReader.available() > 0) //Burn off any characters still in the buffer
        {
          RFIDReader.read();
        }

      }
      bytesread = 0;
      TagCode = "";
    }
  }
}

//This method checks to see if the RFID reader has found the desired tag, if not it continues to run until it does
void scanRFID()
{
  String tag;
  if (RFIDReader.available() > 0)
  {
    ReadSerial(tag);

    if (tag != lastTag)
    {
      lastTag = tag;
      Serial.print("Reading tag " + tag);
    }
    if (tag == target)
    {
      Serial.println("Found the correct tag, disabling motor");
      resetLCD();
      stopMotor();
      state = WAIT;
    }
  }
}


void setup()
{
  // RFID reader SOUT pin connected to Serial RX pin at 2400bps
  RFIDReader.begin(RFIDSerialRate);

  // Set Enable pin as OUTPUT to connect it to the RFID /ENABLE pin
  pinMode(RFIDEnablePin, OUTPUT);

  // Activate the RFID reader
  // Setting the RFIDEnablePin HIGH will deactivate the reader
  // which could be usefull if you wanted to save battery life for
  // example.
  digitalWrite(RFIDEnablePin, LOW);
  Serial.begin(9600);           // set up Serial library at 9600 bps

  digitalWrite(PWM, LOW);       //Set the motor to off

  lcd.begin(16, 2);
  resetLCD();

  state = WAIT;
}

void loop()
{
  char key = kpd.getKey();
  Serial.print("State is currently : " + state);
  if (key != NO_KEY) //If the key is a valid key
  {
    if (key == 'x' && state == 1) //If killswitch key is pressed disbale motor and set state to WAIT
    {
      Serial.println("Killswitch pressed, deactivating motor");
      resetLCD();
      stopMotor();
      state = WAIT;
    }
    else//If numerical key is pressed
    {
      if (key != 'x')
      {
        String message = "Box ";
        message += key;
        message += " Selected";

        lcd.print(message);
        Serial.println("Setting target to " + chips[(key - '0') - 1]);
        target = chips[(key - '0') - 1]; //Get the ID string for the toolbox associated with the key pressed

        if (state != 1) //Set state to 1ing and enable motor if not already on
        {
          Serial.println("Starting search for " + target);
          state = 1;
          startMotor();
        }
      }
    }
  }

  if (state == 1)
  {
    scanRFID(); //Reads from the RFID reader and checks if the correct tag has been found
  }
}