gobot.io/x/gobot@v1.16.0/drivers/i2c/l3gd20h_driver.go (about)

     1  package i2c
     2  
     3  import (
     4  	"bytes"
     5  	"encoding/binary"
     6  
     7  	"gobot.io/x/gobot"
     8  )
     9  
    10  const l3gd20hAddress = 0x6B
    11  
    12  // Control Register 1
    13  const l3gd20hRegisterCtl1 = 0x20
    14  const l3gd20hNormalMode = 0x8
    15  const l3gd20hEnableZ = 0x04
    16  const l3gd20hEnableY = 0x02
    17  const l3gd20hEnableX = 0x01
    18  
    19  // Control Register 4
    20  const l3gd20hRegisterCtl4 = 0x23
    21  
    22  const l3gd20hRegisterOutXLSB = 0x28 | 0x80 // set auto-increment bit.
    23  
    24  const (
    25  	// L3GD20HScale250dps is the 250 degress-per-second scale.
    26  	L3GD20HScale250dps L3GD20HScale = 0x00
    27  	// L3GD20HScale500dps is the 500 degress-per-second scale.
    28  	L3GD20HScale500dps L3GD20HScale = 0x10
    29  	// L3GD20HScale2000dps is the 2000 degress-per-second scale.
    30  	L3GD20HScale2000dps L3GD20HScale = 0x30
    31  )
    32  
    33  // L3GD20HScale is the scale sensitivity of degrees-per-second.
    34  type L3GD20HScale byte
    35  
    36  // L3GD20HDriver is the gobot driver for the Adafruit Triple-Axis Gyroscope L3GD20H.
    37  // Device datasheet: http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/DM00036465.pdf
    38  type L3GD20HDriver struct {
    39  	name       string
    40  	connector  Connector
    41  	connection Connection
    42  	Config
    43  	scale L3GD20HScale
    44  }
    45  
    46  // NewL3GD20HDriver creates a new Gobot driver for the
    47  // L3GD20H I2C Triple-Axis Gyroscope.
    48  //
    49  // Params:
    50  //		conn Connector - the Adaptor to use with this Driver
    51  //
    52  // Optional params:
    53  //		i2c.WithBus(int):	bus to use with this driver
    54  //		i2c.WithAddress(int):	address to use with this driver
    55  //
    56  func NewL3GD20HDriver(c Connector, options ...func(Config)) *L3GD20HDriver {
    57  	l := &L3GD20HDriver{
    58  		name:      gobot.DefaultName("L3GD20H"),
    59  		connector: c,
    60  		Config:    NewConfig(),
    61  		scale:     L3GD20HScale250dps,
    62  	}
    63  
    64  	for _, option := range options {
    65  		option(l)
    66  	}
    67  
    68  	// TODO: add commands to API
    69  	return l
    70  }
    71  
    72  // Name returns the name of the device.
    73  func (d *L3GD20HDriver) Name() string {
    74  	return d.name
    75  }
    76  
    77  // SetName sets the name of the device.
    78  func (d *L3GD20HDriver) SetName(name string) {
    79  	d.name = name
    80  }
    81  
    82  // Connection returns the connection of the device.
    83  func (d *L3GD20HDriver) Connection() gobot.Connection {
    84  	return d.connector.(gobot.Connection)
    85  }
    86  
    87  // Scale returns the scale sensitivity of the device.
    88  func (d *L3GD20HDriver) Scale() L3GD20HScale {
    89  	return d.scale
    90  }
    91  
    92  // SetScale sets the scale sensitivity of the device.
    93  func (d *L3GD20HDriver) SetScale(s L3GD20HScale) {
    94  	d.scale = s
    95  }
    96  
    97  // Start initializes the device.
    98  func (d *L3GD20HDriver) Start() (err error) {
    99  	if err := d.initialization(); err != nil {
   100  		return err
   101  	}
   102  	return nil
   103  }
   104  
   105  // Halt halts the device.
   106  func (d *L3GD20HDriver) Halt() (err error) {
   107  	return nil
   108  }
   109  
   110  // XYZ returns the current change in degrees per second, for the 3 axis.
   111  func (d *L3GD20HDriver) XYZ() (x float32, y float32, z float32, err error) {
   112  	if _, err = d.connection.Write([]byte{l3gd20hRegisterOutXLSB}); err != nil {
   113  		return 0, 0, 0, err
   114  	}
   115  	measurements := make([]byte, 6)
   116  	if _, err = d.connection.Read(measurements); err != nil {
   117  		return 0, 0, 0, err
   118  	}
   119  
   120  	var rawX int16
   121  	var rawY int16
   122  	var rawZ int16
   123  	buf := bytes.NewBuffer(measurements)
   124  	binary.Read(buf, binary.LittleEndian, &rawX)
   125  	binary.Read(buf, binary.LittleEndian, &rawY)
   126  	binary.Read(buf, binary.LittleEndian, &rawZ)
   127  
   128  	// Sensitivity values from the mechanical characteristics in the datasheet.
   129  	sensitivity := d.getSensitivity()
   130  
   131  	return float32(rawX) * sensitivity, float32(rawY) * sensitivity, float32(rawZ) * sensitivity, nil
   132  }
   133  
   134  func (d *L3GD20HDriver) initialization() (err error) {
   135  	bus := d.GetBusOrDefault(d.connector.GetDefaultBus())
   136  	address := d.GetAddressOrDefault(l3gd20hAddress)
   137  
   138  	d.connection, err = d.connector.GetConnection(address, bus)
   139  	if err != nil {
   140  		return err
   141  	}
   142  	// reset the gyroscope.
   143  	if _, err := d.connection.Write([]byte{l3gd20hRegisterCtl1, 0x00}); err != nil {
   144  		return err
   145  	}
   146  	// Enable Z, Y and X axis.
   147  	if _, err := d.connection.Write([]byte{l3gd20hRegisterCtl1, l3gd20hNormalMode | l3gd20hEnableZ | l3gd20hEnableY | l3gd20hEnableX}); err != nil {
   148  		return err
   149  	}
   150  	// Set the sensitivity scale.
   151  	if _, err := d.connection.Write([]byte{l3gd20hRegisterCtl4, byte(d.scale)}); err != nil {
   152  		return err
   153  	}
   154  	return nil
   155  }
   156  
   157  func (d *L3GD20HDriver) getSensitivity() float32 {
   158  	switch d.scale {
   159  	case L3GD20HScale250dps:
   160  		return 0.00875
   161  	case L3GD20HScale500dps:
   162  		return 0.0175
   163  	case L3GD20HScale2000dps:
   164  		return 0.07
   165  	}
   166  	return 0
   167  }