76 lines
1.5 KiB
Go
76 lines
1.5 KiB
Go
package i2c
|
|
|
|
import (
|
|
"time"
|
|
)
|
|
|
|
const lidarliteDefaultAddress = 0x62
|
|
|
|
// LIDARLiteDriver is the Gobot driver for the LIDARLite I2C LIDAR device.
|
|
type LIDARLiteDriver struct {
|
|
*Driver
|
|
}
|
|
|
|
// NewLIDARLiteDriver creates a new driver for the LIDARLite I2C LIDAR device.
|
|
//
|
|
// Params:
|
|
//
|
|
// c 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 NewLIDARLiteDriver(c Connector, options ...func(Config)) *LIDARLiteDriver {
|
|
l := &LIDARLiteDriver{
|
|
Driver: NewDriver(c, "LIDARLite", lidarliteDefaultAddress),
|
|
}
|
|
|
|
for _, option := range options {
|
|
option(l)
|
|
}
|
|
|
|
// TODO: add commands to API
|
|
return l
|
|
}
|
|
|
|
// Distance returns the current distance in cm
|
|
func (h *LIDARLiteDriver) Distance() (int, error) {
|
|
if _, err := h.connection.Write([]byte{0x00, 0x04}); err != nil {
|
|
return 0, err
|
|
}
|
|
time.Sleep(20 * time.Millisecond)
|
|
|
|
if _, err := h.connection.Write([]byte{0x0F}); err != nil {
|
|
return 0, err
|
|
}
|
|
|
|
upper := []byte{0}
|
|
bytesRead, err := h.connection.Read(upper)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
|
|
if bytesRead != 1 {
|
|
return 0, ErrNotEnoughBytes
|
|
}
|
|
|
|
if _, err := h.connection.Write([]byte{0x10}); err != nil {
|
|
return 0, err
|
|
}
|
|
|
|
lower := []byte{0}
|
|
bytesRead, err = h.connection.Read(lower)
|
|
if err != nil {
|
|
return 0, err
|
|
}
|
|
|
|
if bytesRead != 1 {
|
|
return 0, ErrNotEnoughBytes
|
|
}
|
|
|
|
distance := ((int(upper[0]) & 0xff) << 8) | (int(lower[0]) & 0xff)
|
|
|
|
return distance, nil
|
|
}
|