#include <SoftwareSerial.h>

// Pololu micro serial servo controller modes
// Software Serial from Arduino example by Tom Igoe
// put() (now position_absolute) & set_speed() functions found on pololu forums & modified
// mike crist 2009-01-03
/// All this code Meshed together by Oscar G. Torres

#define rxPin 3
#define txPin 4

int xBeePinA=0;
int analogValue = 0;  // the value returned from the analog sensor
long ct=0;
int stateA;
byte incomingByte = 0;	// for incoming serial data
byte nextByte=0;


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

void setup()
{
  pinMode(rxPin, INPUT);
  digitalWrite(txPin, HIGH); // keeps pololu board from getting spurious signal
  pinMode(txPin, OUTPUT);

  //-------------
  pinMode(xBeePinA, INPUT);
  stateA=0;

  // set the data rate for the hardware serial port
  Serial.begin(9600);
  // set the data rate for the SoftwareSerial port
  softSerial.begin(9600);

  // ARM 1
  // set initial servo speeds
  set_speed(1, 60);
  set_speed(2, 110);
  //set and move servos to neutral positions using command 5
  set_neutral(1, 3000);
  set_neutral(2, 1000);
  //initialize servos using command 0
  set_servo_parameters(1, 15);
  set_servo_parameters(2, 15);


}

void loop()
{


  //Get XBee Data
  if(analogRead(xBeePinA) < 250){
    stateA=1;
  }


  if (stateA == 1){                // Set logic to trigger arm when XBee signal is below 250


    position_absolute(1, 5300);
    position_absolute(2, 4600);
    delay(600);

    position_absolute(1, 600);
    position_absolute(2, 1900);
    delay(1000);
    
    position_absolute(1, 4500);
    delay(200);
    position_absolute(2, 4500);
    delay(1200);
    
    stateA = 0;
  }

}

void set_servo_parameters(byte servo, byte rangeVal)
{
  //this function uses pololu mode command 0 to set servo parameters
  //servo is the servo number (typically 0-7)
  //rangeVal of 15 gives 180 deg in 8-bit, 90 deg in 7 bit

  byte temp;
  byte parameters;

  temp = 1 << 6;   //set first two bits of parameters (on = 1, forward = 0)
  temp = temp + (rangeVal & 0x1f);   //put first five bits of rangeVal into temp
  parameters = temp & 0x7f;    //take only bottom 7 bits

  //Send a Pololu Protocol command
  softSerial.print(0x80,BYTE); //start byte
  softSerial.print(0x01,BYTE); //device id
  softSerial.print(0x00,BYTE); //command number
  softSerial.print(servo,BYTE); //servo number
  softSerial.print(parameters,BYTE); //parameters
}

void set_speed(byte servo, byte speedVal)
{
  //this function uses pololu mode command 1 to set speed
  //servo is the servo number (typically 0-7)
  //speedVal is servo speed (1=slowest, 127=fastest, 0=full)
  //set speedVal to zero to turn off speed limiting

  speedVal = speedVal & 0x7f; //take only lower 7 bits of the speed

  //Send a Pololu Protocol command
  softSerial.print(0x80,BYTE);     //start byte
  softSerial.print(0x01,BYTE);     //device id
  softSerial.print(0x01,BYTE);     //command number
  softSerial.print(servo,BYTE);    //servo number
  softSerial.print(speedVal,BYTE); //speed
}

void position_7bit(byte servo, byte posValue)
{
  //this function uses pololu mode command 2 to set position
  //servo is the servo number (typically 0-7)
  //posValue * range (set with command 0) adjusted by neutral (set with command 5)
  //determines servo position

  byte pos = posValue & 0x7f;     //get lower 7 bits of position

  //Send a Pololu Protocol command
  softSerial.print(0x80,BYTE);     //start byte
  softSerial.print(0x01,BYTE);     //device id
  softSerial.print(0x02,BYTE);     //command number
  softSerial.print(servo,BYTE);    //servo number
  softSerial.print(pos, BYTE);     //position
}

void position_8bit(byte servo, byte posValue)
{
  //this function uses pololu mode command 3 to set position
  //servo is the servo number (typically 0-7)
  //posValue * range (set with command 0) adjusted by neutral (set with command 5)
  //determines servo position

  byte temp;
  byte pos_hi,pos_low;

  temp = posValue & 0x80; //get bit 8 of position
  pos_hi = temp >> 7; //shift bit 8 by 7
  pos_low = posValue & 0x7f;     //get lower 7 bits of position

  //Send a Pololu Protocol command
  softSerial.print(0x80,BYTE);     //start byte
  softSerial.print(0x01,BYTE);     //device id
  softSerial.print(0x03,BYTE);     //command number
  softSerial.print(servo,BYTE);    //servo number
  softSerial.print(pos_hi, BYTE);  //bits 8 thru 13
  softSerial.print(pos_low, BYTE); //bottom 7 bits
}

void position_absolute(byte servo, int angle)
{
  //this function uses pololu mode command 4 to set absolute position
  //servo is the servo number (typically 0-7)
  //angle is the absolute position from 500 to 5500

  unsigned int temp;
  byte pos_hi,pos_low;

  temp = angle & 0x1f80;  //get bits 8 thru 13 of position
  pos_hi = temp >> 7;     //shift bits 8 thru 13 by 7
  pos_low = angle & 0x7f; //get lower 7 bits of position
  //Send a Pololu Protocol command
  softSerial.print(0x80, BYTE);    //start byte
  softSerial.print(0x01, BYTE);    //device id
  softSerial.print(0x04, BYTE);    //command number
  softSerial.print(servo, BYTE);   //servo number
  softSerial.print(pos_hi, BYTE);  //bits 8 thru 13
  softSerial.print(pos_low, BYTE); //bottom 7 bits
}

void set_neutral(byte servo, int angle)
{
  //this function uses pololu mode command 5 to set neutral position
  //servo is the servo number (typically 0-7)
  //angle is the absolute position from 500 to 5500

  unsigned int temp;
  byte pos_hi,pos_low;

  temp = angle & 0x1f80;  //get bits 8 thru 13 of position
  pos_hi = temp >> 7;     //shift bits 8 thru 13 by 7
  pos_low = angle & 0x7f; //get lower 7 bits of position

  //Send a Pololu Protocol command
  softSerial.print(0x80, BYTE);    //start byte
  softSerial.print(0x01, BYTE);    //device id
  softSerial.print(0x05, BYTE);    //command number
  softSerial.print(servo, BYTE);   //servo number
  softSerial.print(pos_hi, BYTE);  //bits 8 thru 13
  softSerial.print(pos_low, BYTE); //bottom 7 bits
}