i2c: initial implementation for PCA9685 servo driver

Signed-off-by: deadprogram <[email protected]>

drivers/i2c/pca9685_driver.go

@@ -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))
+}

drivers/i2c/pca9685_driver_test.go

@@ -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")
+}