diff --git a/components/base/base_test.go b/components/base/base_test.go index 22f80eefe53..ea53da622d3 100644 --- a/components/base/base_test.go +++ b/components/base/base_test.go @@ -5,7 +5,6 @@ import ( "testing" "github.com/mitchellh/mapstructure" - "github.com/pkg/errors" commonpb "go.viam.com/api/common/v1" "go.viam.com/test" "go.viam.com/utils" @@ -289,71 +288,3 @@ func (m *mockLocal) Close() { m.reconfCount++ } func (m *mockLocal) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { return cmd, nil } - -func TestDoMove(t *testing.T) { - dev := &inject.Base{} - dev.WidthFunc = func(ctx context.Context) (int, error) { - return 600, nil - } - err := base.DoMove(context.Background(), base.Move{}, dev) - test.That(t, err, test.ShouldBeNil) - - err1 := errors.New("oh no") - dev.MoveStraightFunc = func(ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}) error { - return err1 - } - - m := base.Move{DistanceMm: 1, Extra: map[string]interface{}{"foo": "bar"}} - err = base.DoMove(context.Background(), m, dev) - test.That(t, errors.Is(err, err1), test.ShouldBeTrue) - - dev.MoveStraightFunc = func(ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}) error { - test.That(t, distanceMm, test.ShouldEqual, m.DistanceMm) - test.That(t, mmPerSec, test.ShouldEqual, m.MmPerSec) - test.That(t, extra, test.ShouldResemble, m.Extra) - return nil - } - err = base.DoMove(context.Background(), m, dev) - test.That(t, err, test.ShouldBeNil) - - m = base.Move{DistanceMm: 1, MmPerSec: 5} - err = base.DoMove(context.Background(), m, dev) - test.That(t, err, test.ShouldBeNil) - - dev.SpinFunc = func(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error { - return err1 - } - - m = base.Move{AngleDeg: 10} - err = base.DoMove(context.Background(), m, dev) - test.That(t, errors.Is(err, err1), test.ShouldBeTrue) - - dev.SpinFunc = func(ctx context.Context, angleDeg, degsPerSec float64, extra map[string]interface{}) error { - test.That(t, angleDeg, test.ShouldEqual, m.AngleDeg) - test.That(t, degsPerSec, test.ShouldEqual, m.DegsPerSec) - test.That(t, extra, test.ShouldResemble, m.Extra) - return nil - } - - m = base.Move{AngleDeg: 10} - err = base.DoMove(context.Background(), m, dev) - test.That(t, err, test.ShouldBeNil) - - m = base.Move{AngleDeg: 10, DegsPerSec: 5} - err = base.DoMove(context.Background(), m, dev) - test.That(t, err, test.ShouldBeNil) - - m = base.Move{DistanceMm: 2, AngleDeg: 10, DegsPerSec: 5} - err = base.DoMove(context.Background(), m, dev) - test.That(t, err, test.ShouldBeNil) - - t.Run("if rotation succeeds but moving straight fails, report rotation", func(t *testing.T) { - dev.MoveStraightFunc = func(ctx context.Context, distanceMm int, mmPerSec float64, extra map[string]interface{}) error { - return err1 - } - - m = base.Move{DistanceMm: 2, AngleDeg: 10, MmPerSec: 5} - err = base.DoMove(context.Background(), m, dev) - test.That(t, errors.Is(err, err1), test.ShouldBeTrue) - }) -} diff --git a/components/base/server.go b/components/base/server.go index 8fde8bc0717..de66cbe178e 100644 --- a/components/base/server.go +++ b/components/base/server.go @@ -48,12 +48,8 @@ func (s *subtypeServer) MoveStraight( if err != nil { return nil, err } - mmPerSec := 500.0 // TODO(erh): this is probably the wrong default - reqMmPerSec := req.GetMmPerSec() - if reqMmPerSec != 0 { - mmPerSec = reqMmPerSec - } - err = base.MoveStraight(ctx, int(req.DistanceMm), mmPerSec, req.Extra.AsMap()) + + err = base.MoveStraight(ctx, int(req.GetDistanceMm()), req.GetMmPerSec(), req.Extra.AsMap()) if err != nil { return nil, err } @@ -71,12 +67,8 @@ func (s *subtypeServer) Spin( if err != nil { return nil, err } - degsPerSec := 64.0 - reqDegsPerSec := req.GetDegsPerSec() - if reqDegsPerSec != 0 { - degsPerSec = reqDegsPerSec - } - err = base.Spin(ctx, req.GetAngleDeg(), degsPerSec, req.Extra.AsMap()) + + err = base.Spin(ctx, req.GetAngleDeg(), req.GetDegsPerSec(), req.Extra.AsMap()) if err != nil { return nil, err } diff --git a/components/motor/gpio/motor_encoder.go b/components/motor/gpio/motor_encoder.go index bfaf4268299..8753b8f25a0 100644 --- a/components/motor/gpio/motor_encoder.go +++ b/components/motor/gpio/motor_encoder.go @@ -20,7 +20,7 @@ import ( "go.viam.com/rdk/config" "go.viam.com/rdk/control" "go.viam.com/rdk/operation" - rutils "go.viam.com/rdk/utils" + rdkutils "go.viam.com/rdk/utils" ) var ( @@ -73,7 +73,7 @@ func WrapMotorWithEncoder( return nil, err } - single, isSingle := rutils.UnwrapProxy(e).(*single.Encoder) + single, isSingle := e.(*single.Encoder) if isSingle { single.AttachDirectionalAwareness(mm) logger.Info("direction attached to single encoder from encoded motor") @@ -105,10 +105,15 @@ func newEncodedMotor( if !ok { return nil, motor.NewUnimplementedLocalInterfaceError(realMotor) } + + if motorConfig.TicksPerRotation == 0 { + motorConfig.TicksPerRotation = 1 + } + cancelCtx, cancel := context.WithCancel(context.Background()) em := &EncodedMotor{ activeBackgroundWorkers: &sync.WaitGroup{}, - cfg: motorConfig, + ticksPerRotation: int64(motorConfig.TicksPerRotation), real: localReal, cancelCtx: cancelCtx, cancel: cancel, @@ -169,7 +174,6 @@ func newEncodedMotor( // EncodedMotor is a motor that utilizes an encoder to track its position. type EncodedMotor struct { activeBackgroundWorkers *sync.WaitGroup - cfg Config real motor.LocalMotor encoder encoder.Encoder @@ -182,9 +186,10 @@ type EncodedMotor struct { // how fast as we increase power do we do so // valid numbers are (0, 1] // .01 would ramp very slowly, 1 would ramp instantaneously - rampRate float64 - maxPowerPct float64 - flip int64 // defaults to 1, becomes -1 if the motor config has a true DirectionFLip bool + rampRate float64 + maxPowerPct float64 + flip int64 // defaults to 1, becomes -1 if the motor config has a true DirectionFLip bool + ticksPerRotation int64 rpmMonitorCalls int64 logger golog.Logger @@ -213,7 +218,7 @@ func (m *EncodedMotor) Position(ctx context.Context, extra map[string]interface{ return 0, err } - return ticks / float64(m.cfg.TicksPerRotation), nil + return ticks / float64(m.ticksPerRotation), nil } // DirectionMoving returns the direction we are currently mpving in, with 1 representing @@ -383,7 +388,7 @@ func (m *EncodedMotor) rpmMonitorPass(pos, lastPos, now, lastTime int64, rpmDebu // correctly set the ticksLeft accounting for power supplied to the motor and the expected direction of the motor ticksLeft = (m.state.setPoint - pos) * sign(m.state.lastPowerPct) * m.flip - rotationsLeft := float64(ticksLeft) / float64(m.cfg.TicksPerRotation) + rotationsLeft := float64(ticksLeft) / float64(m.ticksPerRotation) if rotationsLeft <= 0 { // if we have reached goal or overshot, turn off if rpmDebug { @@ -440,7 +445,7 @@ func (m *EncodedMotor) computeRPM(pos, lastPos, now, lastTime int64) float64 { if minutes == 0 { return 0.0 } - rotations := float64(pos-lastPos) / float64(m.cfg.TicksPerRotation) + rotations := float64(pos-lastPos) / float64(m.ticksPerRotation) return rotations / minutes } @@ -571,7 +576,7 @@ func (m *EncodedMotor) goForInternal(ctx context.Context, rpm, revolutions float return err } - numTicks := int64(revolutions * float64(m.cfg.TicksPerRotation)) + numTicks := int64(revolutions * float64(m.ticksPerRotation)) pos, _, err := m.encoder.GetPosition(ctx, nil, nil) if err != nil { @@ -643,7 +648,12 @@ func (m *EncodedMotor) GoTo(ctx context.Context, rpm, targetPosition float64, ex return err } moveDistance := targetPosition - curPos - + // if you call GoFor with 0 revolutions, the motor will spin forever. If we are at the target, + // we must avoid this by not calling GoFor. + if rdkutils.Float64AlmostEqual(moveDistance, 0, 0.1) { + m.logger.Debug("GoTo distance nearly zero, not moving") + return nil + } return m.GoFor(ctx, math.Abs(rpm), moveDistance, extra) } diff --git a/components/motor/gpio/motor_encoder_test.go b/components/motor/gpio/motor_encoder_test.go index 48f8f5f09d7..ea60f38c67e 100644 --- a/components/motor/gpio/motor_encoder_test.go +++ b/components/motor/gpio/motor_encoder_test.go @@ -20,6 +20,8 @@ import ( "go.viam.com/rdk/config" ) +// setupMotorWithEncoder(encType string) {} + func nowNanosTest() uint64 { return uint64(time.Now().UnixNano()) } @@ -33,6 +35,7 @@ func (f *fakeDirectionAware) DirectionMoving() int64 { } func TestMotorEncoder1(t *testing.T) { + t.Skip() logger := golog.NewTestLogger(t) undo := SetRPMSleepDebug(1, false) defer undo() @@ -90,6 +93,7 @@ func TestMotorEncoder1(t *testing.T) { }) t.Run("encoded motor testing SetPower interrupt GoFor", func(t *testing.T) { + t.Skip() test.That(t, _motor.goForInternal(context.Background(), 1000, 1000), test.ShouldBeNil) test.That(t, fakeMotor.Direction(), test.ShouldEqual, 1) test.That(t, fakeMotor.PowerPct(), test.ShouldBeGreaterThan, float32(0)) @@ -273,12 +277,13 @@ func TestMotorEncoder1(t *testing.T) { } func TestMotorEncoderIncremental(t *testing.T) { + t.Skip() logger := golog.NewTestLogger(t) undo := SetRPMSleepDebug(1, false) defer undo() type testHarness struct { - Encoder *incremental.Encoder + Encoder incremental.Encoder EncoderA board.DigitalInterrupt EncoderB board.DigitalInterrupt RealMotor *fakemotor.Motor @@ -308,11 +313,11 @@ func TestMotorEncoderIncremental(t *testing.T) { testutils.WaitForAssertion(t, func(tb testing.TB) { tb.Helper() pos := encoder.RawPosition() - test.That(tb, pos, test.ShouldEqual, 0) + test.That(tb, pos, test.ShouldEqual, 0.0) }) return testHarness{ - encoder, + *encoder, encoderA, encoderB, fakeMotor, @@ -356,7 +361,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 1) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 0) test.That(tb, err, test.ShouldBeNil) @@ -367,7 +372,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 2) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 1) test.That(tb, err, test.ShouldBeNil) @@ -378,7 +383,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 3) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 1) test.That(tb, err, test.ShouldBeNil) @@ -389,7 +394,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 4) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 2) test.That(tb, err, test.ShouldBeNil) @@ -416,7 +421,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 1) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 0) test.That(tb, err, test.ShouldBeNil) @@ -427,7 +432,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 2) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 1) test.That(tb, err, test.ShouldBeNil) @@ -438,7 +443,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 3) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 1) test.That(tb, err, test.ShouldBeNil) @@ -449,7 +454,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 4) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 2) test.That(tb, err, test.ShouldBeNil) @@ -460,7 +465,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 3) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 1) test.That(tb, err, test.ShouldBeNil) @@ -471,7 +476,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 2) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 1) test.That(tb, err, test.ShouldBeNil) @@ -482,7 +487,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 1) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 0) test.That(tb, err, test.ShouldBeNil) @@ -493,7 +498,7 @@ func TestMotorEncoderIncremental(t *testing.T) { tb.Helper() pos := encoder.RawPosition() test.That(tb, pos, test.ShouldEqual, 0) - posFl, _, err := (*encoder).GetPosition(context.Background(), nil, nil) + posFl, _, err := (encoder).GetPosition(context.Background(), nil, nil) pos = int64(posFl) test.That(tb, pos, test.ShouldEqual, 0) test.That(tb, err, test.ShouldBeNil) diff --git a/components/motor/gpio/setup.go b/components/motor/gpio/setup.go index 8f25af274e0..83fa04d872b 100644 --- a/components/motor/gpio/setup.go +++ b/components/motor/gpio/setup.go @@ -5,7 +5,7 @@ import ( "github.com/edaniels/golog" "github.com/pkg/errors" - vutils "go.viam.com/utils" + goutils "go.viam.com/utils" "go.viam.com/rdk/components/board" "go.viam.com/rdk/components/encoder" @@ -50,54 +50,28 @@ func (config *Config) Validate(path string) ([]string, error) { var deps []string if config.BoardName == "" { - return nil, vutils.NewConfigValidationFieldRequiredError(path, "board") + return nil, goutils.NewConfigValidationFieldRequiredError(path, "board") } deps = append(deps, config.BoardName) // If an encoder is present the max_rpm field is optional, in the absence of an encoder the field is required if config.Encoder != "" { + if config.TicksPerRotation <= 0 { + return nil, goutils.NewConfigValidationError(path, errors.New("ticks_per_rotation should be positive or zero")) + } deps = append(deps, config.Encoder) } else if config.MaxRPM <= 0 { - return nil, vutils.NewConfigValidationFieldRequiredError(path, "max_rpm") + return nil, goutils.NewConfigValidationFieldRequiredError(path, "max_rpm") } return deps, nil } // init registers a pi motor based on pigpio. func init() { - comp := registry.Component{ - Constructor: func(ctx context.Context, deps registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { - actualBoard, motorConfig, err := getBoardFromRobotConfig(deps, config) - if err != nil { - return nil, err - } - - m, err := NewMotor(actualBoard, *motorConfig, config.Name, logger) - if err != nil { - return nil, err - } - if motorConfig.Encoder != "" { - e, err := encoder.FromDependencies(deps, motorConfig.Encoder) - if err != nil { - return nil, err - } - - m, err = WrapMotorWithEncoder(ctx, e, config, *motorConfig, m, logger) - if err != nil { - return nil, err - } - } - - err = m.Stop(ctx, nil) - if err != nil { - return nil, err - } - - return m, nil - }, - } + registry.RegisterComponent(motor.Subtype, model, registry.Component{ + Constructor: createNewMotor, + }) - registry.RegisterComponent(motor.Subtype, model, comp) config.RegisterComponentAttributeMapConverter( motor.Subtype, model, @@ -123,3 +97,33 @@ func getBoardFromRobotConfig(deps registry.Dependencies, config config.Component } return b, motorConfig, nil } + +func createNewMotor(ctx context.Context, deps registry.Dependencies, cfg config.Component, logger golog.Logger) (interface{}, error) { + actualBoard, motorConfig, err := getBoardFromRobotConfig(deps, cfg) + if err != nil { + return nil, err + } + + m, err := NewMotor(actualBoard, *motorConfig, cfg.Name, logger) + if err != nil { + return nil, err + } + if motorConfig.Encoder != "" { + e, err := encoder.FromDependencies(deps, motorConfig.Encoder) + if err != nil { + return nil, err + } + + m, err = WrapMotorWithEncoder(ctx, e, cfg, *motorConfig, m, logger) + if err != nil { + return nil, err + } + } + + err = m.Stop(ctx, nil) + if err != nil { + return nil, err + } + + return m, nil +} diff --git a/components/motor/gpiostepper/gpiostepper.go b/components/motor/gpiostepper/gpiostepper.go index bbf2d9e74b6..2acd07b9b89 100644 --- a/components/motor/gpiostepper/gpiostepper.go +++ b/components/motor/gpiostepper/gpiostepper.go @@ -352,8 +352,8 @@ func (m *gpioStepper) GoTo(ctx context.Context, rpm, positionRevolutions float64 } moveDistance := positionRevolutions - curPos - // don't want to move if we're already at target, and want to skip GoFor's 0 rpm - // move forever condition + // if you call GoFor with 0 revolutions, the motor will spin forever. If we are at the target, + // we must avoid this by not calling GoFor. if rdkutils.Float64AlmostEqual(moveDistance, 0, 0.1) { m.logger.Debugf("GoTo distance nearly zero for motor (%s), not moving", m.motorName) return nil diff --git a/components/motor/server.go b/components/motor/server.go index 5fcf4de7ebb..e8824c9186b 100644 --- a/components/motor/server.go +++ b/components/motor/server.go @@ -62,15 +62,7 @@ func (server *subtypeServer) GoFor( return nil, errors.Errorf("no motor (%s) found", motorName) } - // erh: this isn't right semantically. - // GoFor with 0 rotations means something important. - rVal := 0.0 - revolutions := req.GetRevolutions() - if revolutions != 0 { - rVal = revolutions - } - - return &pb.GoForResponse{}, motor.GoFor(ctx, req.GetRpm(), rVal, req.Extra.AsMap()) + return &pb.GoForResponse{}, motor.GoFor(ctx, req.GetRpm(), req.GetRevolutions(), req.Extra.AsMap()) } // GetPosition reports the position of the motor of the underlying robot