Merge pull request hybridgroup#169 from dmarkham/master

Adding sphero API ReadLocator, SetRotationRate, ConfigureLocator

platforms/sphero/sphero_driver.go

@@ -39,13 +39,16 @@ type SpheroDriver struct {
 // NewSpheroDriver returns a new SpheroDriver given a SpheroAdaptor and name.
 //
 // Adds the following API Commands:
+// 	"ConfigureLocator" - See SpheroDriver.ConfigureLocator
 // 	"Roll" - See SpheroDriver.Roll
 // 	"Stop" - See SpheroDriver.Stop
 // 	"GetRGB" - See SpheroDriver.GetRGB
+//	"ReadLocator" - See SpheroDriver.ReadLocator
 // 	"SetBackLED" - See SpheroDriver.SetBackLED
 // 	"SetHeading" - See SpheroDriver.SetHeading
 // 	"SetStabilization" - See SpheroDriver.SetStabilization
 //  "SetDataStreaming" - See SpheroDriver.SetDataStreaming
+//  "SetRotationRate" - See SpheroDriver.SetRotationRate
 func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
 	s := &SpheroDriver{
 		name:            name,
@@ -84,12 +87,22 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
 		return s.GetRGB()
 	})
 
+	s.AddCommand("ReadLocator", func(params map[string]interface{}) interface{} {
+		return s.ReadLocator()
+	})
+
 	s.AddCommand("SetBackLED", func(params map[string]interface{}) interface{} {
 		level := uint8(params["level"].(float64))
 		s.SetBackLED(level)
 		return nil
 	})
 
+	s.AddCommand("SetRotationRate", func(params map[string]interface{}) interface{} {
+		level := uint8(params["level"].(float64))
+		s.SetRotationRate(level)
+		return nil
+	})
+
 	s.AddCommand("SetHeading", func(params map[string]interface{}) interface{} {
 		heading := uint16(params["heading"].(float64))
 		s.SetHeading(heading)
@@ -113,6 +126,16 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
 		return nil
 	})
 
+	s.AddCommand("ConfigureLocator", func(params map[string]interface{}) interface{} {
+		Flags := uint8(params["Flags"].(float64))
+		X := int16(params["X"].(float64))
+		Y := int16(params["Y"].(float64))
+		YawTare := int16(params["YawTare"].(float64))
+
+		s.ConfigureLocator(LocatorConfig{Flags: Flags, X: X, Y: Y, YawTare: YawTare})
+		return nil
+	})
+
 	return s
 }
 
@@ -215,11 +238,28 @@ func (s *SpheroDriver) GetRGB() []uint8 {
 	return []uint8{}
 }
 
+// ReadLocator reads Sphero's current position (X,Y), component velocities and SOG (speed over ground).
+func (s *SpheroDriver) ReadLocator() []int16 {
+	buf := s.getSyncResponse(s.craftPacket([]uint8{}, 0x02, 0x15))
+	if len(buf) == 16 {
+		vals := make([]int16, 5)
+		_ = binary.Read(bytes.NewReader(buf[5:15]), binary.BigEndian, &vals)
+		return vals
+	}
+	return []int16{}
+}
+
 // SetBackLED sets the Sphero Back LED to the specified brightness
 func (s *SpheroDriver) SetBackLED(level uint8) {
 	s.packetChannel <- s.craftPacket([]uint8{level}, 0x02, 0x21)
 }
 
+// SetRotationRate sets the Sphero rotation rate
+// A value of 255 jumps to the maximum (currently 400 degrees/sec).
+func (s *SpheroDriver) SetRotationRate(level uint8) {
+	s.packetChannel <- s.craftPacket([]uint8{level}, 0x02, 0x03)
+}
+
 // SetHeading sets the heading of the Sphero
 func (s *SpheroDriver) SetHeading(heading uint16) {
 	s.packetChannel <- s.craftPacket([]uint8{uint8(heading >> 8), uint8(heading & 0xFF)}, 0x02, 0x01)
@@ -239,6 +279,14 @@ func (s *SpheroDriver) Roll(speed uint8, heading uint16) {
 	s.packetChannel <- s.craftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x02, 0x30)
 }
 
