From 62e91760b0a1457dd53ce745d8ae0a7991a1a461 Mon Sep 17 00:00:00 2001
From: Rearth <davidus2010@gmail.com>
Date: Wed, 13 Feb 2019 13:16:52 +0100
Subject: [PATCH] Added methods to read Sphero Power States

---
 platforms/sphero/ollie/ollie_driver.go  | 39 +++++++++++++++++++------
 platforms/sphero/ollie/ollie_packets.go | 14 +++++++++
 2 files changed, 44 insertions(+), 9 deletions(-)

diff --git a/platforms/sphero/ollie/ollie_driver.go b/platforms/sphero/ollie/ollie_driver.go
index d20558ee..307c9475 100644
--- a/platforms/sphero/ollie/ollie_driver.go
+++ b/platforms/sphero/ollie/ollie_driver.go
@@ -14,15 +14,16 @@ import (
 
 // Driver is the Gobot driver for the Sphero Ollie robot
 type Driver struct {
-	name              string
-	connection        gobot.Connection
-	seq               uint8
-	mtx               sync.Mutex
-	collisionResponse []uint8
-	packetChannel     chan *Packet
-	asyncBuffer       []byte
-	asyncMessage      []byte
-	locatorCallback   func(p Point2D)
+	name               string
+	connection         gobot.Connection
+	seq                uint8
+	mtx                sync.Mutex
+	collisionResponse  []uint8
+	packetChannel      chan *Packet
+	asyncBuffer        []byte
+	asyncMessage       []byte
+	locatorCallback    func(p Point2D)
+	powerstateCallback func(p PowerStatePacket)
 	gobot.Eventer
 }
 
@@ -236,6 +237,10 @@ func (b *Driver) HandleResponses(data []byte, e error) {
 		if data[4] == 0x0B && len(data) == 16 {
 			b.handleLocatorDetected(data)
 		}
+
+		if data[4] == 0x09 {
+			b.handlePowerStateDetected(data)
+		}
 	}
 
 	b.handleCollisionDetected(data)
@@ -248,6 +253,13 @@ func (b *Driver) GetLocatorData(f func(p Point2D)) {
 	b.locatorCallback = f
 }
 
+// GetPowerState calls the passed function with the Power State information from the sphero
+func (b *Driver) GetPowerState(f func(p PowerStatePacket)) {
+	//CID 0x20 is the code for the power state
+	b.PacketChannel() <- b.craftPacket([]uint8{}, 0x00, 0x20)
+	b.powerstateCallback = f
+}
+
 func (b *Driver) handleDataStreaming(data []byte) {
 	// ensure data is the right length:
 	if len(data) != 88 {
@@ -365,6 +377,15 @@ func (b *Driver) craftPacket(body []uint8, did byte, cid byte) *Packet {
 	return packet
 }
 
+func (b *Driver) handlePowerStateDetected(data []uint8) {
+
+	var dataPacket PowerStatePacket
+	buffer := bytes.NewBuffer(data[5:]) // skip header
+	binary.Read(buffer, binary.BigEndian, &dataPacket)
+
+	b.powerstateCallback(dataPacket)
+}
+
 func (b *Driver) handleLocatorDetected(data []uint8) {
 	//read the unsigned raw values
 	ux := binary.BigEndian.Uint16(data[5:7])
diff --git a/platforms/sphero/ollie/ollie_packets.go b/platforms/sphero/ollie/ollie_packets.go
index 2e80d72c..74169cde 100644
--- a/platforms/sphero/ollie/ollie_packets.go
+++ b/platforms/sphero/ollie/ollie_packets.go
@@ -14,6 +14,20 @@ func DefaultCollisionConfig() sphero.CollisionConfig {
 	}
 }
 
+// PowerStatePacket contains all data relevant to the power state of the sphero
+type PowerStatePacket struct {
+	// record Version Code
+	RecVer uint8
+	// High-Level State of the Battery; 1=charging, 2=battery ok, 3=battery low, 4=battery critical
+	PowerState uint8
+	// Battery Voltage, scaled in 100th of a Volt, 0x02EF would be 7.51 volts
+	BattVoltage uint16
+	// Number of charges in the total lifetime of the sphero
+	NumCharges uint16
+	//Seconds awake since last charge
+	TimeSinceChg uint16
+}
+
 // DataStreamingPacket represents the response from a Data Streaming event
 type DataStreamingPacket struct {
 	// 8000 0000h	accelerometer axis X, raw	-2048 to 2047	4mG