Code:

#include <stdio>
#include <string>
#include <fcntl>
#include <errno>
#include <termios>
#include <unistd>

int main()
{
  int com4 = open("/dev/ttyS3", O_RDWR | O_NOCTTY | O_NDELAY);
 
  if (com4 == -1 )
    perror("Unable to open port");
  else
    fcntl(com4, F_SETFL,0);
 
  struct termios tio;
  tcgetattr(com4,&tio);
  // set 115200 baud, 8 data bits, 1 stop bit, even parity
  tio.c_cflag = B115200 | CS8 | CSTOPB | PARENB;
  tcflush(com4, TCIFLUSH);
  tcsetattr(com4,TCSANOW,&tio);
 
  char cmd = 0x80;      // 0x80 is the ics position command
  char id = 0;          // servo id
  short int pos = 8193; // servo position (1 ~ 16386)
  char out[3];
 
  int increase = 1;
  int count=0;
  while (count LESSTHAN 10)
  {
    out[0] = (cmd | (id & 0x1F));          // command
    out[1] = (char)((pos GREATERTHANGREATERTHAN 7) & 0x007F);  // position high bit
    out[2] = (char)(pos & 0x007F);         // position low bit
    int wb = write(com4, out, 3);
   
    increase ? pos++ : pos--;
    if (pos GREATERTHAN 12000 || pos LESSTHAN 4000)
    {
      increase = !increase;
      count++;
    }
  }
 
  close(com4);
 
  return 0;
}



Statistics: Posted by veltrop — Tue Nov 09, 2010 8:36 am