mirror of
https://github.com/kidoman/embd
synced 2024-11-12 04:28:53 +01:00
- Added pca9685 package 16-channel, 12-bit PWM controller with I2C
- Added servo package to control servos using pca9685 package
This commit is contained in:
Kunal Powar
parent
1a6f97f102
commit
e7503cfcd7
285
controller/pca9685/pca9685.go
Normal file
285
controller/pca9685/pca9685.go
Normal file
@ -0,0 +1,285 @@
|
||||
// Package pca9685 allows interfacing with the pca9685 16-channel, 12-bit PWM Controller through I2C protocol.
|
||||
package pca9685
|
||||
|
||||
import (
|
||||
"log"
|
||||
"math"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"github.com/kid0m4n/go-rpi/i2c"
|
||||
)
|
||||
|
||||
const (
|
||||
clockFreq = 25000000
|
||||
pwmControlPoints = 4096
|
||||
|
||||
mode1RegAddr = 0x00
|
||||
preScaleRegAddr = 0xFE
|
||||
|
||||
pwm0OnLowReg = 0x6
|
||||
)
|
||||
|
||||
// A PCA9685 interface implements access to the controller.
|
||||
type PCA9685 interface {
|
||||
// SetPwm sets the ON and OFF time registers for pwm signal shaping.
|
||||
// n: channel 0-15
|
||||
// onTime/offTime: 0-4095
|
||||
SetPwm(n int, onTime int, offTime int) error
|
||||
|
||||
// Wake allows the controller to exit sleep mode and resume with PWM generation.
|
||||
Wake() error
|
||||
|
||||
// Sleep puts the controller in sleep mode. Does not change the pwm control registers.
|
||||
Sleep() error
|
||||
|
||||
// Close stops the controller and resets mode and pwm controller registers.
|
||||
Close() error
|
||||
|
||||
// SetDebug is used to enable logging (debug mode).
|
||||
SetDebug(status bool)
|
||||
}
|
||||
|
||||
type pca9685 struct {
|
||||
bus i2c.Bus
|
||||
addr byte
|
||||
freq int
|
||||
|
||||
initialized bool
|
||||
mu sync.RWMutex
|
||||
|
||||
debug bool
|
||||
}
|
||||
|
||||
// The pca9685 controller supports pwm frequencies from 40Hz to 1000Hz
|
||||
|
||||
// New creates a new PCA9685 interface.
|
||||
func New(bus i2c.Bus, addr byte, freq int) PCA9685 {
|
||||
return &pca9685{bus: bus, addr: addr, freq: freq}
|
||||
}
|
||||
|
||||
// SetDebug is used to enable logging (debug mode).
|
||||
func (d *pca9685) SetDebug(status bool) {
|
||||
d.debug = status
|
||||
}
|
||||
|
||||
func (d *pca9685) mode1Reg() (byte, error) {
|
||||
return d.bus.ReadByteFromReg(d.addr, mode1RegAddr)
|
||||
}
|
||||
|
||||
func (d *pca9685) setup() (err error) {
|
||||
d.mu.RLock()
|
||||
if d.initialized {
|
||||
d.mu.RUnlock()
|
||||
return
|
||||
}
|
||||
d.mu.RUnlock()
|
||||
|
||||
d.mu.Lock()
|
||||
defer d.mu.Unlock()
|
||||
|
||||
preScaleValue := byte(math.Floor(float64(clockFreq/(pwmControlPoints*d.freq))+float64(0.5)) - 1)
|
||||
if d.debug {
|
||||
log.Printf("pca9685: calculated Prescale value = %#02x", preScaleValue)
|
||||
}
|
||||
|
||||
mode1Reg, err := d.mode1Reg()
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: read MODE1 Reg [regAddr: %#02x] Value: [%v]", mode1RegAddr, mode1Reg)
|
||||
}
|
||||
|
||||
if err = d.sleep(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
if err = d.bus.WriteToReg(d.addr, preScaleRegAddr, byte(preScaleValue)); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: prescale value [%#02x] written to PRE_SCALE Reg [regAddr: %#02x]", preScaleValue, preScaleRegAddr)
|
||||
}
|
||||
|
||||
if err = d.wake(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
newmode := ((mode1Reg | 0x01) & 0xDF)
|
||||
if err = d.bus.WriteToReg(d.addr, mode1RegAddr, newmode); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: new mode [%#02x] [disabling register auto increment] written to MODE1 Reg [regAddr: %#02x]", newmode, mode1RegAddr)
|
||||
}
|
||||
|
||||
d.initialized = true
|
||||
if d.debug {
|
||||
log.Printf("pca9685: driver initialized with pwm freq: %v", d.freq)
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
// SetPwm sets the ON and OFF time registers for pwm signal shaping.
|
||||
// channel: 0-15
|
||||
// onTime/offTime: 0-4095
|
||||
func (d *pca9685) SetPwm(n, onTime, offTime int) (err error) {
|
||||
if err = d.setup(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
onTimeLowReg := byte(pwm0OnLowReg + (4 * n))
|
||||
|
||||
onTimeLow := byte(onTime & 0xFF)
|
||||
onTimeHigh := byte(onTime >> 8)
|
||||
offTimeLow := byte(offTime & 0xFF)
|
||||
offTimeHigh := byte(offTime >> 8)
|
||||
|
||||
if err = d.bus.WriteToReg(d.addr, onTimeLowReg, onTimeLow); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: writing On-Time Low [%#02x] to CHAN%v_ON_L reg [RegAddr: %#02x]", onTimeLow, n, onTimeLowReg)
|
||||
}
|
||||
|
||||
onTimeHighReg := onTimeLowReg + 1
|
||||
if err = d.bus.WriteToReg(d.addr, onTimeHighReg, onTimeHighReg); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: writing On-Time High [%#02x] to CHAN%v_ON_H reg [RegAddr: %#02x]", onTimeHigh, n, onTimeHighReg)
|
||||
}
|
||||
|
||||
offTimeLowReg := onTimeHighReg + 1
|
||||
if err = d.bus.WriteToReg(d.addr, offTimeLowReg, offTimeLow); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: writing Off-Time Low [%#02x] to CHAN%v_OFF_L reg [RegAddr: %#02x]", offTimeLow, n, offTimeLowReg)
|
||||
}
|
||||
|
||||
offTimeHighReg := offTimeLowReg + 1
|
||||
if err = d.bus.WriteToReg(d.addr, offTimeHighReg, offTimeHigh); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: writing Off-Time High [%#02x] to CHAN%v_OFF_H reg [RegAddr: %#02x]", offTimeHigh, n, offTimeHighReg)
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
// Close stops the controller and resets mode and pwm controller registers.
|
||||
func (d *pca9685) Close() (err error) {
|
||||
if err = d.setup(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
if err = d.sleep(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
if d.debug {
|
||||
log.Println("pca9685: reset request received")
|
||||
}
|
||||
|
||||
if err = d.bus.WriteToReg(d.addr, mode1RegAddr, 0x00); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
if d.debug {
|
||||
log.Printf("pca9685: cleaning up all PWM control registers")
|
||||
}
|
||||
|
||||
for regAddr := 0x0; regAddr <= 0x45; regAddr++ {
|
||||
if err = d.bus.WriteToReg(d.addr, byte(regAddr), 0x00); err != nil {
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
if d.debug {
|
||||
log.Printf("pca9685: done Cleaning up all PWM control registers")
|
||||
}
|
||||
|
||||
if d.debug {
|
||||
log.Println("pca9685: controller reset")
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func (d *pca9685) sleep() (err error) {
|
||||
if d.debug {
|
||||
log.Println("pca9685: sleep request received")
|
||||
}
|
||||
|
||||
mode1Reg, err := d.mode1Reg()
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
sleepmode := (mode1Reg & 0x7F) | 0x10
|
||||
if err = d.bus.WriteToReg(d.addr, mode1RegAddr, sleepmode); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: sleep mode [%#02x] written to MODE1 Reg [regAddr: %#02x]", sleepmode, mode1RegAddr)
|
||||
}
|
||||
|
||||
if d.debug {
|
||||
log.Println("pca9685: controller set to Sleep mode")
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
// Sleep puts the controller in sleep mode. Does not change the pwm control registers.
|
||||
func (d *pca9685) Sleep() (err error) {
|
||||
if err = d.setup(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
return d.sleep()
|
||||
}
|
||||
|
||||
func (d *pca9685) wake() (err error) {
|
||||
if d.debug {
|
||||
log.Println("pca9685: wake request received")
|
||||
}
|
||||
|
||||
mode1Reg, err := d.mode1Reg()
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
wakeMode := mode1Reg & 0xEF
|
||||
if (mode1Reg & 0x80) == 0x80 {
|
||||
if err = d.bus.WriteToReg(d.addr, mode1RegAddr, wakeMode); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: wake mode [%#02x] written to MODE1 Reg [regAddr: %#02x]", wakeMode, mode1RegAddr)
|
||||
}
|
||||
|
||||
time.Sleep(500 * time.Microsecond)
|
||||
}
|
||||
|
||||
restartOpCode := wakeMode | 0x80
|
||||
if err = d.bus.WriteToReg(d.addr, mode1RegAddr, restartOpCode); err != nil {
|
||||
return
|
||||
}
|
||||
if d.debug {
|
||||
log.Printf("pca9685: restart mode [%#02x] written to MODE1 Reg [regAddr: %#02x]", restartOpCode, mode1RegAddr)
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
// Wake allows the controller to exit sleep mode and resume with PWM generation.
|
||||
func (d *pca9685) Wake() (err error) {
|
||||
if err = d.setup(); err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
return d.wake()
|
||||
}
|
||||
61
motion/servo/servo.go
Normal file
61
motion/servo/servo.go
Normal file
@ -0,0 +1,61 @@
|
||||
// Package servo allows control of servos using a PWM controller.
|
||||
package servo
|
||||
|
||||
import (
|
||||
"log"
|
||||
)
|
||||
|
||||
// A PWM interface implements access to a pwm controller.
|
||||
type PWM interface {
|
||||
SetPwm(n int, onTime int, offTime int) error
|
||||
}
|
||||
|
||||
// A SERVO interface implements access to the servo.
|
||||
type Servo interface {
|
||||
SetAngle(angle int) error
|
||||
|
||||
SetDebug(v bool)
|
||||
}
|
||||
|
||||
type servo struct {
|
||||
pwm PWM
|
||||
freq int
|
||||
n int
|
||||
|
||||
minms, maxms float64
|
||||
|
||||
debug bool
|
||||
}
|
||||
|
||||
// New creates a new SERVO interface.
|
||||
// pwm: instance of a PWM controller.
|
||||
// freq: Frequency of pwm signal (typically 50Hz)
|
||||
// n: PWM channel of the pwm controller to be used
|
||||
// minms: Pulse width corresponding to servo position of 0deg
|
||||
// maxms: Pulse width corresponding to servo position of 180deg
|
||||
func New(pwm PWM, freq int, n int, minms, maxms float64) Servo {
|
||||
return &servo{
|
||||
pwm: pwm,
|
||||
freq: freq,
|
||||
n: n,
|
||||
minms: minms,
|
||||
maxms: maxms,
|
||||
}
|
||||
}
|
||||
|
||||
// SetDebug is used to enable logging (debug mode).
|
||||
func (s *servo) SetDebug(v bool) {
|
||||
s.debug = v
|
||||
}
|
||||
|
||||
// SetAngle sets the servo angle.
|
||||
func (s *servo) SetAngle(angle int) error {
|
||||
ms := s.minms + float64(angle)*(s.maxms-s.minms)/180
|
||||
offTime := int(ms * float64(s.freq) * 4096 / 1000)
|
||||
|
||||
if s.debug {
|
||||
log.Printf("servo: given angle %v calculated ms %2.2f offTime %v", angle, ms, offTime)
|
||||
}
|
||||
|
||||
return s.pwm.SetPwm(s.n, 0, offTime)
|
||||
}
|
||||
46
samples/pca9685.go
Normal file
46
samples/pca9685.go
Normal file
@ -0,0 +1,46 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"log"
|
||||
"os"
|
||||
"os/signal"
|
||||
"time"
|
||||
|
||||
"github.com/kid0m4n/go-rpi/controller/pca9685"
|
||||
"github.com/kid0m4n/go-rpi/i2c"
|
||||
)
|
||||
|
||||
func main() {
|
||||
bus, err := i2c.NewBus(1)
|
||||
if err != nil {
|
||||
log.Panic(err)
|
||||
}
|
||||
|
||||
pca9685 := pca9685.New(bus, 0x41, 1000)
|
||||
pca9685.SetDebug(true)
|
||||
defer pca9685.Close()
|
||||
|
||||
if err := pca9685.SetPwm(15, 0, 1000); err != nil {
|
||||
log.Panic(err)
|
||||
}
|
||||
|
||||
c := make(chan os.Signal, 1)
|
||||
signal.Notify(c, os.Interrupt, os.Kill)
|
||||
|
||||
timer := time.Tick(2 * time.Second)
|
||||
sleeping := false
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-timer:
|
||||
sleeping = !sleeping
|
||||
if sleeping {
|
||||
pca9685.Sleep()
|
||||
} else {
|
||||
pca9685.Wake()
|
||||
}
|
||||
case <-c:
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
51
samples/servo.go
Normal file
51
samples/servo.go
Normal file
@ -0,0 +1,51 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"log"
|
||||
"os"
|
||||
"os/signal"
|
||||
"time"
|
||||
|
||||
"github.com/kid0m4n/go-rpi/controller/pca9685"
|
||||
"github.com/kid0m4n/go-rpi/i2c"
|
||||
"github.com/kid0m4n/go-rpi/motion/servo"
|
||||
)
|
||||
|
||||
func main() {
|
||||
bus, err := i2c.NewBus(1)
|
||||
if err != nil {
|
||||
log.Panic(err)
|
||||
}
|
||||
|
||||
pwm := pca9685.New(bus, 0x42, 50)
|
||||
defer pwm.Close()
|
||||
|
||||
servo := servo.New(pwm, 50, 0, 1, 2.5)
|
||||
servo.SetDebug(true)
|
||||
|
||||
c := make(chan os.Signal, 1)
|
||||
signal.Notify(c, os.Interrupt, os.Kill)
|
||||
|
||||
turnTimer := time.Tick(500 * time.Millisecond)
|
||||
left := true
|
||||
|
||||
servo.SetAngle(90)
|
||||
defer func() {
|
||||
servo.SetAngle(90)
|
||||
}()
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-turnTimer:
|
||||
left = !left
|
||||
switch left {
|
||||
case true:
|
||||
servo.SetAngle(70)
|
||||
case false:
|
||||
servo.SetAngle(110)
|
||||
}
|
||||
case <-c:
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user