+// Configures and enables the Locator
+func (s *SpheroDriver) ConfigureLocator(d LocatorConfig) {
+	buf := new(bytes.Buffer)
+	binary.Write(buf, binary.BigEndian, d)
+
+	s.packetChannel <- s.craftPacket(buf.Bytes(), 0x02, 0x13)
+}
+
 // Enables sensor data streaming
 func (s *SpheroDriver) SetDataStreaming(d DataStreamingConfig) {
 	buf := new(bytes.Buffer)

platforms/sphero/sphero_driver_test.go

@@ -33,22 +33,40 @@ func TestSpheroDriver(t *testing.T) {
 	)
 	gobot.Assert(t, ret, nil)
 
+	ret = d.Command("ConfigureLocator")(
+		map[string]interface{}{"Flags": 1.0, "X": 100.0, "Y": 100.0, "YawTare": 100.0},
+	)
+	gobot.Assert(t, ret, nil)
+
 	ret = d.Command("SetHeading")(
 		map[string]interface{}{"heading": 100.0},
 	)
 	gobot.Assert(t, ret, nil)
 
+	ret = d.Command("SetRotationRate")(
+		map[string]interface{}{"level": 100.0},
+	)
+	gobot.Assert(t, ret, nil)
+
 	ret = d.Command("SetStabilization")(
 		map[string]interface{}{"enable": true},
 	)
 	gobot.Assert(t, ret, nil)
 
+	ret = d.Command("SetStabilization")(
+		map[string]interface{}{"enable": false},
+	)
+	gobot.Assert(t, ret, nil)
+
 	ret = d.Command("Stop")(nil)
 	gobot.Assert(t, ret, nil)
 
 	ret = d.Command("GetRGB")(nil)
 	gobot.Assert(t, ret.([]byte), []byte{})
 
+	ret = d.Command("ReadLocator")(nil)
+	gobot.Assert(t, ret, []int16{})
+
 	gobot.Assert(t, d.Name(), "bot")
 	gobot.Assert(t, d.Connection().Name(), "bot")
 }
@@ -94,6 +112,34 @@ func TestSpheroDriverSetDataStreaming(t *testing.T) {
 	gobot.Assert(t, data.body, buf.Bytes())
 }
 
+func TestConfigureLocator(t *testing.T) {
+	d := initTestSpheroDriver()
+	d.ConfigureLocator(DefaultLocatorConfig())
+	data := <-d.packetChannel
+
+	buf := new(bytes.Buffer)
+	binary.Write(buf, binary.BigEndian, DefaultLocatorConfig())
+
+	gobot.Assert(t, data.body, buf.Bytes())
+
+	ret := d.Command("ConfigureLocator")(
+		map[string]interface{}{
+			"Flags":   1.0,
+			"X":       100.0,
+			"Y":       100.0,
+			"YawTare": 0.0,
+		},
+	)
+	gobot.Assert(t, ret, nil)
+	data = <-d.packetChannel
+
+	lconfig := LocatorConfig{Flags: 1, X: 100, Y: 100, YawTare: 0}
+	buf = new(bytes.Buffer)
+	binary.Write(buf, binary.BigEndian, lconfig)
+
+	gobot.Assert(t, data.body, buf.Bytes())
+}
+
 func TestCalculateChecksum(t *testing.T) {
 	tests := []struct {
 		data     []byte

platforms/sphero/sphero_packets.go

@@ -1,5 +1,29 @@
 package sphero
 
+// DefaultLocatorConfig returns a LocatorConfig with defaults
+func DefaultLocatorConfig() LocatorConfig {
+	return LocatorConfig{
+		Flags:   0x01,
+		X:       0x00,
+		Y:       0x00,
+		YawTare: 0x00,
+	}
+}
+
+// LocatorConfig provides configuration for the Location api.
+// https://github.com/orbotix/DeveloperResources/blob/master/docs/Sphero_API_1.50.pdf
+// The current (X,Y) coordinates of Sphero on the ground plane in centimeters.
+type LocatorConfig struct {
+	// Determines whether calibrate commands automatically correct the yaw tare value
+	Flags uint8
+	// Controls how the X-plane is aligned with Sphero’s heading coordinate system.
+	X int16
+	// Controls how the Y-plane is aligned with Sphero’s heading coordinate system.
+	Y int16
+	// Controls how the X,Y-plane is aligned with Sphero’s heading coordinate system.
+	YawTare int16
+}
+
 // DefaultCollisionConfig returns a CollisionConfig with sensible collision defaults
 func DefaultCollisionConfig() CollisionConfig {
 	return CollisionConfig{