Added some new features to the sphero ollie, bb-8 and sprkplus
Signed-off-by: Rearth <davidus2010@gmail.com>
This commit is contained in:
Rearth
parent
aa34bc3c2c
commit
20677abec3
|
|
@ -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:])
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue