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