/******************************************************************************
Servo file

Michael Simmons
Master Project:
Date Created:	25 Oct 2008
Date Modified:	25 Oct 2008

*******************************************************************************

Description:
	The Servo class is designed to control servos from the PWM pins on the
        robostix.

******************************************************************************/

#include "Servo.h"

// Constructor

Servo::Servo( int handle, int low, int high, int pwm )
{
//  i2cLAddr = I2cLine::init();
  i2cHandle = handle;
  lowVal = low;
  highVal = high;
  centerVal = ( low + high ) / 2;
  rangeVal = ( high - low ) / 2;
  servoNum = pwm;

  avrAddr = 0x0b;
//  portd = 0x12 + 0x20;
//  regValue = 127;
  msg[0].addr = avrAddr;
  msg[0].flags = 0;
  msg[0].len = sizeof(writeReg) + 2;
  msg[0].buf = (char *)&buf[0];

  rdwr.msgs = msg;
  rdwr.nmsgs = 1;

  buf[0] = 2; //I2C_WRITE_IO;
  buf[1] = sizeof(writeReg);
//  buf[2] = portd;

  center();

} //Servo

int Servo::center()
{
//  buf[ 3 ] = 127;
  writeReg.reg = servoNum;
  writeReg.val = centerVal;

  memcpy( &buf[2], (char *) &writeReg, sizeof(writeReg) );

  if ( ioctl( i2cHandle, I2C_RDWR, &rdwr ) < 0 )
  {
    printf( "Error writing to register\n");
    exit ( 1 );
  }
} //Center

int Servo::left( int turnL )
{
  turnL = centerVal - ( turnL * rangeVal / 100 );
//  buf[ 3 ] = turnL;
  writeReg.reg = servoNum;
  writeReg.val = turnL;

  memcpy( &buf[2], (char *) &writeReg, sizeof(writeReg) );

  if ( ioctl( i2cHandle, I2C_RDWR, &rdwr ) < 0 )
  {
    printf( "Error writing to register\n");
    exit ( 1 );
  }
} //Left

int Servo::right( int turnR )
{
  turnR = centerVal + ( turnR * rangeVal / 100 );
//  buf[ 3 ] = turnR;
  writeReg.reg = servoNum;
  writeReg.val = turnR;

  memcpy( &buf[2], (char *) &writeReg, sizeof(writeReg) );

  if ( ioctl( i2cHandle, I2C_RDWR, &rdwr ) < 0 )
  {
    printf( "Error writing to register\n");
    exit ( 1 );
  }
} //Right