forked from kidoman/embd
/
pca9685.go
265 lines (207 loc) · 6.58 KB
/
pca9685.go
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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
// Package pca9685 allows interfacing with the pca9685 16-channel, 12-bit PWM Controller through I2C protocol.
package pca9685
import (
"math"
"sync"
"time"
"github.com/golang/glog"
"github.com/zlowred/embd"
"github.com/zlowred/embd/util"
)
const (
clockFreq = 25000000
pwmControlPoints = 4096
mode1RegAddr = 0x00
preScaleRegAddr = 0xFE
pwm0OnLowReg = 0x6
// inspired by arduino's default freq for analogWrites
defaultFreq = 490
)
// PCA9685 represents a PCA9685 PWM generator.
type PCA9685 struct {
Bus embd.I2CBus
Addr byte
Freq int
initialized bool
mu sync.RWMutex
}
// New creates a new PCA9685 interface.
func New(bus embd.I2CBus, addr byte) *PCA9685 {
return &PCA9685{
Bus: bus,
Addr: addr,
}
}
func (d *PCA9685) mode1Reg() (byte, error) {
return d.Bus.ReadByteFromReg(d.Addr, mode1RegAddr)
}
func (d *PCA9685) setup() error {
d.mu.RLock()
if d.initialized {
d.mu.RUnlock()
return nil
}
d.mu.RUnlock()
d.mu.Lock()
defer d.mu.Unlock()
mode1Reg, err := d.mode1Reg()
if err != nil {
return err
}
glog.V(1).Infof("pca9685: read MODE1 Reg [regAddr: %#02x] Value: [%v]", mode1RegAddr, mode1Reg)
if err := d.sleep(); err != nil {
return err
}
if d.Freq == 0 {
d.Freq = defaultFreq
}
preScaleValue := byte(math.Floor(float64(clockFreq/(pwmControlPoints*d.Freq))+float64(0.5)) - 1)
glog.V(1).Infof("pca9685: calculated prescale value = %#02x", preScaleValue)
if err := d.Bus.WriteByteToReg(d.Addr, preScaleRegAddr, byte(preScaleValue)); err != nil {
return err
}
glog.V(1).Infof("pca9685: prescale value [%#02x] written to PRE_SCALE Reg [regAddr: %#02x]", preScaleValue, preScaleRegAddr)
if err := d.wake(); err != nil {
return err
}
newmode := ((mode1Reg | 0x01) & 0xDF)
if err := d.Bus.WriteByteToReg(d.Addr, mode1RegAddr, newmode); err != nil {
return err
}
glog.V(1).Infof("pca9685: new mode [%#02x] [disabling register auto increment] written to MODE1 Reg [regAddr: %#02x]", newmode, mode1RegAddr)
d.initialized = true
glog.V(1).Infof("pca9685: driver initialized with pwm freq: %v", d.Freq)
return nil
}
// SetPwm sets the ON and OFF time registers for pwm signal shaping.
// channel: 0-15
// onTime/offTime: 0-4095
func (d *PCA9685) SetPwm(channel, onTime, offTime int) error {
if err := d.setup(); err != nil {
return err
}
onTimeLowReg := byte(pwm0OnLowReg + (4 * channel))
onTimeLow := byte(onTime & 0xFF)
onTimeHigh := byte(onTime >> 8)
offTimeLow := byte(offTime & 0xFF)
offTimeHigh := byte(offTime >> 8)
if err := d.Bus.WriteByteToReg(d.Addr, onTimeLowReg, onTimeLow); err != nil {
return err
}
glog.V(2).Infof("pca9685: writing on-time low [%#02x] to CHAN%v_ON_L reg [reg: %#02x]", onTimeLow, channel, onTimeLowReg)
onTimeHighReg := onTimeLowReg + 1
if err := d.Bus.WriteByteToReg(d.Addr, onTimeHighReg, onTimeHigh); err != nil {
return err
}
glog.V(2).Infof("pca9685: writing on-time high [%#02x] to CHAN%v_ON_H reg [reg: %#02x]", onTimeHigh, channel, onTimeHighReg)
offTimeLowReg := onTimeHighReg + 1
if err := d.Bus.WriteByteToReg(d.Addr, offTimeLowReg, offTimeLow); err != nil {
return err
}
glog.V(2).Infof("pca9685: writing off-time low [%#02x] to CHAN%v_OFF_L reg [reg: %#02x]", offTimeLow, channel, offTimeLowReg)
offTimeHighReg := offTimeLowReg + 1
if err := d.Bus.WriteByteToReg(d.Addr, offTimeHighReg, offTimeHigh); err != nil {
return err
}
glog.V(2).Infof("pca9685: writing off-time high [%#02x] to CHAN%v_OFF_H reg [reg: %#02x]", offTimeHigh, channel, offTimeHighReg)
return nil
}
type pwmChannel struct {
d *PCA9685
channel int
}
func (p *pwmChannel) SetMicroseconds(us int) error {
return p.d.setMicroseconds(p.channel, us)
}
// SetAnalog is a convinience method which allows easy manipulation of the PWM
// based on a (0-255) range value.
func (p *pwmChannel) SetAnalog(value byte) error {
offTime := util.Map(int64(value), 0, 255, 0, pwmControlPoints-1)
return p.d.SetPwm(p.channel, 0, int(offTime))
}
func (d *PCA9685) ServoChannel(channel int) *pwmChannel {
return &pwmChannel{d: d, channel: channel}
}
func (d *PCA9685) AnalogChannel(channel int) *pwmChannel {
return &pwmChannel{d: d, channel: channel}
}
// SetMicroseconds is a convinience method which allows easy servo control.
func (d *PCA9685) setMicroseconds(channel, us int) error {
offTime := us * d.Freq * pwmControlPoints / 1000000
return d.SetPwm(channel, 0, offTime)
}
// Close stops the controller and resets mode and pwm controller registers.
func (d *PCA9685) Close() error {
if err := d.setup(); err != nil {
return err
}
if err := d.sleep(); err != nil {
return err
}
glog.V(1).Infof("pca9685: reset request received")
if err := d.Bus.WriteByteToReg(d.Addr, mode1RegAddr, 0x00); err != nil {
return err
}
glog.V(1).Infof("pca9685: cleaning up all PWM control registers")
for regAddr := 0x06; regAddr <= 0x45; regAddr++ {
if err := d.Bus.WriteByteToReg(d.Addr, byte(regAddr), 0x00); err != nil {
return err
}
}
if glog.V(1) {
glog.Infof("pca9685: done Cleaning up all PWM control registers")
glog.Infof("pca9685: controller reset")
}
return nil
}
func (d *PCA9685) sleep() error {
glog.V(1).Infof("pca9685: sleep request received")
mode1Reg, err := d.mode1Reg()
if err != nil {
return err
}
sleepmode := (mode1Reg & 0x7F) | 0x10
if err := d.Bus.WriteByteToReg(d.Addr, mode1RegAddr, sleepmode); err != nil {
return err
}
if glog.V(1) {
glog.Infof("pca9685: sleep mode [%#02x] written to MODE1 Reg [regAddr: %#02x]", sleepmode, mode1RegAddr)
glog.Infoln("pca9685: controller set to Sleep mode")
}
return nil
}
// Sleep puts the controller in sleep mode. Does not change the pwm control registers.
func (d *PCA9685) Sleep() error {
if err := d.setup(); err != nil {
return err
}
return d.sleep()
}
func (d *PCA9685) wake() error {
glog.V(1).Infoln("pca9685: wake request received")
mode1Reg, err := d.mode1Reg()
if err != nil {
return err
}
wakeMode := mode1Reg & 0xEF
if (mode1Reg & 0x80) == 0x80 {
if err := d.Bus.WriteByteToReg(d.Addr, mode1RegAddr, wakeMode); err != nil {
return err
}
glog.V(1).Infof("pca9685: wake mode [%#02x] written to MODE1 Reg [regAddr: %#02x]", wakeMode, mode1RegAddr)
time.Sleep(500 * time.Microsecond)
}
restartOpCode := wakeMode | 0x80
if err := d.Bus.WriteByteToReg(d.Addr, mode1RegAddr, restartOpCode); err != nil {
return err
}
glog.V(1).Infof("pca9685: restart mode [%#02x] written to MODE1 Reg [regAddr: %#02x]", restartOpCode, mode1RegAddr)
return nil
}
// Wake allows the controller to exit sleep mode and resume with PWM generation.
func (d *PCA9685) Wake() error {
if err := d.setup(); err != nil {
return err
}
return d.wake()
}