feat(ultrasonic): Add ultrasonic i2c

add ultrasonic support for i2c driver
add basic test for ultrasonic sensor
fix length and update test for i2c driver

drivers/i2c/grovepi_driver.go

@@ -13,12 +13,13 @@ const grovePiAddress = 0x04
 
 // Commands format
 const (
-	CommandReadDigital  = 1
-	CommandWriteDigital = 2
-	CommandReadAnalog   = 3
-	CommandWriteAnalog  = 4
-	CommandPinMode      = 5
-	CommandReadDHT      = 40
+	CommandReadDigital    = 1
+	CommandWriteDigital   = 2
+	CommandReadAnalog     = 3
+	CommandWriteAnalog    = 4
+	CommandPinMode        = 5
+	CommandReadUltrasonic = 7
+	CommandReadDHT        = 40
 )
 
 // GrovePiDriver is a driver for the GrovePi+ for I²C bus interface.
@@ -133,6 +134,26 @@ func (d *GrovePiDriver) DigitalRead(pin string) (val int, err error) {
 	return
 }
 
+// UltrasonicRead performs a read on an ultrasonic pin.
+func (d *GrovePiDriver) UltrasonicRead(pin string, duration int) (val int, err error) {
+	pin = getPin(pin)
+
+	var pinNum int
+	pinNum, err = strconv.Atoi(pin)
+	if err != nil {
+		return
+	}
+
+	if dir, ok := d.digitalPins[pinNum]; !ok || dir != "input" {
+		d.PinMode(byte(pinNum), "input")
+		d.digitalPins[pinNum] = "input"
+	}
+
+	val, err = d.readUltrasonic(byte(pinNum), duration)
+
+	return
+}
+
 // DigitalWrite writes a value to a specific digital pin implementing the DigitalWriter interface.
 func (d *GrovePiDriver) DigitalWrite(pin string, val byte) (err error) {
 	pin = getPin(pin)
@@ -217,6 +238,28 @@ func (d *GrovePiDriver) readAnalog(pin byte) (int, error) {
 	return ((v1 * 256) + v2), nil
 }
 
+// readUltrasonic reads ultrasonic from the GrovePi.
+func (d *GrovePiDriver) readUltrasonic(pin byte, duration int) (val int, err error) {
+	d.mutex.Lock()
+	defer d.mutex.Unlock()
+
+	buf := []byte{CommandReadUltrasonic, pin, 0, 0}
+	_, err = d.connection.Write(buf)
+	if err != nil {
+		return
+	}
+
+	time.Sleep(time.Duration(duration) * time.Millisecond)
+
+	data := make([]byte, 3)
+	_, err = d.connection.Read(data)
+	if err != nil || data[0] != CommandReadUltrasonic {
+		return 0, err
+	}
+
+	return int(data[1]) * 255 + int(data[2]), err
+}
+
 // readDigital reads digitally from the GrovePi.
 func (d *GrovePiDriver) readDigital(pin byte) (val int, err error) {
 	d.mutex.Lock()

drivers/i2c/grovepi_driver_test.go

@@ -1,13 +1,12 @@
 package i2c
 
 import (
-	"strings"
-	"testing"
-
 	"gobot.io/x/gobot"
 	"gobot.io/x/gobot/drivers/aio"
 	"gobot.io/x/gobot/drivers/gpio"
 	"gobot.io/x/gobot/gobottest"
+	"strings"
+	"testing"
 )
 
 var _ gobot.Driver = (*GrovePiDriver)(nil)
@@ -24,9 +23,8 @@ var _ aio.AnalogReader = (*GrovePiDriver)(nil)
 // must implement the Adaptor interface
 var _ gobot.Adaptor = (*GrovePiDriver)(nil)
 
-func initTestGrovePiDriver() (driver *GrovePiDriver) {
-	driver, _ = initGrovePiDriverWithStubbedAdaptor()
-	return
+func initTestGrovePiDriver() (driver *GrovePiDriver, adaptor *i2cTestAdaptor) {
+	return initGrovePiDriverWithStubbedAdaptor()
 }
 
 func initGrovePiDriverWithStubbedAdaptor() (*GrovePiDriver, *i2cTestAdaptor) {
@@ -35,7 +33,7 @@ func initGrovePiDriverWithStubbedAdaptor() (*GrovePiDriver, *i2cTestAdaptor) {
 }
 
 func TestGrovePiDriverName(t *testing.T) {
-	g := initTestGrovePiDriver()
+	g, _ := initTestGrovePiDriver()
 	gobottest.Refute(t, g.Connection(), nil)
 	gobottest.Assert(t, strings.HasPrefix(g.Name(), "GrovePi"), true)
 }
@@ -45,9 +43,41 @@ func TestGrovePiDriverOptions(t *testing.T) {
 	gobottest.Assert(t, g.GetBusOrDefault(1), 2)
 }
 
+func TestGrovePiDriver_UltrasonicRead(t *testing.T) {
+	g, a := initTestGrovePiDriver()
+	g.Start()
+
+	fakePin := byte(1)
+    fakeI2cResponse := []byte{CommandReadUltrasonic, 1, 2}
+
+	expectedCommand := []byte{CommandReadUltrasonic, fakePin, 0, 0}
+    expectedResult := 257
+
+	resultCommand := make([]byte, 3)
+
+    // capture i2c command
+	a.i2cWriteImpl = func(bytes []byte) (i int, e error) {
+		resultCommand = bytes
+		return len(bytes), nil
+	}
+
+	// fake i2c response
+	a.i2cReadImpl = func(bytes []byte) (i int, e error) {
+		bytes[0] = fakeI2cResponse[0]
+		bytes[1] = fakeI2cResponse[1]
+		bytes[2] = fakeI2cResponse[2]
+		return len(bytes), nil
+	}
+
+	result, _ := g.readUltrasonic(fakePin, 10)
+
+	gobottest.Assert(t, resultCommand, expectedCommand)
+	gobottest.Assert(t, result, expectedResult)
+}
+
 // Methods
 func TestGrovePiDriverStart(t *testing.T) {
-	g := initTestGrovePiDriver()
+	g, _ := initTestGrovePiDriver()
 
 	gobottest.Assert(t, g.Start(), nil)
 }