Feature/add as5600 support

drivers/i2c/as5600_driver.go

@@ -0,0 +1,249 @@
+package i2c
+
+import (
+	"encoding/binary"
+
+	"gobot.io/x/gobot"
+)
+
+const (
+	// DefaultAddress is the default I2C address for the as5600
+	as5600Address = 0x36
+
+	// as5600ZMCO = 0x00
+	// // zero position
+	// as5600ZPOSMSB = 0x01
+	// as5600ZPOSLSB = 0x02
+	// //maximum position
+	// as5600MPOSMSB = 0x03
+	// as5600MPOSLSB = 0x04
+	// maximum angle
+	as5600MANGMSB = 0x05
+	as5600MANGLSB = 0x06
+	// // Customize the device
+	// as5600CONFMSB = 0x07
+	// as5600CONFLSB = 0x08
+	//Unscaled and unmodified angle
+	as5600RAWANGLEMSB = 0x0C
+	as5600RAWANGLELSB = 0x0D
+	// Scaled output value
+	as5600ANGLEMSB = 0x0E
+	as5600ANGLELSB = 0x0F
+	// Current state (basically strength of magnet)
+	as5600STATUS = 0x0B
+	// // Automatic Gain Control
+	// as5600AGC = 0x1A
+	// // Magnitude value of the internal cordic
+	// as5600MAGNITUDEMSB = 0x1B
+	// as5600MAGNITUDELSB = 0x1C
+	// // Burn commands
+	// as5600BURN = 0xFF
+)
+
+// // AS5600 Power Mode
+// const (
+// 	as5600PowerModeNorm = iota
+// 	as5600PowerModeLPM1
+// 	as5600PowerModeLPM2
+// 	as5600PowerModeLPM3
+// )
+
+// // AS5600 Hysteresis
+// const (
+// 	as5600HysteresisNorm = iota
+// 	as5600Hysteresis1LSB
+// 	as5600Hysteresis2LSB
+// 	as5600Hysteresis3LSB
+// )
+
+// const (
+// 	as5600OutputStageAnalogFull = iota
+// 	as5600OutputStageAnalogReduced
+// 	as5600OutputStageDigitalPWM
+// )
+
+const (
+	_ = 1 << iota
+	_
+	_
+	as5600StatusMHBit
+	as5600StatusMLBit
+	as5600StatusMDBit
+)
+
+//go:generate stringer -type=AS5600StatusType
+type AS5600StatusType uint16
+
+// Magnet status
+const (
+	as5600MagnetNotDetected AS5600StatusType = iota
+	as5600MagnetTooWeak
+	as5600MagnetOk
+	as5600MagnetTooStrong
+	as5600MagnetUnknown
+)
+
+// AS5600Driver is a Driver for a AS5600 magnetic encoder
+type AS5600Driver struct {
+	name       string
+	connector  Connector
+	connection Connection
+	Config
+}
+
+// NewAS5600Driver 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 NewAS5600Driver(a Connector, options ...func(Config)) *AS5600Driver {
+
+	as := &AS5600Driver{
+		name:      gobot.DefaultName("AS5600"),
+		connector: a,
+		Config:    NewConfig(),
+	}
+
+	for _, option := range options {
+		option(as)
+	}
+
+	return as
+}
+
+// Name returns the name for this Driver
+func (as *AS5600Driver) Name() string {
+
+	return as.name
+}
+
+// SetName sets the name for this Driver
+func (as *AS5600Driver) SetName(n string) {
+
+	as.name = n
+}
+
+// Connection returns the connection for this Driver
+func (as *AS5600Driver) Connection() gobot.Connection {
+
+	return as.connector.(gobot.Connection)
+}
+
+// Start initializes the AS5600
+func (as *AS5600Driver) Start() (err error) {
+
+	bus := as.GetBusOrDefault(as.connector.GetDefaultBus())
+	address := as.GetAddressOrDefault(as5600Address)
+
+	if as.connection, err = as.connector.GetConnection(address, bus); err != nil {
+		return err
+	}
+
+	return nil
+}
+
+// Halt returns true if devices is halted successfully
+func (as *AS5600Driver) Halt() (err error) {
+
+	return nil
+}
+
+// DetectMagnet returns if the magnet is detected
+func (as *AS5600Driver) DetecMagnet() (bool, error) {
+	var magStatus uint8
+	var err error
+
+	magStatus, err = as.connection.ReadByteData(as5600STATUS)
+
+	if err != nil {
+		return false, err
+	}
+
+	return (magStatus&as5600StatusMDBit != 0), err
+}
+
+// GetMagnetStrength returns the magnet's strength
+func (as *AS5600Driver) GetMagnetStrength() (AS5600StatusType, error) {
+	var magStatus uint8
+	var err error
+
+	magStatus, err = as.connection.ReadByteData(as5600STATUS)
+	if err != nil {
+		return as5600MagnetUnknown, err
+	}
+	if magStatus&as5600StatusMDBit == 0 {
+		return as5600MagnetNotDetected, nil
+	}
+	if magStatus&as5600StatusMHBit != 0 {
+		return as5600MagnetTooStrong, nil
+	}
+	if magStatus&as5600StatusMLBit != 0 {
+		return as5600MagnetTooWeak, nil
+	}
+
+	return as5600MagnetOk, nil
+}
+
+// GetRawAngle gets raw value of magnet position.
+// start, end, and max angle settings do not apply
+func (as *AS5600Driver) GetRawAngle() (uint16, error) {
+	var angle []byte
+	var err error
+	var rc uint16
+
+	angle, err = as.read(as5600RAWANGLEMSB, 2)
+	if err != nil {
+		return 0x0, err
+	}
+	rc = binary.LittleEndian.Uint16(angle[0:])
+
+	return rc & 0x0fff, nil
+}
+
+// GetScaledAngle gets scaled value of magnet position.
+// start, end, or max angle settings are used to determine value
+func (as *AS5600Driver) GetScaledAngle() (uint16, error) {
+	var angle []byte
+	var err error
+	var rc uint16
+
+	angle, err = as.read(as5600ANGLEMSB, 2)
+	if err != nil {
+		return 0x0, err
+	}
+	rc = binary.LittleEndian.Uint16(angle[0:])
+
+	return rc & 0x0fff, nil
+}
+
+// GetMaxAngle gets value of maximum angle register.
+func (as *AS5600Driver) GetMaxAngle() (uint16, error) {
+	var angle []byte
+	var err error
+	var rc uint16
+
+	angle, err = as.read(as5600MANGMSB, 2)
+	if err != nil {
+		return 0x0, err
+	}
+	rc = binary.LittleEndian.Uint16(angle[0:])
+
+	return rc & 0x0fff, nil
+}
+
+func (as *AS5600Driver) read(address byte, n int) ([]byte, error) {
+
+	if _, err := as.connection.Write([]byte{address}); err != nil {
+		return nil, err
+	}
+	buf := make([]byte, n)
+	bytesRead, err := as.connection.Read(buf)
+	if bytesRead != n || err != nil {
+		return nil, err
+	}
+
+	return buf, nil
+}