/* A very simply program to write to servo registers on a Devantech
   SD21 servo controller connected via their USB-to-I2C convertor. 

   Information about the hardware:
   http://www.robot-electronics.co.uk/htm/usb_i2c_tech.htm
   http://www.robot-electronics.co.uk/htm/sd21tech.htm

   You'll need a driver for the USB-to-I2C convertor.  They're available here:
   http://www.ftdichip.com/FTDrivers.htm

   This code assumes the VCP driver, and was written for MacOS X.  It
   should run on Linux too, though you'll probably have to change the
   serial device name.
   
   Change the device name used in the open call below to the one from
   your hardware.
*/

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <termios.h>

main(int argc, char **argv)
{
  int fd, i, written, reg, value;
  char cmd[80];
  char buf[80];
  struct termios t;

  /* check the user typed two arguments */
  if (argc != 3) {
    fprintf(stderr, "Usage: servo-set <register> <value>\n");
    exit(1);
  }

  /* check the register value is legal */
  reg = atoi(argv[1]);
  if (reg < 0 || reg > 80) {
    fprintf(stderr, "Bad reg number %d\n", reg);
    exit(1);
  }

  /* only values between 0 and 255 make much sense */
  value = atoi(argv[2]);

  /* open the serial device.  The precise device name depends on the
     serial number of the USB-to-I2C convertor.  You'll need to change
     this to whatever device appears on your computer when you plug
     in the device */
  fd = open("/dev/cu.usbserial-A2001ngD", O_RDWR);
  if (fd < 0) {
    fprintf(stderr, "Failed to open device\n");
  }

  /* Set the bitrate of the virtual serial port */
  tcgetattr(fd, &t);
  cfsetspeed(&t, 19200);
  tcsetattr(fd, TCSANOW, &t);

  /* Create the command to send to the device.  This trivial code only
     sends single byte values */
  cmd[0]=0x55; /* The code to write multiple bytes to single-byte
		  addressed devices */
  cmd[1]=0xC2; /* The I2C bus ID of the SD21.  Some versions use 0xC3 instead */
  cmd[2]=reg;  /* The register we want to set */
  cmd[3]=0x01; /* The number of bytes in the command */
  cmd[4]=value; /* The value to write to the register */

  /* Actually do the write */
  written = write(fd, cmd, 5);
  if (written < 5) {
    fprintf(stderr, "Write failed\n");
    exit(1);
  }
  
  exit(0);
}