From 564bb6c8b9dc2354f4885d9f28c4a8ba6454bba6 Mon Sep 17 00:00:00 2001 From: Abram Hindle Date: Sun, 7 Sep 2014 23:01:30 -0600 Subject: [PATCH 1/3] Handle locator stuff with async streaming. see http://orbotixinc.github.io/Sphero-Docs/docs/sphero-api/bootloader-and-sphero.html --- platforms/sphero/sphero_driver.go | 59 +++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/platforms/sphero/sphero_driver.go b/platforms/sphero/sphero_driver.go index 47e3c53c0..5609ca327 100644 --- a/platforms/sphero/sphero_driver.go +++ b/platforms/sphero/sphero_driver.go @@ -37,6 +37,18 @@ type Collision struct { Timestamp uint32 } + +type Locator struct { + // did 02 + // dlen x0b + // XPOS, YPOS, XVEL, YVEL, SOG + Xpos, Ypos, Xvel, Yvel, SOG int16 +} + +const LocatorMask2 uint32 = 0x0C000000 +const AccelOneMask2 uint32 = 0x02000000 +const VelocityMask2 unit32 = 0x01800000 + func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver { s := &SpheroDriver{ Driver: *gobot.NewDriver( @@ -49,6 +61,7 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver { } s.AddEvent("collision") + s.AddEvent("locator") s.AddCommand("SetRGB", func(params map[string]interface{}) interface{} { r := uint8(params["r"].(float64)) g := uint8(params["g"].(float64)) @@ -141,6 +154,10 @@ func (s *SpheroDriver) Start() bool { if evt[2] == 0x07 { s.handleCollisionDetected(evt) } + if evt[2] == 0x02 { + s.handleLocator(evt) + } + } time.Sleep(100 * time.Millisecond) } @@ -204,6 +221,33 @@ func (s *SpheroDriver) ConfigureCollisionDetectionRaw(xThreshold, xSpeed, yThres s.packetChannel <- s.craftPacket([]uint8{0x01, xThreshold, xSpeed, yThreshold, ySpeed, deadTime}, 0x02, 0x12) } +// +//DID,CID,SEQ,DLEN,N,M,MASK,PCNT,MASK2 +//x02,x11,8bit,0eh,16bit,16bit,32bit,8bit,32bit + +func (s *SpheroDriver) ConfigureDataStreaming(seq uint8, n, m uint16, mask uint32, pcnt uint8, mask2 unit32) { + // Meth 0x01 to enable, 0x00 to disable + s.packetChannel <- s.craftPacket([]uint8{seq, 0x0e, + uint8(n >> 8), uint8(n & 0xFF), + uint8(m >> 8), uint8(m & 0xFF), + uint8(mask >> 24 & 0xFF), uint8(mask >> 16 & 0xFF), + uint8(mask >> 8 & 0xFF), uint8(mask & 0xFF), + pcnt, + uint8(mask2 >> 24 & 0xFF), uint8(mask2 >> 16 & 0xFF), + uint8(mask2 >> 8 & 0xFF), uint8(mask2 & 0xFF)}, + 0x02, //DID + 0x11) //CID +} + +// 2 is good! +func (s *SpheroDriver) ConfigureLocatorStreaming(updatesPerSecond int) { + mask2 := ( LocatorMask2 | AccelOneMask2 | VelocityMask2 ) + updates := 400 / updatesPerSecond + s.ConfigureDataStreaming(0x01, updates, 1, 0, 0, mask2) +} + + + func (s *SpheroDriver) configureDefaultCollisionDetection() { s.ConfigureCollisionDetectionRaw(0x40, 0x40, 0x50, 0x50, 0x60) } @@ -228,6 +272,21 @@ func (s *SpheroDriver) handleCollisionDetected(data []uint8) { gobot.Publish(s.Event("collision"), data) } +func (s *SpheroDriver) handleLocator(data []uint8) { + // 22 = 5 byte async header + 16 bytes of data + 1 byte checksum + checksum := data[len(data)-1] + if checksum == calculateChecksum(data[2:len(data)-1]) { + // SOP+SOP+ID+DLEN+DLEN + buffer := bytes.NewBuffer(data[5:]) + var locator Locator + binary.Read(buffer, binary.BigEndian, &locator) + gobot.Publish(s.Event("locator"), locator) + return + } +} + + + func (s *SpheroDriver) getSyncResponse(packet *packet) []byte { s.packetChannel <- packet for i := 0; i < 500; i++ { From e1ad7306fec6198d1dc13f2a4b8dba256f2ca302 Mon Sep 17 00:00:00 2001 From: Abram Hindle Date: Mon, 8 Sep 2014 00:25:06 -0600 Subject: [PATCH 2/3] Locator now is sort of working. Locator example added. --- examples/sphero_locator.go | 67 +++++++++++++++++++++++++++++++ platforms/sphero/sphero_driver.go | 50 ++++++++++++++++------- 2 files changed, 102 insertions(+), 15 deletions(-) create mode 100644 examples/sphero_locator.go diff --git a/examples/sphero_locator.go b/examples/sphero_locator.go new file mode 100644 index 000000000..244fdb27b --- /dev/null +++ b/examples/sphero_locator.go @@ -0,0 +1,67 @@ +package main + +import ( + "fmt" + "time" + + "github.com/edmontongo/gobot" + "github.com/edmontongo/gobot/platforms/sphero" +) + +func main() { + gbot := gobot.NewGobot() + + adaptor := sphero.NewSpheroAdaptor("Sphero", "/dev/rfcomm0") + spheroDriver := sphero.NewSpheroDriver(adaptor, "sphero") + collisions := 0 + work := func() { + + spheroDriver.ConfigureCollisionDetectionRaw(0x10, 0x01, 0x10, 0x01, 200) + + gobot.On(spheroDriver.Event("collision"), func(data interface{}) { + fmt.Printf("Collision Detected!%+v\n", data) + collisions = collisions + 1 + }) + + gobot.On(spheroDriver.Event("locator"), func(data interface{}) { + fmt.Printf("Locator Detected!%+v\n", data) + }) + + + gobot.Every(time.Second, func() { + // just hit the sphero around + //spheroDriver.Roll(uint8(gobot.Rand(1)), uint16(gobot.Rand(360))) + fmt.Printf("Collisions: %v\n", collisions) + + }) + gobot.Every(time.Second, func() { + // Lame, keep reinstalling streaming + // and collisions til we're sure we have a collision! + if (collisions < 1) { + fmt.Printf("Trying to enable Collision Detection!\n") + spheroDriver.ConfigureCollisionDetectionRaw(0x20, 0x20, 0x20, 0x20, 200) + fmt.Printf("Trying to enable LOcator!\n") + + spheroDriver.ConfigureLocatorStreaming(2) + } + }) + + gobot.Every(1*time.Second, func() { + r := uint8(255) + g := uint8(gobot.Rand(255)) + b := uint8(gobot.Rand(255)) + spheroDriver.SetRGB(r, g, b) + }) + + } + + robot := gobot.NewRobot("sphero", + []gobot.Connection{adaptor}, + []gobot.Device{spheroDriver}, + work, + ) + + gbot.AddRobot(robot) + + gbot.Start() +} diff --git a/platforms/sphero/sphero_driver.go b/platforms/sphero/sphero_driver.go index 5609ca327..a1d81fba3 100644 --- a/platforms/sphero/sphero_driver.go +++ b/platforms/sphero/sphero_driver.go @@ -5,7 +5,7 @@ import ( "encoding/binary" "fmt" "time" - + "log" "github.com/edmontongo/gobot" ) @@ -37,17 +37,18 @@ type Collision struct { Timestamp uint32 } - +// OK this is sorta bad because it is hardcoded +// response of all 3 masks type Locator struct { // did 02 // dlen x0b // XPOS, YPOS, XVEL, YVEL, SOG - Xpos, Ypos, Xvel, Yvel, SOG int16 + Xpos, Ypos, Accel, Xvel, Yvel int16 } const LocatorMask2 uint32 = 0x0C000000 const AccelOneMask2 uint32 = 0x02000000 -const VelocityMask2 unit32 = 0x01800000 +const VelocityMask2 uint32 = 0x01800000 func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver { s := &SpheroDriver{ @@ -153,11 +154,13 @@ func (s *SpheroDriver) Start() bool { evt, s.asyncResponse = s.asyncResponse[len(s.asyncResponse)-1], s.asyncResponse[:len(s.asyncResponse)-1] if evt[2] == 0x07 { s.handleCollisionDetected(evt) - } - if evt[2] == 0x02 { + } else if evt[2] == 0x03 { s.handleLocator(evt) + } else { + log.Printf("Async: %v\n", evt[2]) } + } time.Sleep(100 * time.Millisecond) } @@ -221,29 +224,32 @@ func (s *SpheroDriver) ConfigureCollisionDetectionRaw(xThreshold, xSpeed, yThres s.packetChannel <- s.craftPacket([]uint8{0x01, xThreshold, xSpeed, yThreshold, ySpeed, deadTime}, 0x02, 0x12) } -// +// See http://orbotixinc.github.io/Sphero-Docs/docs/sphero-api/bootloader-and-sphero.html //DID,CID,SEQ,DLEN,N,M,MASK,PCNT,MASK2 //x02,x11,8bit,0eh,16bit,16bit,32bit,8bit,32bit - -func (s *SpheroDriver) ConfigureDataStreaming(seq uint8, n, m uint16, mask uint32, pcnt uint8, mask2 unit32) { +// ff fc 02 11 23 0e 00 c8 00 01 00 00 00 00 00 0f 80 00 00 c8 +func (s *SpheroDriver) ConfigureDataStreaming(seq uint8, n, m uint16, mask uint32, pcnt uint8, mask2 uint32) { // Meth 0x01 to enable, 0x00 to disable - s.packetChannel <- s.craftPacket([]uint8{seq, 0x0e, - uint8(n >> 8), uint8(n & 0xFF), - uint8(m >> 8), uint8(m & 0xFF), + s.packetChannel <- s.specialCraftPacket([]uint8{ //seq, 0x0e, + uint8(n >> 8 & 0xFF), uint8(n & 0xFF), + uint8(m >> 8 & 0xFF), uint8(m & 0xFF), uint8(mask >> 24 & 0xFF), uint8(mask >> 16 & 0xFF), uint8(mask >> 8 & 0xFF), uint8(mask & 0xFF), pcnt, uint8(mask2 >> 24 & 0xFF), uint8(mask2 >> 16 & 0xFF), uint8(mask2 >> 8 & 0xFF), uint8(mask2 & 0xFF)}, + 0xFC, 0x02, //DID 0x11) //CID } -// 2 is good! +// 2 is good for updates per second func (s *SpheroDriver) ConfigureLocatorStreaming(updatesPerSecond int) { mask2 := ( LocatorMask2 | AccelOneMask2 | VelocityMask2 ) - updates := 400 / updatesPerSecond - s.ConfigureDataStreaming(0x01, updates, 1, 0, 0, mask2) + //mask2 := uint32(0xaa800000) + updates := uint16(400 / updatesPerSecond) + log.Printf("Sending Data Streaming %v Mask2\n", mask2) + s.ConfigureDataStreaming(0xAB, updates, 1, 0, 0, mask2) } @@ -282,6 +288,8 @@ func (s *SpheroDriver) handleLocator(data []uint8) { binary.Read(buffer, binary.BigEndian, &locator) gobot.Publish(s.Event("locator"), locator) return + } else { + log.Printf("handleLocator failed to parse %v\n", data) } } @@ -303,6 +311,16 @@ func (s *SpheroDriver) getSyncResponse(packet *packet) []byte { return []byte{} } + +func (s *SpheroDriver) specialCraftPacket(body []uint8, sop2 byte, did byte, cid byte) *packet { + packet := new(packet) + packet.body = body + dlen := len(packet.body) + 1 + packet.header = []uint8{0xFF, sop2, did, cid, s.seq, uint8(dlen)} + packet.checksum = s.calculateChecksum(packet) + return packet +} + func (s *SpheroDriver) craftPacket(body []uint8, did byte, cid byte) *packet { packet := new(packet) packet.body = body @@ -312,6 +330,8 @@ func (s *SpheroDriver) craftPacket(body []uint8, did byte, cid byte) *packet { return packet } + + func (s *SpheroDriver) write(packet *packet) { buf := append(packet.header, packet.body...) buf = append(buf, packet.checksum) From 7d4ae54452a0bc3af69c6a9750a116510de7b871 Mon Sep 17 00:00:00 2001 From: Abram Hindle Date: Thu, 25 Sep 2014 22:13:08 -0600 Subject: [PATCH 3/3] Make Sphero Header reading more sane -- align to 0xff 0xff and 0xff 0xfe --- platforms/sphero/sphero_driver.go | 46 ++++++++++++++++++++++++++++--- 1 file changed, 42 insertions(+), 4 deletions(-) diff --git a/platforms/sphero/sphero_driver.go b/platforms/sphero/sphero_driver.go index a1d81fba3..497dd928a 100644 --- a/platforms/sphero/sphero_driver.go +++ b/platforms/sphero/sphero_driver.go @@ -133,11 +133,14 @@ func (s *SpheroDriver) Start() bool { go func() { for { header := s.readHeader() - // log.Printf("header: %x\n", header) + /* if header[2] == 0x7 { + log.Printf("header: %x\n", header) + } */ + if header != nil && len(header) != 0 { body := s.readBody(header[4]) - // log.Printf("body: %x\n", body) if header[1] == 0xFE { + //log.Printf("body: %x\n", body) async := append(header, body...) s.asyncResponse = append(s.asyncResponse, async) } else { @@ -275,7 +278,8 @@ func (s *SpheroDriver) handleCollisionDetected(data []uint8) { return } } - gobot.Publish(s.Event("collision"), data) + log.Printf("handleCollisionDetected failed to parse %v\n", data) + //gobot.Publish(s.Event("collision"), data) } func (s *SpheroDriver) handleLocator(data []uint8) { @@ -361,8 +365,42 @@ func calculateChecksum(buf []byte) uint8 { return uint8(^(calculatedChecksum % 256)) } +func arrShift(arr []uint8) { + for i := 0; i < len(arr)-1; i++ { + arr[i] = arr[i+1] + } +} + func (s *SpheroDriver) readHeader() []uint8 { - return s.readNextChunk(5) + // find ff ff or ff fe + sm := []uint8{0,0,0,0,0} + sm = s.readNextChunk(5) + waste := 0 + // increase strictness, only a valid header will be returned + for (!(sm[0]==0xff && (sm[1]==0xff || (sm[1]==0xfe && sm[2] >= 0x00 && sm[2] <= 0x0a )))) { + arrShift(sm) + small := s.readNextChunk(1) + sm[4] = small[0] + waste = waste + 1 + } + if (sm[2] == 0x07 && sm[4]!=0x11) { + log.Printf("readHeader corrupted 0x07 %v\n", sm) + sm[4] = 0x11 + } + if waste > 0 { + log.Printf("readHeader wasted %v bytes\n", waste) + } + return sm + + /* + for (!(x==0xff && (y==0xff || (y==0xfe && z >= 0x00 && z <= 0x0a)))) { + x = y + y = z + small = s.readNextChunk(1) + z = small[0] + }*/ + //up_to_len := s.readNextChunk(2) + //return []uint8{x,y, z,up_to_len[0], up_to_len[1]} } func (s *SpheroDriver) readBody(length uint8) []uint8 {