#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
// setting servo controller numbers by Oscar G. Torres =)


#define rxPin 4
#define txPin 5

int ct=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);

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

///// set up the servo numbers, only do this once per servo controller. This code is sets controller three.
softSerial.print(0x80,BYTE);    // Start byte to change the settings of the pololu servo controller
softSerial.print(0x02,BYTE);    // Setting to change servo numbers
softSerial.print(0x01,BYTE);    // New # setting from 0 to 15 (hexadecimal 0x00, 0x01, 0x02... to 0x0F)
///// after you upload servo numbers, delete or comment out the three lines of code above. 

// set initial servo speeds
 set_speed(8, 10);
 //set and move servos to neutral positions using command 5
 set_neutral(8, 2500);
 //initialize servos using command 0
 set_servo_parameters(8, 15);
}

void loop()
{
  //// Make servo number 16 (aka servo 0 on third servo controller) move
  position_absolute(8, 2500);
  delay(1000);
  position_absolute(8, 2000);
  delay(1000);
  
}

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
}