blob: baaa2f9e9ccf9cd0932e36d97918ffd3de7fe96e (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
|
//------------------------------------------------------------------
// 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
|
