/*
 * RFIDservo
 * by larskflem
 *
 * Makes the servo drive to a specified angle when a given rfid tag is present, 
 * waits, and then moves back to its original position.
 *
 * Adapted from http://www.arduino.cc/en/Tutorial/Button and Tod E. Kurt's sketch 
 * "Servo Serial Simple" found on his blog of fun, http://todbot.com/blog/bionicarduino/ and 
 * code examples from Loic's work with the APSX RFID reader:
 * http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1188236776/0
 */

#include <SoftwareSerial.h>

#define rxPin 0
#define txPin 2
#define TAG_LEN 12
#define ARR_LEN 3

// set up a new serial 
SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);

// rfid variables
int ledPinY = 11;                // yellow feedback LED for RFID
int ledPinG = 12;                // green feedback LED for servo 
int serIn;
byte code[TAG_LEN];		 // var that will hold the bytes-in read from the serialBuffer
int bytes_read = 0;

// tags
// blue keychain = 0,0,DF,D5,53,11,0,1,4,E0,7A,A
// flat transparent = 0,0,C0,D,54,11,0,1,4,E0,5F,D6
// white 2 cm = 0,0,58,4E,A8,10,0,1,4,E0,2,FE
char target_tag[ARR_LEN][TAG_LEN] = 
{{0x00, 0x00, 0xdf, 0xd5, 0x53, 0x11, 0x00, 0x01, 0x04, 0xe0, 0x7a, 0x0a},
{0x00, 0x00, 0xc0, 0x0d, 0x54, 0x11, 0x00, 0x01, 0x04, 0xe0, 0x5f, 0xd6},
{0x00, 0x00, 0x58, 0x4e, 0xa8, 0x10, 0x00, 0x01, 0x04, 0xe0, 0x02, 0xfe}}; 

// servo variables
int servoPin = 7;		// servo connected to digital pin
int inputVal = 0;		// variable for reading the pin status
int myAngle;			// angle of the servo (roughly in degrees) 0-180
int pulseWidth;			// function variable
int servoVal = 1;		// static variable for angle servo is positioned to

/* 
 * The servo's 180 degrees are split into nine positions, set by servoVal: 
 * value = degrees
 *  0 = 0
 *  1 = 20
 *  2 = 40
 *  3 = 60
 *  4 = 80
 *  5 = 100
 *  6 = 120
 *  7 = 140
 *  8 = 160
 *  9 = 180
 */

void setup()  {
  // define pin modes for tx, rx, led pins:
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  pinMode(ledPinY, OUTPUT);		// set feedback LED for detected RFID as output
  pinMode(ledPinG, OUTPUT);		// set feedback LED for recognized RFID as output
  pinMode(servoPin, OUTPUT);		// set servoPin pin as output
  
  mySerial.begin(19200);                // set the data rate for the SoftwareSerial port
  Serial.begin(19200);                  // opens serial port, sets data rate to 19200 bps
  Serial.println("Rfid Servo ready");
}

void loop () {
  mySerial.print(0xfa, BYTE); // request Tag code
	 
  if(serialAvailable()){
	// keep reading from serial untill there are bytes in the serial buffer
	while (serialAvailable() && bytes_read < TAG_LEN){         // read Serial	  
	code[bytes_read] = Serial.read();
	bytes_read++;                                              // ready to read next digit
	}
	    // feedback LED
            digitalWrite(ledPinY, HIGH);
	    delay(200);
	    digitalWrite(ledPinY, LOW);
  }
  
  // print out later in the loop the sentence only if it has actually been collected;
  if( bytes_read >= TAG_LEN){
    for(int i=0; i<bytes_read; i++){
	    Serial.print(code[i], HEX);    // print out the byte at the specified index
		Serial.print(",");
    }
    for(int i = 0; i < ARR_LEN; i++)
    {
      if(memcmp(code, target_tag[i], TAG_LEN) == 0 )
	{
	  servoAction();
	}
    }
    Serial.flush(); // flush serial buffer
  }

  Serial.println();
  delay(200);
  bytes_read = 0;
}

void servoPulse(int servoPin, int myAngle) {
  pulseWidth = (myAngle * 9) + 700;   // converts angle to microseconds
  digitalWrite(servoPin, HIGH);       // set servo high
  delayMicroseconds(pulseWidth);      // wait a very small amount 
  digitalWrite(servoPin, LOW);        // set servo low
  delay(20);                          // refresh cycle of typical servos (20 ms)
}

void servoAction() {
  digitalWrite(ledPinG, HIGH);			        // turn feedback LED ON
  servoVal = servoVal * (180/9);			// convert from number to degrees
    for( int i=0; i<50; i++ ) {		        	// drive to selected angle
      servoPulse(servoPin, servoVal);
    }
  servoVal = servoVal / (180/9);			// convert from degrees to number
  delay(400);						// wait
  digitalWrite(ledPinG, LOW);				// turn feedback LED OFF
    for( int i=0; i<50; i++ ) {			        // drive back
      servoPulse(servoPin, servoVal - servoVal);
    } 
}

void greenHelloWorld() {
  // for debugging
  digitalWrite(ledPinG, HIGH);
  delay(300);
  digitalWrite(ledPinG, LOW);
  delay(300);
}