servo_testing.cpp

// Time interval, this executes one piece of code from time to time
// Download Metro lybrary and instructions:
// http://www.arduino.cc/playground/Code/Metro
#include <Metro.h>
int duration = 3500; // duration of the interval in miliseconds
Metro intervaller = Metro (duration);

// servos minimum and maximum position
#define MIN_POS 2700 // the minuimum pulse width for your servos
#define MAX_POS 5000 // maximum pulse width for your servos

// variable to hold new destination positions
int d0; // destination0

// set servo speed, goes from 1 to 127
//int servoSpeed = 127;
int servoSpeed = 0; // turn off speed limiting
int del;

// setup runs once, when the sketch starts
void setup() 
{
  // initialize serial comunication
  Serial.begin(9600);

  // set servo pin and speed
  servoSetSpeed(0, servoSpeed);
  del = 0;
}

// main program loop, executes forever after setup()
void loop() 
{
  sorta_scale();
  /*
  moveServo(MIN_POS, del);
  delay(800);
  moveServo(MAX_POS, del);
  delay(800);
  moveServo(MIN_POS, del);
  delay(800);
  */
}

void sorta_scale() {
  for (d0=2700;d0 <= 5000;d0+=5) 
  {
    moveServo(d0, del);
    delay(700);
  }
}

void random_pos() {
  d0 = random(MIN_POS,MAX_POS);
  moveServo(d0, del);
  delay(1000);
}

void moveServo(int d0, int del) 
{
  put(0,d0);
  // delay to make the servo move (how fast servo moves, lower is faster).
  delay(del);
}


// functions from this forum topic:
// http://forum.pololu.com/viewtopic.php?f=16&t=745&start=60&st=0&sk=t&sd=a

void put(int servo, int angle)
{
  //servo is the servo number (typically 0-7)
  //angle is the absolute position from 500 to 5500

  unsigned char buff[6];

  unsigned int temp;
  unsigned char pos_hi,pos_low;

  //Convert the angle data into two 7-bit bytes
  temp=angle&0x1f80;
  pos_hi=temp>>7;
  pos_low=angle & 0x7f;

  //Construct a Pololu Protocol command sentence
  buff[0]=0x80; //start byte
  buff[1]=0x01; //device id
  buff[2]=0x04; //command number
  buff[3]=servo; //servo number
  buff[4]=pos_hi; //data1
  buff[5]=pos_low; //data2

  //Send the command to the servo controller
  for(int i=0;i<6;i++){
    Serial.print(buff[i],BYTE);
  }

}

void servoSetSpeed(int servo, int speed)
{
  //servo is the servo number (typically 0-7)
  //speed is servo speed (1=fastest, 127=slowest)
  //set speed to zero to turn off speed limiting

  unsigned char buff[5];
  unsigned char speedcmd;

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

  buff[0]=0x80;//start byte
  buff[1]=0x01;//device id
  buff[2]=0x01;//command number
  buff[3]=servo;//servo number
  buff[4]=speed;//data1

  for(int i=0;i<5;i++){
    Serial.print(buff[i],BYTE);
  }
}