gobot.io/x/gobot/v2@v2.1.0/platforms/sphero/ollie/ollie_driver.go (about)

     1  package ollie
     2  
     3  import (
     4  	"bytes"
     5  	"encoding/binary"
     6  	"fmt"
     7  	"sync"
     8  	"time"
     9  
    10  	"gobot.io/x/gobot/v2"
    11  	"gobot.io/x/gobot/v2/platforms/ble"
    12  	"gobot.io/x/gobot/v2/platforms/sphero"
    13  )
    14  
    15  // Driver is the Gobot driver for the Sphero Ollie robot
    16  type Driver struct {
    17  	name               string
    18  	connection         gobot.Connection
    19  	seq                uint8
    20  	mtx                sync.Mutex
    21  	collisionResponse  []uint8
    22  	packetChannel      chan *Packet
    23  	asyncBuffer        []byte
    24  	asyncMessage       []byte
    25  	locatorCallback    func(p Point2D)
    26  	powerstateCallback func(p PowerStatePacket)
    27  	gobot.Eventer
    28  }
    29  
    30  const (
    31  	// bluetooth service IDs
    32  	spheroBLEService    = "22bb746f2bb075542d6f726568705327"
    33  	robotControlService = "22bb746f2ba075542d6f726568705327"
    34  
    35  	// BLE characteristic IDs
    36  	wakeCharacteristic     = "22bb746f2bbf75542d6f726568705327"
    37  	txPowerCharacteristic  = "22bb746f2bb275542d6f726568705327"
    38  	antiDosCharacteristic  = "22bb746f2bbd75542d6f726568705327"
    39  	commandsCharacteristic = "22bb746f2ba175542d6f726568705327"
    40  	responseCharacteristic = "22bb746f2ba675542d6f726568705327"
    41  
    42  	// SensorData event
    43  	SensorData = "sensordata"
    44  
    45  	// Collision event
    46  	Collision = "collision"
    47  
    48  	// Error event
    49  	Error = "error"
    50  
    51  	// Packet header size
    52  	PacketHeaderSize = 5
    53  
    54  	// Response packet max size
    55  	ResponsePacketMaxSize = 20
    56  
    57  	// Collision Packet data size: The number of bytes following the DLEN field through the end of the packet
    58  	CollisionDataSize = 17
    59  
    60  	// Full size of the collision response
    61  	CollisionResponseSize = PacketHeaderSize + CollisionDataSize
    62  )
    63  
    64  // MotorModes is used to configure the motor
    65  type MotorModes uint8
    66  
    67  // MotorModes required for SetRawMotorValues command
    68  const (
    69  	Off MotorModes = iota
    70  	Forward
    71  	Reverse
    72  	Brake
    73  	Ignore
    74  )
    75  
    76  // Packet describes head, body and checksum for a data package to be sent to the sphero.
    77  type Packet struct {
    78  	Header   []uint8
    79  	Body     []uint8
    80  	Checksum uint8
    81  }
    82  
    83  // Point2D represents a koordinate in 2-Dimensional space
    84  type Point2D struct {
    85  	X int16
    86  	Y int16
    87  }
    88  
    89  // NewDriver creates a Driver for a Sphero Ollie
    90  func NewDriver(a ble.BLEConnector) *Driver {
    91  	n := &Driver{
    92  		name:          gobot.DefaultName("Ollie"),
    93  		connection:    a,
    94  		Eventer:       gobot.NewEventer(),
    95  		packetChannel: make(chan *Packet, 1024),
    96  	}
    97  
    98  	n.AddEvent(Collision)
    99  
   100  	return n
   101  }
   102  
   103  // PacketChannel returns the channel for packets to be sent to the sp
   104  func (b *Driver) PacketChannel() chan *Packet { return b.packetChannel }
   105  
   106  // Sequence returns the Sequence number of the current packet
   107  func (b *Driver) Sequence() uint8 { return b.seq }
   108  
   109  // Connection returns the connection to this Ollie
   110  func (b *Driver) Connection() gobot.Connection { return b.connection }
   111  
   112  // Name returns the name for the Driver
   113  func (b *Driver) Name() string { return b.name }
   114  
   115  // SetName sets the Name for the Driver
   116  func (b *Driver) SetName(n string) { b.name = n }
   117  
   118  // adaptor returns BLE adaptor
   119  func (b *Driver) adaptor() ble.BLEConnector {
   120  	return b.Connection().(ble.BLEConnector)
   121  }
   122  
   123  // Start tells driver to get ready to do work
   124  func (b *Driver) Start() (err error) {
   125  	b.Init()
   126  
   127  	// send commands
   128  	go func() {
   129  		for {
   130  			packet := <-b.packetChannel
   131  			err := b.write(packet)
   132  			if err != nil {
   133  				b.Publish(b.Event(Error), err)
   134  			}
   135  		}
   136  	}()
   137  
   138  	go func() {
   139  		for {
   140  			b.adaptor().ReadCharacteristic(responseCharacteristic)
   141  			time.Sleep(100 * time.Millisecond)
   142  		}
   143  	}()
   144  
   145  	b.ConfigureCollisionDetection(DefaultCollisionConfig())
   146  
   147  	return
   148  }
   149  
   150  // Halt stops Ollie driver (void)
   151  func (b *Driver) Halt() (err error) {
   152  	b.Sleep()
   153  	time.Sleep(750 * time.Microsecond)
   154  	return
   155  }
   156  
   157  // Init is used to initialize the Ollie
   158  func (b *Driver) Init() (err error) {
   159  	b.AntiDOSOff()
   160  	b.SetTXPower(7)
   161  	b.Wake()
   162  
   163  	// subscribe to Sphero response notifications
   164  	b.adaptor().Subscribe(responseCharacteristic, b.HandleResponses)
   165  
   166  	return
   167  }
   168  
   169  // AntiDOSOff turns off Anti-DOS code so we can control Ollie
   170  func (b *Driver) AntiDOSOff() (err error) {
   171  	str := "011i3"
   172  	buf := &bytes.Buffer{}
   173  	buf.WriteString(str)
   174  
   175  	err = b.adaptor().WriteCharacteristic(antiDosCharacteristic, buf.Bytes())
   176  	if err != nil {
   177  		fmt.Println("AntiDOSOff error:", err)
   178  		return err
   179  	}
   180  
   181  	return
   182  }
   183  
   184  // Wake wakes Ollie up so we can play
   185  func (b *Driver) Wake() (err error) {
   186  	buf := []byte{0x01}
   187  
   188  	err = b.adaptor().WriteCharacteristic(wakeCharacteristic, buf)
   189  	if err != nil {
   190  		fmt.Println("Wake error:", err)
   191  		return err
   192  	}
   193  
   194  	return
   195  }
   196  
   197  // SetTXPower sets transmit level
   198  func (b *Driver) SetTXPower(level int) (err error) {
   199  	buf := []byte{byte(level)}
   200  
   201  	err = b.adaptor().WriteCharacteristic(txPowerCharacteristic, buf)
   202  	if err != nil {
   203  		fmt.Println("SetTXLevel error:", err)
   204  		return err
   205  	}
   206  
   207  	return
   208  }
   209  
   210  // HandleResponses handles responses returned from Ollie
   211  func (b *Driver) HandleResponses(data []byte, e error) {
   212  
   213  	//since packets can only be 20 bytes long, we have to puzzle them together
   214  	newMessage := false
   215  
   216  	//append message parts to existing
   217  	if len(data) > 0 && data[0] != 0xFF {
   218  		b.asyncBuffer = append(b.asyncBuffer, data...)
   219  	}
   220  
   221  	//clear message when new one begins (first byte is always 0xFF)
   222  	if len(data) > 0 && data[0] == 0xFF {
   223  		b.asyncMessage = b.asyncBuffer
   224  		b.asyncBuffer = data
   225  		newMessage = true
   226  	}
   227  
   228  	parts := b.asyncMessage
   229  	//3 is the id of data streaming, located at index 2 byte
   230  	if newMessage && len(parts) > 2 && parts[2] == 3 {
   231  		b.handleDataStreaming(parts)
   232  	}
   233  
   234  	//index 1 is the type of the message, 0xFF being a direct response, 0xFE an asynchronous message
   235  	if len(data) > 4 && data[1] == 0xFF && data[0] == 0xFF {
   236  		//locator request
   237  		if data[4] == 0x0B && len(data) == 16 {
   238  			b.handleLocatorDetected(data)
   239  		}
   240  
   241  		if data[4] == 0x09 {
   242  			b.handlePowerStateDetected(data)
   243  		}
   244  	}
   245  
   246  	b.handleCollisionDetected(data)
   247  }
   248  
   249  // GetLocatorData calls the passed function with the data from the locator
   250  func (b *Driver) GetLocatorData(f func(p Point2D)) {
   251  	//CID 0x15 is the code for the locator request
   252  	b.PacketChannel() <- b.craftPacket([]uint8{}, 0x02, 0x15)
   253  	b.locatorCallback = f
   254  }
   255  
   256  // GetPowerState calls the passed function with the Power State information from the sphero
   257  func (b *Driver) GetPowerState(f func(p PowerStatePacket)) {
   258  	//CID 0x20 is the code for the power state
   259  	b.PacketChannel() <- b.craftPacket([]uint8{}, 0x00, 0x20)
   260  	b.powerstateCallback = f
   261  }
   262  
   263  func (b *Driver) handleDataStreaming(data []byte) {
   264  	// ensure data is the right length:
   265  	if len(data) != 88 {
   266  		return
   267  	}
   268  
   269  	//data packet is the same as for the normal sphero, since the same communication api is used
   270  	//only difference in communication is that the "newer" spheros use BLE for communinations
   271  	var dataPacket DataStreamingPacket
   272  	buffer := bytes.NewBuffer(data[5:]) // skip header
   273  	binary.Read(buffer, binary.BigEndian, &dataPacket)
   274  
   275  	b.Publish(SensorData, dataPacket)
   276  }
   277  
   278  // SetRGB sets the Ollie to the given r, g, and b values
   279  func (b *Driver) SetRGB(r uint8, g uint8, bl uint8) {
   280  	b.packetChannel <- b.craftPacket([]uint8{r, g, bl, 0x01}, 0x02, 0x20)
   281  }
   282  
   283  // Roll tells the Ollie to roll
   284  func (b *Driver) Roll(speed uint8, heading uint16) {
   285  	b.packetChannel <- b.craftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x02, 0x30)
   286  }
   287  
   288  // Boost executes the boost macro from within the SSB which takes a
   289  // 1 byte parameter which is either 01h to begin boosting or 00h to stop.
   290  func (b *Driver) Boost(state bool) {
   291  	s := uint8(0x01)
   292  	if !state {
   293  		s = 0x00
   294  	}
   295  	b.packetChannel <- b.craftPacket([]uint8{s}, 0x02, 0x31)
   296  }
   297  
   298  // SetStabilization enables or disables the built-in auto stabilizing features of the Ollie
   299  func (b *Driver) SetStabilization(state bool) {
   300  	s := uint8(0x01)
   301  	if !state {
   302  		s = 0x00
   303  	}
   304  	b.packetChannel <- b.craftPacket([]uint8{s}, 0x02, 0x02)
   305  }
   306  
   307  // SetRotationRate allows you to control the rotation rate that Sphero will use to meet new
   308  // heading commands. A value of 255 jumps to the maximum (currently 400 degrees/sec).
   309  // A value of zero doesn't make much sense so it's interpreted as 1, the minimum.
   310  func (b *Driver) SetRotationRate(speed uint8) {
   311  	b.packetChannel <- b.craftPacket([]uint8{speed}, 0x02, 0x03)
   312  }
   313  
   314  // SetRawMotorValues allows you to take over one or both of the motor output values,
   315  // instead of having the stabilization system control them. Each motor (left and right)
   316  // requires a mode and a power value from 0-255
   317  func (b *Driver) SetRawMotorValues(lmode MotorModes, lpower uint8, rmode MotorModes, rpower uint8) {
   318  	b.packetChannel <- b.craftPacket([]uint8{uint8(lmode), lpower, uint8(rmode), rpower}, 0x02, 0x33)
   319  }
   320  
   321  // SetBackLEDOutput allows you to control the brightness of the back(tail) LED.
   322  func (b *Driver) SetBackLEDOutput(value uint8) {
   323  	b.packetChannel <- b.craftPacket([]uint8{value}, 0x02, 0x21)
   324  }
   325  
   326  // Stop tells the Ollie to stop
   327  func (b *Driver) Stop() {
   328  	b.Roll(0, 0)
   329  }
   330  
   331  // Sleep says Go to sleep
   332  func (b *Driver) Sleep() {
   333  	b.packetChannel <- b.craftPacket([]uint8{0x00, 0x00, 0x00, 0x00, 0x00}, 0x00, 0x22)
   334  }
   335  
   336  // EnableStopOnDisconnect auto-sends a Stop command after losing the connection
   337  func (b *Driver) EnableStopOnDisconnect() {
   338  	b.packetChannel <- b.craftPacket([]uint8{0x00, 0x00, 0x00, 0x01}, 0x02, 0x37)
   339  }
   340  
   341  // ConfigureCollisionDetection configures the sensitivity of the detection.
   342  func (b *Driver) ConfigureCollisionDetection(cc sphero.CollisionConfig) {
   343  	b.packetChannel <- b.craftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12)
   344  }
   345  
   346  // SetDataStreamingConfig passes the config to the sphero to stream sensor data
   347  func (b *Driver) SetDataStreamingConfig(d sphero.DataStreamingConfig) {
   348  	buf := new(bytes.Buffer)
   349  	binary.Write(buf, binary.BigEndian, d)
   350  	b.PacketChannel() <- b.craftPacket(buf.Bytes(), 0x02, 0x11)
   351  }
   352  
   353  func (b *Driver) write(packet *Packet) (err error) {
   354  	buf := append(packet.Header, packet.Body...)
   355  	buf = append(buf, packet.Checksum)
   356  	err = b.adaptor().WriteCharacteristic(commandsCharacteristic, buf)
   357  	if err != nil {
   358  		fmt.Println("send command error:", err)
   359  		return err
   360  	}
   361  
   362  	b.mtx.Lock()
   363  	defer b.mtx.Unlock()
   364  	b.seq++
   365  	return
   366  }
   367  
   368  func (b *Driver) craftPacket(body []uint8, did byte, cid byte) *Packet {
   369  	b.mtx.Lock()
   370  	defer b.mtx.Unlock()
   371  
   372  	packet := new(Packet)
   373  	packet.Body = body
   374  	dlen := len(packet.Body) + 1
   375  	packet.Header = []uint8{0xFF, 0xFF, did, cid, b.seq, uint8(dlen)}
   376  	packet.Checksum = b.calculateChecksum(packet)
   377  	return packet
   378  }
   379  
   380  func (b *Driver) handlePowerStateDetected(data []uint8) {
   381  
   382  	var dataPacket PowerStatePacket
   383  	buffer := bytes.NewBuffer(data[5:]) // skip header
   384  	binary.Read(buffer, binary.BigEndian, &dataPacket)
   385  
   386  	b.powerstateCallback(dataPacket)
   387  }
   388  
   389  func (b *Driver) handleLocatorDetected(data []uint8) {
   390  	//read the unsigned raw values
   391  	ux := binary.BigEndian.Uint16(data[5:7])
   392  	uy := binary.BigEndian.Uint16(data[7:9])
   393  
   394  	//convert to signed values
   395  	var x, y int16
   396  
   397  	if ux > 32255 {
   398  		x = int16(ux - 65535)
   399  	} else {
   400  		x = int16(ux)
   401  	}
   402  
   403  	if uy > 32255 {
   404  		y = int16(uy - 65535)
   405  	} else {
   406  		y = int16(uy)
   407  	}
   408  
   409  	//create point obj
   410  	p := new(Point2D)
   411  	p.X = x
   412  	p.Y = y
   413  
   414  	if b.locatorCallback != nil {
   415  		b.locatorCallback(*p)
   416  	}
   417  }
   418  
   419  func (b *Driver) handleCollisionDetected(data []uint8) {
   420  
   421  	if len(data) == ResponsePacketMaxSize {
   422  		// Check if this is the header of collision response. (i.e. first part of data)
   423  		// Collision response is 22 bytes long. (individual packet size is maxed at 20)
   424  		switch data[1] {
   425  		case 0xFE:
   426  			if data[2] == 0x07 {
   427  				// response code 7 is for a detected collision
   428  				if len(b.collisionResponse) == 0 {
   429  					b.collisionResponse = append(b.collisionResponse, data...)
   430  				}
   431  			}
   432  		}
   433  	} else if len(data) == CollisionResponseSize-ResponsePacketMaxSize {
   434  		// if this is the remaining part of the collision response,
   435  		// then make sure the header and first part of data is already received
   436  		if len(b.collisionResponse) == ResponsePacketMaxSize {
   437  			b.collisionResponse = append(b.collisionResponse, data...)
   438  		}
   439  	} else {
   440  		return // not collision event
   441  	}
   442  
   443  	// check expected sizes
   444  	if len(b.collisionResponse) != CollisionResponseSize || b.collisionResponse[4] != CollisionDataSize {
   445  		return
   446  	}
   447  
   448  	// confirm checksum
   449  	size := len(b.collisionResponse)
   450  	chk := b.collisionResponse[size-1] // last byte is checksum
   451  	if chk != calculateChecksum(b.collisionResponse[2:size-1]) {
   452  		return
   453  	}
   454  
   455  	var collision sphero.CollisionPacket
   456  	buffer := bytes.NewBuffer(b.collisionResponse[5:]) // skip header
   457  	binary.Read(buffer, binary.BigEndian, &collision)
   458  	b.collisionResponse = nil // clear the current response
   459  
   460  	b.Publish(Collision, collision)
   461  }
   462  
   463  func (b *Driver) calculateChecksum(packet *Packet) uint8 {
   464  	buf := append(packet.Header, packet.Body...)
   465  	return calculateChecksum(buf[2:])
   466  }
   467  
   468  func calculateChecksum(buf []byte) byte {
   469  	var calculatedChecksum uint16
   470  	for i := range buf {
   471  		calculatedChecksum += uint16(buf[i])
   472  	}
   473  	return uint8(^(calculatedChecksum % 256))
   474  }