90 lines
1.4 KiB
C
90 lines
1.4 KiB
C
|
//------------------------------------------------------------------
|
||
|
|
// 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
|
||
|
|
|
||
|
|
|
||
|
|
|