//------------------------------------------------------------------
// qfixServoBoard4.h
//
// This file contains the class ServoBoard4 which represents
// four servos attached to the ServoBoard4.
//
// Copyright 2006 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------

#ifndef qfixServoBoard4_h
#define qfixServoBoard4_h


#include "qfixI2C.h"
#include "qfixI2CDefs.h"
#include "string.h"

static const uint8_t I2C_ADDR_SERVO = 3;

static const uint8_t SERVO_CMD_CHANGEID = 0;
static const uint8_t SERVO_CMD_SET      = 1;


class ServoBoard4
{
private:
  I2C i2c;
  uint8_t id;

public:
  /** Constructor with default ID 0
  */
  ServoBoard4();

  /** Constructor with given ID
  */
  ServoBoard4(uint8_t ID);

  /** Change logical device ID
  */
  void changeID(uint8_t newID);

  /** 
  */
  void set(uint8_t index, uint8_t pwm);
};



ServoBoard4::ServoBoard4() : i2c(), id(0)
{
}


ServoBoard4::ServoBoard4(uint8_t ID) : i2c(), id(ID)
{
}


void ServoBoard4::set(uint8_t index, uint8_t pwm)
{
  uint8_t buf[5];

  buf[0] = id;
  buf[1] = SERVO_CMD_SET;
  buf[2] = index;
  buf[3] = pwm;
  i2c.send(I2C_ADDR_SERVO, buf, 4);
}


void ServoBoard4::changeID(uint8_t newID)
{
  uint8_t buf[3];

  buf[0] = id;
  buf[1] = SERVO_CMD_CHANGEID;
  buf[2] = newID;
  i2c.send(I2C_ADDR_SERVO, buf, 3);

  id = newID;
}


#endif