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 }