From 20677abec3e22698ea32d7eb8bfdaa6124ed455e Mon Sep 17 00:00:00 2001 From: Rearth Date: Wed, 23 Jan 2019 14:30:55 +0100 Subject: [PATCH] Added some new features to the sphero ollie, bb-8 and sprkplus Signed-off-by: Rearth --- platforms/sphero/ollie/ollie_driver.go | 140 +++++++++++++++++--- platforms/sphero/ollie/ollie_driver_test.go | 86 ++++++++++++ platforms/sphero/ollie/ollie_packets.go | 86 ++++++++++++ 3 files changed, 294 insertions(+), 18 deletions(-) diff --git a/platforms/sphero/ollie/ollie_driver.go b/platforms/sphero/ollie/ollie_driver.go index ea9aeeff..d20558ee 100644 --- a/platforms/sphero/ollie/ollie_driver.go +++ b/platforms/sphero/ollie/ollie_driver.go @@ -19,7 +19,10 @@ type Driver struct { seq uint8 mtx sync.Mutex collisionResponse []uint8 - packetChannel chan *packet + packetChannel chan *Packet + asyncBuffer []byte + asyncMessage []byte + locatorCallback func(p Point2D) gobot.Eventer } @@ -57,6 +60,7 @@ const ( CollisionResponseSize = PacketHeaderSize + CollisionDataSize ) +// MotorModes is used to configure the motor type MotorModes uint8 // MotorModes required for SetRawMotorValues command @@ -68,10 +72,17 @@ const ( Ignore ) -type packet struct { - header []uint8 - body []uint8 - checksum uint8 +// Packet describes head, body and checksum for a data package to be sent to the sphero. +type Packet struct { + Header []uint8 + Body []uint8 + Checksum uint8 +} + +// Point2D represents a koordinate in 2-Dimensional space +type Point2D struct { + X int16 + Y int16 } // NewDriver creates a Driver for a Sphero Ollie @@ -80,7 +91,7 @@ func NewDriver(a ble.BLEConnector) *Driver { name: gobot.DefaultName("Ollie"), connection: a, Eventer: gobot.NewEventer(), - packetChannel: make(chan *packet, 1024), + packetChannel: make(chan *Packet, 1024), } n.AddEvent(Collision) @@ -88,6 +99,12 @@ func NewDriver(a ble.BLEConnector) *Driver { return n } +// PacketChannel returns the channel for packets to be sent to the sp +func (b *Driver) PacketChannel() chan *Packet { return b.packetChannel } + +// Sequence returns the Sequence number of the current packet +func (b *Driver) Sequence() uint8 { return b.seq } + // Connection returns the connection to this Ollie func (b *Driver) Connection() gobot.Connection { return b.connection } @@ -191,11 +208,61 @@ func (b *Driver) SetTXPower(level int) (err error) { // HandleResponses handles responses returned from Ollie func (b *Driver) HandleResponses(data []byte, e error) { - //fmt.Println("response data:", data, e) + + //since packets can only be 20 bytes long, we have to puzzle them together + newMessage := false + + //append message parts to existing + if len(data) > 0 && data[0] != 0xFF { + b.asyncBuffer = append(b.asyncBuffer, data...) + } + + //clear message when new one begins (first byte is always 0xFF) + if len(data) > 0 && data[0] == 0xFF { + b.asyncMessage = b.asyncBuffer + b.asyncBuffer = data + newMessage = true + } + + parts := b.asyncMessage + //3 is the id of data streaming, located at index 2 byte + if newMessage && len(parts) > 2 && parts[2] == 3 { + b.handleDataStreaming(parts) + } + + //index 1 is the type of the message, 0xFF being a direct response, 0xFE an asynchronous message + if len(data) > 4 && data[1] == 0xFF && data[0] == 0xFF { + //locator request + if data[4] == 0x0B && len(data) == 16 { + b.handleLocatorDetected(data) + } + } b.handleCollisionDetected(data) } +// GetLocatorData calls the passed function with the data from the locator +func (b *Driver) GetLocatorData(f func(p Point2D)) { + //CID 0x15 is the code for the locator request + b.PacketChannel() <- b.craftPacket([]uint8{}, 0x02, 0x15) + b.locatorCallback = f +} + +func (b *Driver) handleDataStreaming(data []byte) { + // ensure data is the right length: + if len(data) != 88 { + return + } + + //data packet is the same as for the normal sphero, since the same communication api is used + //only difference in communication is that the "newer" spheros use BLE for communinations + var dataPacket DataStreamingPacket + buffer := bytes.NewBuffer(data[5:]) // skip header + binary.Read(buffer, binary.BigEndian, &dataPacket) + + b.Publish(SensorData, dataPacket) +} + // SetRGB sets the Ollie to the given r, g, and b values func (b *Driver) SetRGB(r uint8, g uint8, bl uint8) { b.packetChannel <- b.craftPacket([]uint8{r, g, bl, 0x01}, 0x02, 0x20) @@ -264,9 +331,16 @@ func (b *Driver) ConfigureCollisionDetection(cc sphero.CollisionConfig) { b.packetChannel <- b.craftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12) } -func (b *Driver) write(packet *packet) (err error) { - buf := append(packet.header, packet.body...) - buf = append(buf, packet.checksum) +//SetDataStreamingConfig passes the config to the sphero to stream sensor data +func (b *Driver) SetDataStreamingConfig(d sphero.DataStreamingConfig) { + buf := new(bytes.Buffer) + binary.Write(buf, binary.BigEndian, d) + b.PacketChannel() <- b.craftPacket(buf.Bytes(), 0x02, 0x11) +} + +func (b *Driver) write(packet *Packet) (err error) { + buf := append(packet.Header, packet.Body...) + buf = append(buf, packet.Checksum) err = b.adaptor().WriteCharacteristic(commandsCharacteristic, buf) if err != nil { fmt.Println("send command error:", err) @@ -279,18 +353,48 @@ func (b *Driver) write(packet *packet) (err error) { return } -func (b *Driver) craftPacket(body []uint8, did byte, cid byte) *packet { +func (b *Driver) craftPacket(body []uint8, did byte, cid byte) *Packet { b.mtx.Lock() defer b.mtx.Unlock() - packet := new(packet) - packet.body = body - dlen := len(packet.body) + 1 - packet.header = []uint8{0xFF, 0xFF, did, cid, b.seq, uint8(dlen)} - packet.checksum = b.calculateChecksum(packet) + packet := new(Packet) + packet.Body = body + dlen := len(packet.Body) + 1 + packet.Header = []uint8{0xFF, 0xFF, did, cid, b.seq, uint8(dlen)} + packet.Checksum = b.calculateChecksum(packet) return packet } +func (b *Driver) handleLocatorDetected(data []uint8) { + //read the unsigned raw values + ux := binary.BigEndian.Uint16(data[5:7]) + uy := binary.BigEndian.Uint16(data[7:9]) + + //convert to signed values + var x, y int16 + + if ux > 32255 { + x = int16(ux - 65535) + } else { + x = int16(ux) + } + + if uy > 32255 { + y = int16(uy - 65535) + } else { + y = int16(uy) + } + + //create point obj + p := new(Point2D) + p.X = x + p.Y = y + + if b.locatorCallback != nil { + b.locatorCallback(*p) + } +} + func (b *Driver) handleCollisionDetected(data []uint8) { if len(data) == ResponsePacketMaxSize { @@ -335,8 +439,8 @@ func (b *Driver) handleCollisionDetected(data []uint8) { b.Publish(Collision, collision) } -func (b *Driver) calculateChecksum(packet *packet) uint8 { - buf := append(packet.header, packet.body...) +func (b *Driver) calculateChecksum(packet *Packet) uint8 { + buf := append(packet.Header, packet.Body...) return calculateChecksum(buf[2:]) } diff --git a/platforms/sphero/ollie/ollie_driver_test.go b/platforms/sphero/ollie/ollie_driver_test.go index a98f73e1..b1ec7048 100644 --- a/platforms/sphero/ollie/ollie_driver_test.go +++ b/platforms/sphero/ollie/ollie_driver_test.go @@ -1,10 +1,15 @@ package ollie import ( + "fmt" + "math" + "strconv" "testing" + "time" "gobot.io/x/gobot" "gobot.io/x/gobot/gobottest" + "gobot.io/x/gobot/platforms/sphero" ) var _ gobot.Driver = (*Driver)(nil) @@ -25,3 +30,84 @@ func TestOllieDriverStartAndHalt(t *testing.T) { gobottest.Assert(t, d.Start(), nil) gobottest.Assert(t, d.Halt(), nil) } + +func TestLocatorData(t *testing.T) { + d := initTestOllieDriver() + + tables := []struct { + x1 byte + x2 byte + y1 byte + y2 byte + x int16 + y int16 + }{ + {0x00, 0x05, 0x00, 0x05, 5, 5}, + {0x00, 0x00, 0x00, 0x00, 0, 0}, + {0x00, 0x0A, 0x00, 0xF0, 10, 240}, + {0x01, 0x00, 0x01, 0x00, 256, 256}, + {0xFF, 0xFE, 0xFF, 0xFE, -1, -1}, + } + + for _, point := range tables { + //0x0B is the locator ID + packet := []byte{0xFF, 0xFF, 0x00, 0x00, 0x0B, point.x1, point.x2, point.y1, point.y2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00} + + d.GetLocatorData(func(p Point2D) { + gobottest.Assert(t, p.Y, point.y) + }) + d.HandleResponses(packet, nil) + } +} + +func TestDataStreaming(t *testing.T) { + d := initTestOllieDriver() + + d.SetDataStreamingConfig(sphero.DefaultDataStreamingConfig()) + + response := false + d.On("sensordata", func(data interface{}) { + cont := data.(DataStreamingPacket) + fmt.Printf("got streaming packet: %+v \n", cont) + gobottest.Assert(t, cont.RawAccX, int16(10)) + response = true + }) + + //example data packet + p1 := []string{"FFFE030053000A003900FAFFFE0007FFFF000000", + "000000000000000000FFECFFFB00010000004B01", + "BD1034FFFF000300000000000000000000000000", + "0000002701FDE500560000000000000065000000", + "0000000000000071", + } + + for _, elem := range p1 { + var bytes []byte + for i := 0; i < len([]rune(elem)); i += 2 { + a := []rune(elem)[i : i+2] + b, _ := strconv.ParseUint(string(a), 16, 16) + c := uint16(b) + bytes = append(bytes, byte(c)) + } + d.HandleResponses(bytes, nil) + + } + + //send empty packet to indicate start of next message + d.HandleResponses([]byte{0xFF}, nil) + time.Sleep(10 * time.Millisecond) + if response == false { + t.Error("no response recieved") + } +} + +func parseBytes(s string) (f byte) { + i, err := strconv.ParseUint(s, 16, 32) + if err != nil { + return + } + + f = byte(math.Float32frombits(uint32(i))) + + return +} diff --git a/platforms/sphero/ollie/ollie_packets.go b/platforms/sphero/ollie/ollie_packets.go index 6da74f59..2e80d72c 100644 --- a/platforms/sphero/ollie/ollie_packets.go +++ b/platforms/sphero/ollie/ollie_packets.go @@ -13,3 +13,89 @@ func DefaultCollisionConfig() sphero.CollisionConfig { Dead: 0x60, } } + +// DataStreamingPacket represents the response from a Data Streaming event +type DataStreamingPacket struct { + // 8000 0000h accelerometer axis X, raw -2048 to 2047 4mG + RawAccX int16 + // 4000 0000h accelerometer axis Y, raw -2048 to 2047 4mG + RawAccY int16 + // 2000 0000h accelerometer axis Z, raw -2048 to 2047 4mG + RawAccZ int16 + // 1000 0000h gyro axis X, raw -32768 to 32767 0.068 degrees + RawGyroX int16 + // 0800 0000h gyro axis Y, raw -32768 to 32767 0.068 degrees + RawGyroY int16 + // 0400 0000h gyro axis Z, raw -32768 to 32767 0.068 degrees + RawGyroZ int16 + // 0200 0000h Reserved + Rsrv1 int16 + // 0100 0000h Reserved + Rsrv2 int16 + // 0080 0000h Reserved + Rsrv3 int16 + // 0040 0000h right motor back EMF, raw -32768 to 32767 22.5 cm + RawRMotorBack int16 + // 0020 0000h left motor back EMF, raw -32768 to 32767 22.5 cm + RawLMotorBack int16 + // 0010 0000h left motor, PWM, raw -2048 to 2047 duty cycle + RawLMotor int16 + // 0008 0000h right motor, PWM raw -2048 to 2047 duty cycle + RawRMotor int16 + // 0004 0000h IMU pitch angle, filtered -179 to 180 degrees + FiltPitch int16 + // 0002 0000h IMU roll angle, filtered -179 to 180 degrees + FiltRoll int16 + // 0001 0000h IMU yaw angle, filtered -179 to 180 degrees + FiltYaw int16 + // 0000 8000h accelerometer axis X, filtered -32768 to 32767 1/4096 G + FiltAccX int16 + // 0000 4000h accelerometer axis Y, filtered -32768 to 32767 1/4096 G + FiltAccY int16 + // 0000 2000h accelerometer axis Z, filtered -32768 to 32767 1/4096 G + FiltAccZ int16 + // 0000 1000h gyro axis X, filtered -20000 to 20000 0.1 dps + FiltGyroX int16 + // 0000 0800h gyro axis Y, filtered -20000 to 20000 0.1 dps + FiltGyroY int16 + // 0000 0400h gyro axis Z, filtered -20000 to 20000 0.1 dps + FiltGyroZ int16 + // 0000 0200h Reserved + Rsrv4 int16 + // 0000 0100h Reserved + Rsrv5 int16 + // 0000 0080h Reserved + Rsrv6 int16 + // 0000 0040h right motor back EMF, filtered -32768 to 32767 22.5 cm + FiltRMotorBack int16 + // 0000 0020h left motor back EMF, filtered -32768 to 32767 22.5 cm + FiltLMotorBack int16 + // 0000 0010h Reserved 1 + Rsrv7 int16 + // 0000 0008h Reserved 2 + Rsrv8 int16 + // // 0000 0004h Reserved 3 + // Rsrv9 int16 + // // 0000 0002h Reserved 4 + // Rsrv10 int16 + // // 0000 0001h Reserved 5 + // Rsrv11 int16 + // 8000 0000h Quaternion Q0 -10000 to 10000 1/10000 Q + Quat0 int16 + // 4000 0000h Quaternion Q1 -10000 to 10000 1/10000 Q + Quat1 int16 + // 2000 0000h Quaternion Q2 -10000 to 10000 1/10000 Q + Quat2 int16 + // 1000 0000h Quaternion Q3 -10000 to 10000 1/10000 Q + Quat3 int16 + // 0800 0000h Odometer X -32768 to 32767 cm + OdomX int16 + // 0400 0000h Odometer Y -32768 to 32767 cm + OdomY int16 + // 0200 0000h AccelOne 0 to 8000 1 mG + AccelOne int16 + // 0100 0000h Velocity X -32768 to 32767 mm/s + VeloX int16 + // 0080 0000h Velocity Y -32768 to 32767 mm/s + VeloY int16 +}