1
0
mirror of https://github.com/hybridgroup/gobot.git synced 2025-05-01 13:48:57 +08:00

226 lines
4.9 KiB
Go
Raw Normal View History

2014-04-26 18:07:04 -07:00
package sphero
import (
"fmt"
"github.com/hybridgroup/gobot"
"time"
)
type packet struct {
header []uint8
body []uint8
checksum uint8
}
2014-04-28 11:40:20 -07:00
type SpheroDriver struct {
gobot.Driver
2014-04-26 18:07:04 -07:00
Adaptor *SpheroAdaptor
seq uint8
async_response [][]uint8
sync_response [][]uint8
packet_channel chan *packet
response_channel chan []uint8
}
2014-04-28 11:40:20 -07:00
func NewSpheroDriver(a *SpheroAdaptor) *SpheroDriver {
return &SpheroDriver{
2014-04-26 18:07:04 -07:00
Driver: gobot.Driver{
Events: make(map[string]chan interface{}),
Commands: []string{
"SetRGBC",
"RollC",
"StopC",
"GetRGBC",
"SetBackLEDC",
"SetHeadingC",
"SetStabilizationC",
},
},
Adaptor: a,
packet_channel: make(chan *packet, 1024),
response_channel: make(chan []uint8, 1024),
}
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) Init() bool {
return true
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) Start() bool {
go func() {
for {
2014-04-26 18:07:04 -07:00
packet := <-s.packet_channel
s.write(packet)
}
}()
go func() {
for {
2014-04-26 18:07:04 -07:00
response := <-s.response_channel
s.sync_response = append(s.sync_response, response)
}
}()
go func() {
for {
2014-04-26 18:07:04 -07:00
header := s.readHeader()
if header != nil && len(header) != 0 {
2014-04-26 18:07:04 -07:00
body := s.readBody(header[4])
if header[1] == 0xFE {
async := append(header, body...)
2014-04-26 18:07:04 -07:00
s.async_response = append(s.async_response, async)
} else {
2014-04-26 18:07:04 -07:00
s.response_channel <- append(header, body...)
}
}
}
}()
go func() {
for {
var evt []uint8
2014-04-26 18:07:04 -07:00
for len(s.async_response) != 0 {
evt, s.async_response = s.async_response[len(s.async_response)-1], s.async_response[:len(s.async_response)-1]
if evt[2] == 0x07 {
2014-04-26 18:07:04 -07:00
s.handleCollisionDetected(evt)
}
}
time.Sleep(100 * time.Millisecond)
}
}()
2014-04-26 18:07:04 -07:00
s.configureCollisionDetection()
return true
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) Halt() bool {
go func() {
for {
2014-04-26 18:07:04 -07:00
s.Stop()
}
}()
time.Sleep(1 * time.Second)
return true
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) SetRGB(r uint8, g uint8, b uint8) {
2014-04-26 18:07:04 -07:00
s.packet_channel <- s.craftPacket([]uint8{r, g, b, 0x01}, 0x20)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) GetRGB() []uint8 {
2014-04-26 18:07:04 -07:00
return s.syncResponse(s.craftPacket([]uint8{}, 0x22))
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) SetBackLED(level uint8) {
2014-04-26 18:07:04 -07:00
s.packet_channel <- s.craftPacket([]uint8{level}, 0x21)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) SetHeading(heading uint16) {
2014-04-26 18:07:04 -07:00
s.packet_channel <- s.craftPacket([]uint8{uint8(heading >> 8), uint8(heading & 0xFF)}, 0x01)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) SetStabilization(on bool) {
b := uint8(0x01)
if on == false {
b = 0x00
}
2014-04-26 18:07:04 -07:00
s.packet_channel <- s.craftPacket([]uint8{b}, 0x02)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) Roll(speed uint8, heading uint16) {
2014-04-26 18:07:04 -07:00
s.packet_channel <- s.craftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x30)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) Stop() {
2014-04-26 18:07:04 -07:00
s.Roll(0, 0)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) configureCollisionDetection() {
2014-04-26 18:07:04 -07:00
s.Events["Collision"] = make(chan interface{})
s.packet_channel <- s.craftPacket([]uint8{0x01, 0x40, 0x40, 0x50, 0x50, 0x60}, 0x12)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) handleCollisionDetected(data []uint8) {
2014-04-26 18:07:04 -07:00
gobot.Publish(s.Events["Collision"], data)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) syncResponse(packet *packet) []byte {
2014-04-26 18:07:04 -07:00
s.packet_channel <- packet
for i := 0; i < 500; i++ {
2014-04-26 18:07:04 -07:00
for key := range s.sync_response {
if s.sync_response[key][3] == packet.header[4] && len(s.sync_response[key]) > 6 {
var response []byte
2014-04-26 18:07:04 -07:00
response, s.sync_response = s.sync_response[len(s.sync_response)-1], s.sync_response[:len(s.sync_response)-1]
return response
}
}
time.Sleep(10 * time.Microsecond)
}
return make([]byte, 0)
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) craftPacket(body []uint8, cid byte) *packet {
packet := new(packet)
packet.body = body
dlen := len(packet.body) + 1
2014-04-26 18:07:04 -07:00
packet.header = []uint8{0xFF, 0xFF, 0x02, cid, s.seq, uint8(dlen)}
packet.checksum = s.calculateChecksum(packet)
return packet
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) write(packet *packet) {
buf := append(packet.header, packet.body...)
buf = append(buf, packet.checksum)
2014-04-26 18:07:04 -07:00
length, err := s.Adaptor.sp.Write(buf)
if err != nil {
2014-04-26 18:07:04 -07:00
fmt.Println(s.Name, err)
s.Adaptor.Disconnect()
2014-04-28 11:40:20 -07:00
fmt.Println("Reconnecting to SpheroDriver...")
2014-04-26 18:07:04 -07:00
s.Adaptor.Connect()
return
} else if length != len(buf) {
2014-04-26 18:07:04 -07:00
fmt.Println("Not enough bytes written", s.Name)
}
2014-04-26 18:07:04 -07:00
s.seq += 1
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) calculateChecksum(packet *packet) uint8 {
buf := append(packet.header, packet.body...)
buf = buf[2:]
var calculatedChecksum uint16
for i := range buf {
calculatedChecksum += uint16(buf[i])
}
return uint8(^(calculatedChecksum % 256))
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) readHeader() []uint8 {
2014-04-26 18:07:04 -07:00
data := s.readNextChunk(5)
if data == nil {
return nil
} else {
return data
}
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) readBody(length uint8) []uint8 {
2014-04-26 18:07:04 -07:00
data := s.readNextChunk(length)
if data == nil {
return nil
} else {
return data
}
}
2014-04-28 11:40:20 -07:00
func (s *SpheroDriver) readNextChunk(length uint8) []uint8 {
time.Sleep(1000 * time.Microsecond)
var read = make([]uint8, int(length))
2014-04-26 18:07:04 -07:00
l, err := s.Adaptor.sp.Read(read[:])
if err != nil || length != uint8(l) {
return nil
} else {
return read
}
}