From 87c0572e4a1f8497adb1b7e2cac2e36fefc4a764 Mon Sep 17 00:00:00 2001
From: Bruno Albuquerque <bga@openly.com>
Date: Tue, 26 Sep 2023 11:26:07 -0700
Subject: [PATCH] sphero: Add support for calibration

-  Includes a sample program to show how it works.
---
 examples/sphero_calibration.go    | 67 +++++++++++++++++++++++++++++++
 platforms/sphero/sphero_driver.go | 35 ++++++++++++++++
 2 files changed, 102 insertions(+)
 create mode 100644 examples/sphero_calibration.go

diff --git a/examples/sphero_calibration.go b/examples/sphero_calibration.go
new file mode 100644
index 00000000..d48d0542
--- /dev/null
+++ b/examples/sphero_calibration.go
@@ -0,0 +1,67 @@
+//go:build example
+// +build example
+
+//
+// Do not build by default.
+
+package main
+
+import (
+	"gobot.io/x/gobot/v2"
+	"gobot.io/x/gobot/v2/api"
+	"gobot.io/x/gobot/v2/platforms/keyboard"
+	"gobot.io/x/gobot/v2/platforms/sphero"
+)
+
+func main() {
+	master := gobot.NewMaster()
+	a := api.NewAPI(master)
+	a.Start()
+
+	ballConn := sphero.NewAdaptor("/dev/rfcomm0")
+	ball := sphero.NewSpheroDriver(ballConn)
+
+	keys := keyboard.NewDriver()
+
+	calibrating := false
+
+	work := func() {
+		keys.On(keyboard.Key, func(data interface{}) {
+			key := data.(keyboard.KeyEvent)
+
+			switch key.Key {
+			case keyboard.ArrowUp:
+				if calibrating {
+					break
+				}
+				ball.Roll(100, 0)
+			case keyboard.ArrowDown:
+				if calibrating {
+					break
+				}
+				ball.Roll(100, 100)
+			case keyboard.ArrowLeft:
+				ball.Roll(100, 270)
+			case keyboard.ArrowRight:
+				ball.Roll(100, 90)
+			case keyboard.Spacebar:
+				if calibrating {
+					ball.FinishCalibration()
+				} else {
+					ball.StartCalibration()
+				}
+				calibrating = !calibrating
+			}
+		})
+	}
+
+	robot := gobot.NewRobot("sphero-calibration",
+		[]gobot.Connection{ballConn},
+		[]gobot.Device{ball, keys},
+		work,
+	)
+
+	master.AddRobot(robot)
+
+	master.Start()
+}
diff --git a/platforms/sphero/sphero_driver.go b/platforms/sphero/sphero_driver.go
index 04b74f42..e1f568a9 100644
--- a/platforms/sphero/sphero_driver.go
+++ b/platforms/sphero/sphero_driver.go
@@ -37,6 +37,7 @@ type SpheroDriver struct {
 	syncResponse    [][]uint8
 	packetChannel   chan *packet
 	responseChannel chan []uint8
+	originalColor   []uint8 // Only used for calibration.
 	gobot.Eventer
 	gobot.Commander
 }
@@ -322,6 +323,40 @@ func (s *SpheroDriver) ConfigureCollisionDetection(cc CollisionConfig) {
 	s.packetChannel <- s.craftPacket([]uint8{cc.Method, cc.Xt, cc.Yt, cc.Xs, cc.Ys, cc.Dead}, 0x02, 0x12)
 }
 
+// SetCalibration sets up Sphero for manual heading calibration.
+// It does this by turning on the tail light (so you can tell where it's
+// facing) and disabling stabilization (so you can adjust the heading).
+//
+// When done, call FinishCalibration to set the new heading, and re-enable
+// stabilization.
+func (s *SpheroDriver) StartCalibration() {
+	s.mtx.Lock()
+	s.originalColor = s.GetRGB()
+	s.SetRGB(0, 0, 0)
+	s.SetBackLED(127)
+	s.SetStabilization(false)
+	s.mtx.Unlock()
+}
+
+// FinishCalibration ends Sphero's calibration mode, by setting
+// the new heading as current, and re-enabling normal defaults. This is a NOP
+// in case StartCalibration was not called.
+func (s *SpheroDriver) FinishCalibration() {
+	s.mtx.Lock()
+	if s.originalColor == nil {
+		// Piggybacking on the original color being set to know if we are
+		// calibrating or not.
+		return
+	}
+
+	s.SetHeading(0)
+	s.SetRGB(s.originalColor[0], s.originalColor[1], s.originalColor[2])
+	s.SetBackLED(0)
+	s.SetStabilization(true)
+	s.originalColor = nil
+	s.mtx.Unlock()
+}
+
 func (s *SpheroDriver) enableStopOnDisconnect() {
 	s.packetChannel <- s.craftPacket([]uint8{0x00, 0x00, 0x00, 0x01}, 0x02, 0x37)
 }