From 0b1d3b9e55c6924f305291ee2598fb979d4e704d Mon Sep 17 00:00:00 2001
From: randhid <35934754+randhid@users.noreply.github.com>
Date: Mon, 25 Sep 2023 16:52:33 -0400
Subject: [PATCH] Move rest of spin functions into spin.go in sensor base
 (#2981)

---
 .../base/sensorcontrolled/sensorcontrolled.go | 52 ----------------
 components/base/sensorcontrolled/spin.go      | 61 ++++++++++++++++++-
 2 files changed, 58 insertions(+), 55 deletions(-)

diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go
index 371a786dd6c..e9b3e635c12 100644
--- a/components/base/sensorcontrolled/sensorcontrolled.go
+++ b/components/base/sensorcontrolled/sensorcontrolled.go
@@ -3,7 +3,6 @@ package sensorcontrolled
 
 import (
 	"context"
-	"math"
 	"sync"
 	"time"
 
@@ -24,8 +23,6 @@ const (
 	velocitiesPollTime = 5 * time.Millisecond
 	boundCheckTurn     = 2.0
 	boundCheckTarget   = 5.0
-	oneTurn            = 360.0
-	increment          = 0.01
 	sensorDebug        = false
 )
 
@@ -173,55 +170,6 @@ func (sb *sensorBase) isPolling() bool {
 	return sb.sensorLoopPolling
 }
 
-// Spin commands a base to turn about its center at a angular speed and for a specific angle.
-func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error {
-	if int(angleDeg) >= 360 {
-		sb.setPolling(false)
-		sb.logger.Warn("feedback for spin calls over 360 not supported yet, spinning without sensor")
-		return sb.controlledBase.Spin(ctx, angleDeg, degsPerSec, nil)
-	}
-	ctx, done := sb.opMgr.New(ctx)
-	defer done()
-	// check if a sensor context has been started
-	if sb.sensorLoopDone != nil {
-		sb.sensorLoopDone()
-	}
-
-	sb.setPolling(true)
-	// start a sensor context for the sensor loop based on the longstanding base
-	// creator context
-	var sensorCtx context.Context
-	sensorCtx, sb.sensorLoopDone = context.WithCancel(context.Background())
-	if err := sb.stopSpinWithSensor(sensorCtx, angleDeg, degsPerSec); err != nil {
-		return err
-	}
-
-	// starts a goroutine from within wheeled base's runAll function to run motors in the background
-	if err := sb.startRunningMotors(ctx, angleDeg, degsPerSec); err != nil {
-		return err
-	}
-
-	// IsMoving returns true when moving, which is not a success condition for our control loop
-	baseStopped := func(ctx context.Context) (bool, error) {
-		moving, err := sb.IsMoving(ctx)
-		return !moving, err
-	}
-	return sb.opMgr.WaitForSuccess(
-		ctx,
-		yawPollTime,
-		baseStopped,
-	)
-}
-
-func (sb *sensorBase) startRunningMotors(ctx context.Context, angleDeg, degsPerSec float64) error {
-	if math.Signbit(angleDeg) != math.Signbit(degsPerSec) {
-		degsPerSec *= -1
-	}
-	return sb.controlledBase.SetVelocity(ctx,
-		r3.Vector{X: 0, Y: 0, Z: 0},
-		r3.Vector{X: 0, Y: 0, Z: degsPerSec}, nil)
-}
-
 func (sb *sensorBase) MoveStraight(
 	ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{},
 ) error {
diff --git a/components/base/sensorcontrolled/spin.go b/components/base/sensorcontrolled/spin.go
index 682ca6f9dc1..1116fc7cc8d 100644
--- a/components/base/sensorcontrolled/spin.go
+++ b/components/base/sensorcontrolled/spin.go
@@ -5,6 +5,7 @@ import (
 	"math"
 	"time"
 
+	"github.com/golang/geo/r3"
 	"github.com/pkg/errors"
 	"go.viam.com/utils"
 
@@ -12,6 +13,60 @@ import (
 	rdkutils "go.viam.com/rdk/utils"
 )
 
+const (
+	increment = 0.01 // angle fraction multiplier to check
+	oneTurn   = 360.0
+)
+
+// Spin commands a base to turn about its center at a angular speed and for a specific angle.
+func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error {
+	if int(angleDeg) >= 360 {
+		sb.setPolling(false)
+		sb.logger.Warn("feedback for spin calls over 360 not supported yet, spinning without sensor")
+		return sb.controlledBase.Spin(ctx, angleDeg, degsPerSec, nil)
+	}
+	ctx, done := sb.opMgr.New(ctx)
+	defer done()
+	// check if a sensor context has been started
+	if sb.sensorLoopDone != nil {
+		sb.sensorLoopDone()
+	}
+
+	sb.setPolling(true)
+	// start a sensor context for the sensor loop based on the longstanding base
+	// creator context
+	var sensorCtx context.Context
+	sensorCtx, sb.sensorLoopDone = context.WithCancel(context.Background())
+	if err := sb.stopSpinWithSensor(sensorCtx, angleDeg, degsPerSec); err != nil {
+		return err
+	}
+
+	// starts a goroutine from within wheeled base's runAll function to run motors in the background
+	if err := sb.startRunningMotors(ctx, angleDeg, degsPerSec); err != nil {
+		return err
+	}
+
+	// IsMoving returns true when moving, which is not a success condition for our control loop
+	baseStopped := func(ctx context.Context) (bool, error) {
+		moving, err := sb.IsMoving(ctx)
+		return !moving, err
+	}
+	return sb.opMgr.WaitForSuccess(
+		ctx,
+		yawPollTime,
+		baseStopped,
+	)
+}
+
+func (sb *sensorBase) startRunningMotors(ctx context.Context, angleDeg, degsPerSec float64) error {
+	if math.Signbit(angleDeg) != math.Signbit(degsPerSec) {
+		degsPerSec *= -1
+	}
+	return sb.controlledBase.SetVelocity(ctx,
+		r3.Vector{X: 0, Y: 0, Z: 0},
+		r3.Vector{X: 0, Y: 0, Z: degsPerSec}, nil)
+}
+
 func (sb *sensorBase) stopSpinWithSensor(
 	ctx context.Context, angleDeg, degsPerSec float64,
 ) error {
@@ -78,9 +133,9 @@ func (sb *sensorBase) stopSpinWithSensor(
 
 				if sensorDebug {
 					sb.logger.Debugf("minTravel %t, atTarget %t, overshot %t", minTravel, atTarget, overShot)
-					sb.logger.Debugf("angleDeg %.2f, increment %.2f", // , fullTurns %d",
-						angleDeg, increment) // , fullTurns)
-					sb.logger.Debugf("currYaw %.2f, startYaw %.2f, targetYaw %.2f",
+					sb.logger.Debugf("angleDeg %.2f", angleDeg)
+					sb.logger.Debugf(
+						"currYaw %.2f, startYaw %.2f, targetYaw %.2f",
 						currYaw, startYaw, targetYaw)
 				}