From b2694db62bafe5705424e19ebb337e578a5f287b Mon Sep 17 00:00:00 2001 From: deadprogram Date: Fri, 23 Jun 2017 20:13:28 +0200 Subject: [PATCH] i2c: initial implementation for PCA9685 servo driver Signed-off-by: deadprogram --- drivers/i2c/pca9685_driver.go | 168 +++++++++++++++++++++++++++++ drivers/i2c/pca9685_driver_test.go | 96 +++++++++++++++++ 2 files changed, 264 insertions(+) create mode 100644 drivers/i2c/pca9685_driver.go create mode 100644 drivers/i2c/pca9685_driver_test.go diff --git a/drivers/i2c/pca9685_driver.go b/drivers/i2c/pca9685_driver.go new file mode 100644 index 00000000..47b6580a --- /dev/null +++ b/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)) +} diff --git a/drivers/i2c/pca9685_driver_test.go b/drivers/i2c/pca9685_driver_test.go new file mode 100644 index 00000000..cf9e947b --- /dev/null +++ b/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") +}