diff --git a/platforms/sphero/sphero_driver.go b/platforms/sphero/sphero_driver.go index 2ef958dda..9472a8b2c 100644 --- a/platforms/sphero/sphero_driver.go +++ b/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) diff --git a/platforms/sphero/sphero_driver_test.go b/platforms/sphero/sphero_driver_test.go index 2a23a6fca..18d5fdfcc 100644 --- a/platforms/sphero/sphero_driver_test.go +++ b/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 diff --git a/platforms/sphero/sphero_packets.go b/platforms/sphero/sphero_packets.go index ca060de9b..105eeb7a5 100644 --- a/platforms/sphero/sphero_packets.go +++ b/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{