i2c: initial implementation for PCA9685 servo driver
Signed-off-by: deadprogram <ron@hybridgroup.com>
This commit is contained in:
deadprogram
parent
f16e2cdb04
commit
b2694db62b
|
|
@ -0,0 +1,168 @@
|
|||
package i2c
|
||||
|
||||
import (
|
||||
"strconv"
|
||||
"time"
|
||||
|
||||
"gobot.io/x/gobot"
|
||||
)
|
||||
|
||||
const pca9685Address = 0x40
|
||||
|
||||
const (
|
||||
PCA9685_MODE1 = 0x00
|
||||
PCA9685_PRESCALE = 0xFE
|
||||
PCA9685_SUBADR1 = 0x02
|
||||
PCA9685_SUBADR2 = 0x03
|
||||
PCA9685_SUBADR3 = 0x04
|
||||
PCA9685_LED0_ON_L = 0x06
|
||||
PCA9685_LED0_ON_H = 0x07
|
||||
PCA9685_LED0_OFF_L = 0x08
|
||||
PCA9685_LED0_OFF_H = 0x09
|
||||
PCA9685_ALLLED_ON_L = 0xFA
|
||||
PCA9685_ALLLED_ON_H = 0xFB
|
||||
PCA9685_ALLLED_OFF_L = 0xFC
|
||||
PCA9685_ALLLED_OFF_H = 0xFD
|
||||
)
|
||||
|
||||
type PCA9685Driver struct {
|
||||
name string
|
||||
connector Connector
|
||||
connection Connection
|
||||
Config
|
||||
}
|
||||
|
||||
// NewPCA9685Driver creates a new driver with specified i2c interface
|
||||
// Params:
|
||||
// conn Connector - the Adaptor to use with this Driver
|
||||
//
|
||||
// Optional params:
|
||||
// i2c.WithBus(int): bus to use with this driver
|
||||
// i2c.WithAddress(int): address to use with this driver
|
||||
//
|
||||
func NewPCA9685Driver(a Connector, options ...func(Config)) *PCA9685Driver {
|
||||
m := &PCA9685Driver{
|
||||
name: gobot.DefaultName("PCA9685"),
|
||||
connector: a,
|
||||
Config: NewConfig(),
|
||||
}
|
||||
|
||||
for _, option := range options {
|
||||
option(m)
|
||||
}
|
||||
|
||||
// TODO: add commands for API
|
||||
return m
|
||||
}
|
||||
|
||||
// Name returns the Name for the Driver
|
||||
func (h *PCA9685Driver) Name() string { return h.name }
|
||||
|
||||
// SetName sets the Name for the Driver
|
||||
func (h *PCA9685Driver) SetName(n string) { h.name = n }
|
||||
|
||||
// Connection returns the connection for the Driver
|
||||
func (h *PCA9685Driver) Connection() gobot.Connection { return h.connector.(gobot.Connection) }
|
||||
|
||||
// Start initializes the pca9685
|
||||
func (h *PCA9685Driver) Start() (err error) {
|
||||
bus := h.GetBusOrDefault(h.connector.GetDefaultBus())
|
||||
address := h.GetAddressOrDefault(pca9685Address)
|
||||
|
||||
h.connection, err = h.connector.GetConnection(address, bus)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if _, err := h.connection.Write([]byte{PCA9685_MODE1, 0x00}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if _, err := h.connection.Write([]byte{PCA9685_ALLLED_OFF_H, 0x10}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
// Halt returns true if devices is halted successfully
|
||||
func (h *PCA9685Driver) Halt() (err error) { return }
|
||||
|
||||
// SetPWM sets the channel to whichever pwm setting from 0-4096
|
||||
func (h *PCA9685Driver) SetPWM(channel int, on uint16, off uint16) (err error) {
|
||||
if _, err := h.connection.Write([]byte{byte(PCA9685_LED0_ON_L + 4*channel), byte(on), byte(on >> 8), byte(off), byte(off >> 8)}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
// if _, err := h.connection.Write([]byte{byte(PCA9685_LED0_ON_L + 4*channel), byte(on & 0xff)}); err != nil {
|
||||
// return err
|
||||
// }
|
||||
// if _, err := h.connection.Write([]byte{byte(PCA9685_LED0_ON_H + 4*channel), byte(on >> 8)}); err != nil {
|
||||
// return err
|
||||
// }
|
||||
// if _, err := h.connection.Write([]byte{byte(PCA9685_LED0_OFF_L + 4*channel), byte(off & 0xff)}); err != nil {
|
||||
// return err
|
||||
// }
|
||||
// if _, err := h.connection.Write([]byte{byte(PCA9685_LED0_OFF_H + 4*channel), byte(off >> 8)}); err != nil {
|
||||
// return err
|
||||
// }
|
||||
return
|
||||
}
|
||||
|
||||
func (h *PCA9685Driver) SetPWMFreq(freq float32) error {
|
||||
// Correct for overshoot in the frequency setting (see issue #11).
|
||||
freq *= 0.9
|
||||
|
||||
var prescalevel float32 = 25000000
|
||||
prescalevel /= 4096
|
||||
prescalevel /= freq
|
||||
prescalevel -= 1
|
||||
prescale := byte(prescalevel + 0.5)
|
||||
|
||||
if _, err := h.connection.Write([]byte{byte(PCA9685_MODE1)}); err != nil {
|
||||
return err
|
||||
}
|
||||
data := make([]byte, 1)
|
||||
oldmode, err := h.connection.Read(data)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
newmode := (oldmode & 0x7F) | 0x10 // sleep
|
||||
if _, err := h.connection.Write([]byte{byte(PCA9685_MODE1), byte(newmode)}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if _, err := h.connection.Write([]byte{byte(PCA9685_PRESCALE), prescale}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if _, err := h.connection.Write([]byte{byte(PCA9685_MODE1), byte(oldmode)}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
if _, err := h.connection.Write([]byte{byte(PCA9685_MODE1), byte(oldmode | 0xa1)}); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (h *PCA9685Driver) PwmWrite(pin string, val byte) (err error) {
|
||||
i, err := strconv.Atoi(pin)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
v := gobot.ToScale(gobot.FromScale(float64(val), 0, 255), 0, 4096)
|
||||
return h.SetPWM(i, 0, uint16(v))
|
||||
}
|
||||
|
||||
func (h *PCA9685Driver) ServoWrite(pin string, val byte) (err error) {
|
||||
i, err := strconv.Atoi(pin)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
v := gobot.ToScale(gobot.FromScale(float64(val), 0, 180), 200, 500)
|
||||
return h.SetPWM(i, 0, uint16(v))
|
||||
}
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
package i2c
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"strings"
|
||||
"testing"
|
||||
|
||||
"gobot.io/x/gobot"
|
||||
"gobot.io/x/gobot/drivers/gpio"
|
||||
"gobot.io/x/gobot/gobottest"
|
||||
)
|
||||
|
||||
// ensure that PCA9685Driver fulfills Gobot Driver interface
|
||||
var _ gobot.Driver = (*PCA9685Driver)(nil)
|
||||
|
||||
// and also the PwmWriter and ServoWriter interfaces
|
||||
var _ gpio.PwmWriter = (*PCA9685Driver)(nil)
|
||||
var _ gpio.ServoWriter = (*PCA9685Driver)(nil)
|
||||
|
||||
// --------- HELPERS
|
||||
func initTestPCA9685Driver() (driver *PCA9685Driver) {
|
||||
driver, _ = initTestPCA9685DriverWithStubbedAdaptor()
|
||||
return
|
||||
}
|
||||
|
||||
func initTestPCA9685DriverWithStubbedAdaptor() (*PCA9685Driver, *i2cTestAdaptor) {
|
||||
adaptor := newI2cTestAdaptor()
|
||||
return NewPCA9685Driver(adaptor), adaptor
|
||||
}
|
||||
|
||||
// --------- TESTS
|
||||
|
||||
func TestNewPCA9685Driver(t *testing.T) {
|
||||
// Does it return a pointer to an instance of PCA9685Driver?
|
||||
var pca interface{} = NewPCA9685Driver(newI2cTestAdaptor())
|
||||
_, ok := pca.(*PCA9685Driver)
|
||||
if !ok {
|
||||
t.Errorf("NewPCA9685Driver() should have returned a *PCA9685Driver")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPCA9685DriverName(t *testing.T) {
|
||||
pca := initTestPCA9685Driver()
|
||||
gobottest.Refute(t, pca.Connection(), nil)
|
||||
gobottest.Assert(t, strings.HasPrefix(pca.Name(), "PCA9685"), true)
|
||||
}
|
||||
|
||||
func TestPCA9685DriverOptions(t *testing.T) {
|
||||
pca := NewPCA9685Driver(newI2cTestAdaptor(), WithBus(2))
|
||||
gobottest.Assert(t, pca.GetBusOrDefault(1), 2)
|
||||
}
|
||||
|
||||
// Methods
|
||||
func TestPCA9685DriverStart(t *testing.T) {
|
||||
pca := initTestPCA9685Driver()
|
||||
|
||||
gobottest.Assert(t, pca.Start(), nil)
|
||||
}
|
||||
|
||||
func TestPCA9685DriverStartConnectError(t *testing.T) {
|
||||
d, adaptor := initTestPCA9685DriverWithStubbedAdaptor()
|
||||
adaptor.Testi2cConnectErr(true)
|
||||
gobottest.Assert(t, d.Start(), errors.New("Invalid i2c connection"))
|
||||
}
|
||||
|
||||
func TestPCA9685DriverStartWriteError(t *testing.T) {
|
||||
pca, adaptor := initTestPCA9685DriverWithStubbedAdaptor()
|
||||
adaptor.i2cWriteImpl = func([]byte) (int, error) {
|
||||
return 0, errors.New("write error")
|
||||
}
|
||||
gobottest.Assert(t, pca.Start(), errors.New("write error"))
|
||||
}
|
||||
|
||||
func TestPCA9685DriverHalt(t *testing.T) {
|
||||
pca := initTestPCA9685Driver()
|
||||
|
||||
gobottest.Assert(t, pca.Halt(), nil)
|
||||
}
|
||||
|
||||
// func TestPCA9685DriverWriteData(t *testing.T) {
|
||||
// pca, adaptor := initTestPCA9685DriverWithStubbedAdaptor()
|
||||
//
|
||||
// adaptor.i2cReadImpl = func(b []byte) (int, error) {
|
||||
// copy(b, []byte{0x00, 0x01, 0x02, 0x04})
|
||||
// return 4, nil
|
||||
// }
|
||||
// pca.Start()
|
||||
// mpu.GetData()
|
||||
// gobottest.Assert(t, mpu.Temperature, int16(36))
|
||||
// }
|
||||
|
||||
func TestPCA9685DriverSetName(t *testing.T) {
|
||||
pca := initTestPCA9685Driver()
|
||||
pca.SetName("TESTME")
|
||||
gobottest.Assert(t, pca.Name(), "TESTME")
|
||||
}
|
||||
Loading…
Reference in New Issue