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) }