diff --git a/drivers/i2c/adxl345_driver.go b/drivers/i2c/adxl345_driver.go new file mode 100644 index 000000000..82b3ccc78 --- /dev/null +++ b/drivers/i2c/adxl345_driver.go @@ -0,0 +1,328 @@ +package i2c + +import ( + "encoding/binary" + + "github.com/pkg/errors" + "gobot.io/x/gobot" +) + +const ADXL345AddressLow = 0x53 +const ADXL345AddressHigh = 0x1D + +const ( + // Data rate + ADXL345_RATE_3200HZ = 0x0F // 3200 Hz + ADXL345_RATE_1600HZ = 0x0E // 1600 Hz + ADXL345_RATE_800HZ = 0x0D // 800 Hz + ADXL345_RATE_400HZ = 0x0C // 400 Hz + ADXL345_RATE_200HZ = 0x0B // 200 Hz + ADXL345_RATE_100HZ = 0x0A // 100 Hz + ADXL345_RATE_50HZ = 0x09 // 50 Hz + ADXL345_RATE_25HZ = 0x08 // 25 Hz + ADXL345_RATE_12_5HZ = 0x07 // 12.5 Hz + ADXL345_RATE_6_25HZ = 0x06 // 6.25 Hz + ADXL345_RATE_3_13HZ = 0x05 // 3.13 Hz + ADXL345_RATE_1_56HZ = 0x04 // 1.56 Hz + ADXL345_RATE_0_78HZ = 0x03 // 0.78 Hz + ADXL345_RATE_0_39HZ = 0x02 // 0.39 Hz + ADXL345_RATE_0_20HZ = 0x01 // 0.20 Hz + ADXL345_RATE_0_10HZ = 0x00 // 0.10 Hz + + // Data range + ADXL345_RANGE_2G = 0x00 // +-2 g + ADXL345_RANGE_4G = 0x01 // +-4 g + ADXL345_RANGE_8G = 0x02 // +-8 g + ADXL345_RANGE_16G = 0x03 // +-16 g) + + ADXL345_REG_DEVID = 0x00 // R, 11100101, Device ID + ADXL345_REG_THRESH_TAP = 0x1D // R/W, 00000000, Tap threshold + ADXL345_REG_OFSX = 0x1E // R/W, 00000000, X-axis offset + ADXL345_REG_OFSY = 0x1F // R/W, 00000000, Y-axis offset + ADXL345_REG_OFSZ = 0x20 // R/W, 00000000, Z-axis offset + ADXL345_REG_DUR = 0x21 // R/W, 00000000, Tap duration + ADXL345_REG_LATENT = 0x22 // R/W, 00000000, Tap latency + ADXL345_REG_WINDOW = 0x23 // R/W, 00000000, Tap window + ADXL345_REG_THRESH_ACT = 0x24 // R/W, 00000000, Activity threshold + ADXL345_REG_THRESH_INACT = 0x25 // R/W, 00000000, Inactivity threshold + ADXL345_REG_TIME_INACT = 0x26 // R/W, 00000000, Inactivity time + ADXL345_REG_ACT_INACT_CTL = 0x27 // R/W, 00000000, Axis enable control for activity and inactiv ity detection + ADXL345_REG_THRESH_FF = 0x28 // R/W, 00000000, Free-fall threshold + ADXL345_REG_TIME_FF = 0x29 // R/W, 00000000, Free-fall time + ADXL345_REG_TAP_AXES = 0x2A // R/W, 00000000, Axis control for single tap/double tap + ADXL345_REG_ACT_TAP_STATUS = 0x2B // R, 00000000, Source of single tap/double tap + ADXL345_REG_BW_RATE = 0x2C // R/W, 00001010, Data rate and power mode control + ADXL345_REG_POWER_CTL = 0x2D // R/W, 00000000, Power-saving features control + ADXL345_REG_INT_ENABLE = 0x2E // R/W, 00000000, Interrupt enable control + ADXL345_REG_INT_MAP = 0x2F // R/W, 00000000, Interrupt mapping control + ADXL345_REG_INT_SOUCE = 0x30 // R, 00000010, Source of interrupts + ADXL345_REG_DATA_FORMAT = 0x31 // R/W, 00000000, Data format control + ADXL345_REG_DATAX0 = 0x32 // R, 00000000, X-Axis Data 0 + ADXL345_REG_DATAX1 = 0x33 // R, 00000000, X-Axis Data 1 + ADXL345_REG_DATAY0 = 0x34 // R, 00000000, Y-Axis Data 0 + ADXL345_REG_DATAY1 = 0x35 // R, 00000000, Y-Axis Data 1 + ADXL345_REG_DATAZ0 = 0x36 // R, 00000000, Z-Axis Data 0 + ADXL345_REG_DATAZ1 = 0x37 // R, 00000000, Z-Axis Data 1 + ADXL345_REG_FIFO_CTL = 0x38 // R/W, 00000000, FIFO control + ADXL345_REG_FIFO_STATUS = 0x39 // R, 00000000, FIFO status +) + +// ADXL345Driver is the gobot driver for the digital accelerometer ADXL345 +// +// Datasheet EN: http://www.analog.com/media/en/technical-documentation/data-sheets/ADXL345.pdf +// Datasheet JP: http://www.analog.com/media/jp/technical-documentation/data-sheets/ADXL345_jp.pdf +// +// Ported from the Arduino driver https://github.com/jakalada/Arduino-ADXL345 +type ADXL345Driver struct { + name string + connector Connector + connection Connection + + powerCtl adxl345PowerCtl + dataFormat adxl345DataFormat + bwRate adxl345BwRate + + x, y, z float64 + rawX, rawY, rawZ int16 + + Config +} + +// Internal structure for the power configuration +type adxl345PowerCtl struct { + link uint8 + autoSleep uint8 + measure uint8 + sleep uint8 + wakeUp uint8 +} + +// Internal structure for the sensor's data format configuration +type adxl345DataFormat struct { + selfTest uint8 + spi uint8 + intInvert uint8 + fullRes uint8 + justify uint8 + sensorRange uint8 +} + +// Internal structure for the sampling rate configuration +type adxl345BwRate struct { + lowPower uint8 + rate uint8 +} + +// NewADXL345Driver 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 NewADXL345Driver(a Connector, options ...func(Config)) *ADXL345Driver { + m := &ADXL345Driver{ + name: gobot.DefaultName("ADXL345"), + connector: a, + powerCtl: adxl345PowerCtl{ + measure: 1, + }, + dataFormat: adxl345DataFormat{ + sensorRange: ADXL345_RANGE_2G, + }, + bwRate: adxl345BwRate{ + lowPower: 1, + rate: ADXL345_RATE_100HZ, + }, + Config: NewConfig(), + } + + for _, option := range options { + option(m) + } + + // TODO: add commands for API + return m +} + +// Name returns the Name for the Driver +func (h *ADXL345Driver) Name() string { return h.name } + +// SetName sets the Name for the Driver +func (h *ADXL345Driver) SetName(n string) { h.name = n } + +// Connection returns the connection for the Driver +func (h *ADXL345Driver) Connection() gobot.Connection { return h.connector.(gobot.Connection) } + +// Start initialized the adxl345 +func (h *ADXL345Driver) Start() (err error) { + bus := h.GetBusOrDefault(h.connector.GetDefaultBus()) + address := h.GetAddressOrDefault(ADXL345AddressLow) + + h.connection, err = h.connector.GetConnection(address, bus) + if err != nil { + return err + } + + if _, err := h.connection.Write([]byte{ADXL345_REG_BW_RATE, h.bwRate.toByte()}); err != nil { + return err + } + + if _, err := h.connection.Write([]byte{ADXL345_REG_POWER_CTL, h.powerCtl.toByte()}); err != nil { + return err + } + + if _, err := h.connection.Write([]byte{ADXL345_REG_DATA_FORMAT, h.dataFormat.toByte()}); err != nil { + return err + } + + return +} + +// Stop adxl345 +func (h *ADXL345Driver) Stop() (err error) { + h.powerCtl.measure = 0 + if h.connection == nil { + return errors.New("connection not available") + } + if _, err := h.connection.Write([]byte{ADXL345_REG_POWER_CTL, h.powerCtl.toByte()}); err != nil { + return err + } + + return +} + +// Halt returns true if devices is halted successfully +func (h *ADXL345Driver) Halt() (err error) { + h.Stop() + return +} + +// XYZ returns the adjusted x, y and z axis from the adxl345 +func (h *ADXL345Driver) XYZ() (float64, float64, float64, error) { + err := h.update() + return h.x, h.y, h.z, err +} + +// XYZ returns the raw x,y and z axis from the adxl345 +func (h *ADXL345Driver) RawXYZ() (int16, int16, int16, error) { + err := h.update() + return h.rawX, h.rawY, h.rawZ, err +} + +// update the cached values for the axis to avoid errors if the connection is not available (polling too frequently) +func (h *ADXL345Driver) update() (err error) { + + if h.connection == nil { + return errors.New("connection not available") + } + + h.connection.Write([]byte{ADXL345_REG_DATAX0}) + buf := []byte{0, 0, 0, 0, 0, 0} + + _, err = h.connection.Read(buf) + if err != nil { + return + } + + h.rawX = int16(binary.LittleEndian.Uint16(buf[0:2])) + h.rawY = int16(binary.LittleEndian.Uint16(buf[2:4])) + h.rawZ = int16(binary.LittleEndian.Uint16(buf[4:6])) + + h.x = h.dataFormat.ConvertToSI(h.rawX) + h.y = h.dataFormat.ConvertToSI(h.rawY) + h.z = h.dataFormat.ConvertToSI(h.rawZ) + + return +} + +// SetRate change the current rate of the sensor +func (h *ADXL345Driver) UseLowPower(power bool) (err error) { + if power { + h.bwRate.lowPower = 1 + } else { + h.bwRate.lowPower = 0 + } + if _, err := h.connection.Write([]byte{ADXL345_REG_BW_RATE, h.bwRate.toByte()}); err != nil { + return err + } + return +} + +// SetRate change the current rate of the sensor +func (h *ADXL345Driver) SetRate(rate byte) (err error) { + if rate <= ADXL345_RATE_3200HZ { + return errors.New("not a valid rate") + } + h.bwRate.rate = rate & 0x0F + if _, err := h.connection.Write([]byte{ADXL345_REG_BW_RATE, h.bwRate.toByte()}); err != nil { + return err + } + return +} + +// SetRange change the current range of the sensor +func (h *ADXL345Driver) SetRange(sensorRange byte) (err error) { + if sensorRange != ADXL345_RANGE_2G && + sensorRange != ADXL345_RANGE_4G && + sensorRange != ADXL345_RANGE_8G && + sensorRange != ADXL345_RANGE_16G { + return errors.New("not a valid range") + } + h.dataFormat.sensorRange = sensorRange & 0x03 + if _, err := h.connection.Write([]byte{ADXL345_REG_DATA_FORMAT, h.dataFormat.toByte()}); err != nil { + return err + } + return +} + +// ConvertToSI adjusts the raw values from the adxl345 with the range configuration +func (d *adxl345DataFormat) ConvertToSI(rawValue int16) float64 { + switch d.sensorRange { + case ADXL345_RANGE_2G: + return float64(rawValue) * 2 / 512 + case ADXL345_RANGE_4G: + return float64(rawValue) * 4 / 512 + case ADXL345_RANGE_8G: + return float64(rawValue) * 8 / 512 + case ADXL345_RANGE_16G: + return float64(rawValue) * 16 / 512 + default: + return 0 + } +} + +// toByte returns a byte from the powerCtl configuration +func (p *adxl345PowerCtl) toByte() (bits uint8) { + bits = 0x00 + bits = bits | (p.link << 5) + bits = bits | (p.autoSleep << 4) + bits = bits | (p.measure << 3) + bits = bits | (p.sleep << 2) + bits = bits | p.wakeUp + + return bits +} + +// toByte returns a byte from the dataFormat configuration +func (d *adxl345DataFormat) toByte() (bits uint8) { + bits = 0x00 + bits = bits | (d.selfTest << 7) + bits = bits | (d.spi << 6) + bits = bits | (d.intInvert << 5) + bits = bits | (d.fullRes << 3) + bits = bits | (d.justify << 2) + bits = bits | d.sensorRange + + return bits +} + +// toByte returns a byte from the bwRate configuration +func (b *adxl345BwRate) toByte() (bits uint8) { + bits = 0x00 + bits = bits | (b.lowPower << 4) + bits = bits | b.rate + + return bits +} diff --git a/drivers/i2c/adxl345_driver_test.go b/drivers/i2c/adxl345_driver_test.go new file mode 100644 index 000000000..b9ae464d4 --- /dev/null +++ b/drivers/i2c/adxl345_driver_test.go @@ -0,0 +1,153 @@ +package i2c + +import ( + "bytes" + "errors" + "strings" + "testing" + + "gobot.io/x/gobot" + "gobot.io/x/gobot/gobottest" +) + +var _ gobot.Driver = (*ADXL345Driver)(nil) + +// --------- HELPERS +func initTestADXL345Driver() (driver *ADXL345Driver) { + driver, _ = initTestADXL345DriverWithStubbedAdaptor() + return +} + +func initTestADXL345DriverWithStubbedAdaptor() (*ADXL345Driver, *i2cTestAdaptor) { + adaptor := newI2cTestAdaptor() + return NewADXL345Driver(adaptor), adaptor +} + +// --------- TESTS + +func TestNewADXL345Driver(t *testing.T) { + // Does it return a pointer to an instance of ADXL345Driver? + var mma interface{} = NewADXL345Driver(newI2cTestAdaptor()) + _, ok := mma.(*ADXL345Driver) + if !ok { + t.Errorf("NewADXL345Driver() should have returned a *ADXL345Driver") + } +} + +// Methods +func TestADXL345Driver(t *testing.T) { + mma := initTestADXL345Driver() + + gobottest.Refute(t, mma.Connection(), nil) + gobottest.Assert(t, strings.HasPrefix(mma.Name(), "ADXL345"), true) +} + +func TestADXL345DriverSetName(t *testing.T) { + d := initTestADXL345Driver() + d.SetName("TESTME") + gobottest.Assert(t, d.Name(), "TESTME") +} + +func TestADXL345DriverOptions(t *testing.T) { + d := NewADXL345Driver(newI2cTestAdaptor(), WithBus(2)) + gobottest.Assert(t, d.GetBusOrDefault(1), 2) +} + +func TestADXL345DriverStart(t *testing.T) { + d := initTestADXL345Driver() + gobottest.Assert(t, d.Start(), nil) +} + +func TestADXL345StartConnectError(t *testing.T) { + d, adaptor := initTestADXL345DriverWithStubbedAdaptor() + adaptor.Testi2cConnectErr(true) + gobottest.Assert(t, d.Start(), errors.New("Invalid i2c connection")) +} + +func TestADXL345DriverStartWriteError(t *testing.T) { + mma, adaptor := initTestADXL345DriverWithStubbedAdaptor() + adaptor.i2cWriteImpl = func([]byte) (int, error) { + return 0, errors.New("write error") + } + gobottest.Assert(t, mma.Start(), errors.New("write error")) +} + +func TestADXL345DriverHalt(t *testing.T) { + d := initTestADXL345Driver() + gobottest.Assert(t, d.Start(), nil) + gobottest.Assert(t, d.Halt(), nil) +} + +func TestADXL345DriverNullXYZ(t *testing.T) { + d, _ := initTestADXL345DriverWithStubbedAdaptor() + d.Start() + x, y, z, _ := d.XYZ() + gobottest.Assert(t, x, 0.0) + gobottest.Assert(t, y, 0.0) + gobottest.Assert(t, z, 0.0) +} + +func TestADXL345DriverXYZ(t *testing.T) { + d, adaptor := initTestADXL345DriverWithStubbedAdaptor() + d.Start() + + adaptor.i2cReadImpl = func(b []byte) (int, error) { + buf := new(bytes.Buffer) + buf.Write([]byte{218, 0, 251, 255, 100, 0}) + copy(b, buf.Bytes()) + return buf.Len(), nil + } + + x, y, z, _ := d.XYZ() + gobottest.Assert(t, x, 0.8515625) + gobottest.Assert(t, y, -0.01953125) + gobottest.Assert(t, z, 0.390625) +} + +func TestADXL345DriverXYZError(t *testing.T) { + d, adaptor := initTestADXL345DriverWithStubbedAdaptor() + d.Start() + + adaptor.i2cReadImpl = func(b []byte) (int, error) { + return 0, errors.New("read error") + } + + _, _, _, err := d.XYZ() + gobottest.Assert(t, err, errors.New("read error")) +} + + +func TestADXL345DriverRawXYZ(t *testing.T) { + d, adaptor := initTestADXL345DriverWithStubbedAdaptor() + d.Start() + + adaptor.i2cReadImpl = func(b []byte) (int, error) { + buf := new(bytes.Buffer) + buf.Write([]byte{218, 0, 251, 255, 100, 0}) + copy(b, buf.Bytes()) + return buf.Len(), nil + } + + x, y, z, _ := d.RawXYZ() + gobottest.Assert(t, int(x), 218) + gobottest.Assert(t, int(y), -5) + gobottest.Assert(t, int(z), 100) +} + +func TestADXL345DriverRawXYZError(t *testing.T) { + d, adaptor := initTestADXL345DriverWithStubbedAdaptor() + d.Start() + + adaptor.i2cReadImpl = func(b []byte) (int, error) { + return 0, errors.New("read error") + } + + _, _, _, err := d.RawXYZ() + gobottest.Assert(t, err, errors.New("read error")) +} + +func TestADXL345DriverSetRange(t *testing.T) { + d := initTestADXL345Driver() + d.Start() + gobottest.Assert(t, d.SetRange(ADXL345_RANGE_16G), nil) +} diff --git a/examples/firmata_adxl345.go b/examples/firmata_adxl345.go new file mode 100644 index 000000000..8496d41f1 --- /dev/null +++ b/examples/firmata_adxl345.go @@ -0,0 +1,43 @@ +// +build example +// +// Do not build by default. + +/* + How to run + Pass serial port to use as the first param: + + go run examples/firmata_adxl345.go /dev/ttyACM0 +*/ + +package main + +import ( + "fmt" + "os" + "time" + + "gobot.io/x/gobot" + "gobot.io/x/gobot/drivers/i2c" + "gobot.io/x/gobot/platforms/firmata" +) + +func main() { + firmataAdaptor := firmata.NewAdaptor(os.Args[1]) + adxl345 := i2c.NewADXL345Driver(firmataAdaptor) + + work := func() { + gobot.Every(100*time.Millisecond, func() { + x, y, z, _ := adxl345.XYZ() + + fmt.Printf("x: %.7f | y: %.7f | z: %.7f \n", x, y, z) + }) + } + + robot := gobot.NewRobot("adxl345Bot", + []gobot.Connection{firmataAdaptor}, + []gobot.Device{adxl345}, + work, + ) + + robot.Start() +}