Move rest of spin functions into spin.go in sensor base (viamrobotics…

…#2981)
maximpertsov · Sep 25, 2023 · 0b1d3b9 · 0b1d3b9
1 parent 7d633dd
commit 0b1d3b9
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 55 deletions.
diff --git 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
@@ -5,13 +5,68 @@ import (
 	"math"
 	"time"
 
+	"github.com/golang/geo/r3"
 	"github.com/pkg/errors"
 	"go.viam.com/utils"
 
 	"go.viam.com/rdk/components/movementsensor"
 	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)
 				}