diff --git a/components/motor/dimensionengineering/sabertooth.go b/components/motor/dimensionengineering/sabertooth.go index a536c91a723..8dc6d7891b1 100644 --- a/components/motor/dimensionengineering/sabertooth.go +++ b/components/motor/dimensionengineering/sabertooth.go @@ -3,7 +3,6 @@ package dimensionengineering import ( "context" - "errors" "fmt" "io" "math" @@ -14,6 +13,7 @@ import ( "github.com/edaniels/golog" "github.com/jacobsa/go-serial/serial" "github.com/mitchellh/mapstructure" + "github.com/pkg/errors" utils "go.viam.com/utils" "go.viam.com/rdk/components/motor" @@ -67,6 +67,9 @@ type Motor struct { // A manager to ensure only a single operation is happening at any given time since commands could overlap on the serial port opMgr operation.SingleOperationManager + + // Name of the motor + motorName string } // Config adds DimensionEngineering-specific config options. @@ -127,7 +130,7 @@ func init() { if !ok { return nil, rdkutils.NewUnexpectedTypeError(conf, config.ConvertedAttributes) } - return NewMotor(ctx, conf, logger) + return NewMotor(ctx, conf, config.Name, logger) }, } registry.RegisterComponent(motor.Subtype, modelName, _motor) @@ -216,7 +219,7 @@ func (cfg *Config) validateValues() error { } // NewMotor returns a Sabertooth driven motor. -func NewMotor(ctx context.Context, c *Config, logger golog.Logger) (motor.LocalMotor, error) { +func NewMotor(ctx context.Context, c *Config, name string, logger golog.Logger) (motor.LocalMotor, error) { globalMu.Lock() defer globalMu.Unlock() @@ -257,6 +260,7 @@ func NewMotor(ctx context.Context, c *Config, logger golog.Logger) (motor.LocalM dirFlip: c.DirectionFlip, minPowerPct: c.MinPowerPct, maxPowerPct: c.MaxPowerPct, + motorName: name, } if err := m.configure(c); err != nil { @@ -384,7 +388,7 @@ func (m *Motor) SetPower(ctx context.Context, powerPct float64, extra map[string } c, err := newCommand(m.c.address, cmd, m.Channel, byte(int(rawSpeed))) if err != nil { - return err + return errors.Wrapf(err, "error in SetPower from motor (%s)", m.motorName) } err = m.c.sendCmd(c) return err @@ -401,7 +405,7 @@ func (m *Motor) GoFor(ctx context.Context, rpm, revolutions float64, extra map[s powerPct, waitDur := goForMath(m.maxRPM, rpm, revolutions) err := m.SetPower(ctx, powerPct, extra) if err != nil { - return err + return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) } if revolutions == 0 { @@ -418,7 +422,7 @@ func (m *Motor) GoFor(ctx context.Context, rpm, revolutions float64, extra map[s // at a specific speed. Regardless of the directionality of the RPM this function will move the motor // towards the specified target/position. func (m *Motor) GoTo(ctx context.Context, rpm, position float64, extra map[string]interface{}) error { - return errors.New("not supported") + return motor.NewGoToUnsupportedError(fmt.Sprintf("Channel %d on Sabertooth %d", m.Channel, m.c.address)) } // GoTillStop moves a motor until stopped by the controller (due to switch or function) or stopFunc. @@ -428,7 +432,8 @@ func (m *Motor) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx c // ResetZeroPosition defines the current position to be zero (+/- offset). func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { - return errors.New("not supported") + return motor.NewResetZeroPositionUnsupportedError(fmt.Sprintf("Channel %d on Sabertooth %d", + m.Channel, m.c.address)) } // Position reports the position in revolutions. diff --git a/components/motor/dmc4000/dmc.go b/components/motor/dmc4000/dmc.go index b47d00e7562..b696b702226 100644 --- a/components/motor/dmc4000/dmc.go +++ b/components/motor/dmc4000/dmc.go @@ -4,7 +4,6 @@ package dmc4000 import ( "bytes" "context" - "errors" "fmt" "io" "math" @@ -16,6 +15,7 @@ import ( "github.com/edaniels/golog" "github.com/jacobsa/go-serial/serial" "github.com/mitchellh/mapstructure" + "github.com/pkg/errors" "go.viam.com/utils" "go.viam.com/utils/usb" @@ -62,6 +62,7 @@ type Motor struct { jogging bool opMgr operation.SingleOperationManager powerPct float64 + motorName string } // Config adds DMC-specific config options. @@ -90,7 +91,7 @@ func init() { _motor := registry.Component{ Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { - return NewMotor(ctx, config.ConvertedAttributes.(*Config), logger) + return NewMotor(ctx, config.ConvertedAttributes.(*Config), config.Name, logger) }, } registry.RegisterComponent(motor.Subtype, modelName, _motor) @@ -114,7 +115,7 @@ func init() { } // NewMotor returns a DMC4000 driven motor. -func NewMotor(ctx context.Context, c *Config, logger golog.Logger) (motor.LocalMotor, error) { +func NewMotor(ctx context.Context, c *Config, name string, logger golog.Logger) (motor.LocalMotor, error) { if c.SerialDevice == "" { devs := usb.Search(usbFilter, func(vendorID, productID int) bool { if vendorID == 0x403 && productID == 0x6001 { @@ -167,6 +168,7 @@ func NewMotor(ctx context.Context, c *Config, logger golog.Logger) (motor.LocalM MaxAcceleration: c.MaxAcceleration, HomeRPM: c.HomeRPM, powerPct: 0.0, + motorName: name, } if m.MaxRPM <= 0 { @@ -542,7 +544,7 @@ func (m *Motor) GoTo(ctx context.Context, rpm, position float64, extra map[strin defer m.c.mu.Unlock() if err := m.doGoTo(rpm, position); err != nil { - return err + return motor.NewGoToUnsupportedError(m.motorName) } return m.opMgr.WaitForSuccess( @@ -601,6 +603,9 @@ func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map m.c.mu.Lock() defer m.c.mu.Unlock() _, err := m.c.sendCmd(fmt.Sprintf("DP%s=%d", m.Axis, int(offset*float64(m.TicksPerRotation)))) + if err != nil { + return errors.Wrapf(err, "error in ResetZeroPosition from motor (%s)", m.motorName) + } return err } @@ -622,7 +627,7 @@ func (m *Motor) Stop(ctx context.Context, extra map[string]interface{}) error { m.jogging = false _, err := m.c.sendCmd(fmt.Sprintf("ST%s", m.Axis)) if err != nil { - return err + return errors.Wrapf(err, "error in Stop function from motor (%s)", m.motorName) } return m.opMgr.WaitForSuccess( @@ -646,6 +651,9 @@ func (m *Motor) IsMoving(ctx context.Context) (bool, error) { // IsPowered returns whether or not the motor is currently moving. func (m *Motor) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { on, err := m.IsMoving(ctx) + if err != nil { + return on, m.powerPct, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) + } return on, m.powerPct, err } diff --git a/components/motor/errors.go b/components/motor/errors.go index ebd23795022..b16bc6329d1 100644 --- a/components/motor/errors.go +++ b/components/motor/errors.go @@ -25,3 +25,8 @@ func NewFeatureUnsupportedError(feature Feature, motorName string) error { func NewZeroRPMError() error { return errors.New("Cannot move motor at 0 RPM") } + +// NewGoToUnsupportedError returns error when a motor is required to support GoTo feature. +func NewGoToUnsupportedError(motorName string) error { + return errors.Errorf("motor with name %s does not support GoTo", motorName) +} diff --git a/components/motor/fake/motor.go b/components/motor/fake/motor.go index ebd8175ea99..25a4f1dc60f 100644 --- a/components/motor/fake/motor.go +++ b/components/motor/fake/motor.go @@ -321,7 +321,7 @@ func (m *Motor) GoTo(ctx context.Context, rpm, pos float64, extra map[string]int // GoTillStop always returns an error. func (m *Motor) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx context.Context) bool) error { - return errors.New("not supported") + return motor.NewGoTillStopUnsupportedError(m.Name) } // ResetZeroPosition resets the zero position. @@ -336,7 +336,7 @@ func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map err := m.Encoder.Reset(ctx, int64(offset*float64(m.TicksPerRotation)), extra) if err != nil { - return err + return errors.Wrapf(err, "error in ResetZeroPosition from motor (%s)", m.Name) } return nil @@ -352,7 +352,7 @@ func (m *Motor) Stop(ctx context.Context, extra map[string]interface{}) error { if m.Encoder != nil { err := m.Encoder.SetSpeed(ctx, 0.0) if err != nil { - return err + return errors.Wrapf(err, "error in Stop from motor (%s)", m.Name) } } return nil diff --git a/components/motor/gpio/basic.go b/components/motor/gpio/basic.go index 184c26c8942..f9fa793f758 100644 --- a/components/motor/gpio/basic.go +++ b/components/motor/gpio/basic.go @@ -17,7 +17,7 @@ import ( // NewMotor constructs a new GPIO based motor on the given board using the // given configuration. -func NewMotor(b board.Board, mc Config, logger golog.Logger) (motor.Motor, error) { +func NewMotor(b board.Board, mc Config, name string, logger golog.Logger) (motor.Motor, error) { if mc.MaxPowerPct == 0 { mc.MaxPowerPct = 1.0 } @@ -40,6 +40,7 @@ func NewMotor(b board.Board, mc Config, logger golog.Logger) (motor.Motor, error maxRPM: mc.MaxRPM, dirFlip: mc.DirectionFlip, logger: logger, + motorName: name, } if mc.Pins.A != "" { @@ -103,6 +104,7 @@ type Motor struct { powerPct float64 maxRPM float64 dirFlip bool + motorName string opMgr operation.SingleOperationManager logger golog.Logger @@ -263,7 +265,7 @@ func (m *Motor) GoFor(ctx context.Context, rpm, revolutions float64, extra map[s powerPct, waitDur := goForMath(m.maxRPM, rpm, revolutions) err := m.SetPower(ctx, powerPct, extra) if err != nil { - return err + return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) } if revolutions == 0 { @@ -295,15 +297,15 @@ func (m *Motor) IsMoving(ctx context.Context) (bool, error) { // GoTo is not supported. func (m *Motor) GoTo(ctx context.Context, rpm, positionRevolutions float64, extra map[string]interface{}) error { - return errors.New("not supported") + return motor.NewGoToUnsupportedError(m.motorName) } // ResetZeroPosition is not supported. func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { - return errors.New("not supported") + return motor.NewResetZeroPositionUnsupportedError(m.motorName) } // GoTillStop is not supported. func (m *Motor) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx context.Context) bool) error { - return motor.NewGoTillStopUnsupportedError("(name unavailable)") + return motor.NewGoTillStopUnsupportedError(m.motorName) } diff --git a/components/motor/gpio/basic_test.go b/components/motor/gpio/basic_test.go index b6168379712..18c0a672c03 100644 --- a/components/motor/gpio/basic_test.go +++ b/components/motor/gpio/basic_test.go @@ -12,6 +12,7 @@ import ( "go.viam.com/rdk/components/board" fakeboard "go.viam.com/rdk/components/board/fake" "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/config" ) const maxRPM = 100 @@ -22,10 +23,14 @@ func TestMotorABPWM(t *testing.T) { b := &fakeboard.Board{GPIOPins: map[string]*fakeboard.GPIOPin{}} logger := golog.NewTestLogger(t) + mc := config.Component{ + Name: "abc", + } + t.Run("motor (A/B/PWM) initialization errors", func(t *testing.T) { m, err := NewMotor(b, Config{ Pins: PinConfig{A: "1", B: "2", PWM: "3"}, MaxPowerPct: 100, PWMFreq: 4000, - }, logger) + }, mc.Name, logger) test.That(t, m, test.ShouldBeNil) test.That(t, err, test.ShouldBeError, errors.New("max_power_pct must be between 0.06 and 1.0")) }) @@ -33,7 +38,7 @@ func TestMotorABPWM(t *testing.T) { m, err := NewMotor(b, Config{ Pins: PinConfig{A: "1", B: "2", PWM: "3"}, MaxRPM: maxRPM, PWMFreq: 4000, - }, logger) + }, mc.Name, logger) test.That(t, err, test.ShouldBeNil) t.Run("motor (A/B/PWM) Off testing", func(t *testing.T) { @@ -133,8 +138,13 @@ func TestMotorDirPWM(t *testing.T) { b := &fakeboard.Board{GPIOPins: map[string]*fakeboard.GPIOPin{}} logger := golog.NewTestLogger(t) + mc := config.Component{ + Name: "fake_motor", + } + t.Run("motor (DIR/PWM) initialization errors", func(t *testing.T) { - m, err := NewMotor(b, Config{Pins: PinConfig{Direction: "1", EnablePinLow: "2", PWM: "3"}, PWMFreq: 4000}, logger) + m, err := NewMotor(b, Config{Pins: PinConfig{Direction: "1", EnablePinLow: "2", PWM: "3"}, PWMFreq: 4000}, + mc.Name, logger) test.That(t, err, test.ShouldBeNil) test.That(t, m.GoFor(ctx, 50, 10, nil), test.ShouldBeError, errors.New("not supported, define max_rpm attribute != 0")) @@ -142,7 +152,7 @@ func TestMotorDirPWM(t *testing.T) { _, err = NewMotor( b, Config{Pins: PinConfig{Direction: "1", EnablePinLow: "2", PWM: "3"}, MaxPowerPct: 100, PWMFreq: 4000}, - logger, + mc.Name, logger, ) test.That(t, err, test.ShouldBeError, errors.New("max_power_pct must be between 0.06 and 1.0")) }) @@ -150,7 +160,7 @@ func TestMotorDirPWM(t *testing.T) { m, err := NewMotor(b, Config{ Pins: PinConfig{Direction: "1", EnablePinLow: "2", PWM: "3"}, MaxRPM: maxRPM, PWMFreq: 4000, - }, logger) + }, mc.Name, logger) test.That(t, err, test.ShouldBeNil) t.Run("motor (DIR/PWM) Off testing", func(t *testing.T) { @@ -211,11 +221,14 @@ func TestMotorAB(t *testing.T) { ctx := context.Background() b := &fakeboard.Board{GPIOPins: map[string]*fakeboard.GPIOPin{}} logger := golog.NewTestLogger(t) + mc := config.Component{ + Name: "fake_motor", + } m, err := NewMotor(b, Config{ Pins: PinConfig{A: "1", B: "2", EnablePinLow: "3"}, MaxRPM: maxRPM, PWMFreq: 4000, - }, logger) + }, mc.Name, logger) test.That(t, err, test.ShouldBeNil) t.Run("motor (A/B) On testing", func(t *testing.T) { @@ -281,16 +294,28 @@ func TestMotorABNoEncoder(t *testing.T) { ctx := context.Background() b := &fakeboard.Board{GPIOPins: map[string]*fakeboard.GPIOPin{}} logger := golog.NewTestLogger(t) + mc := config.Component{ + Name: "fake_motor", + } m, err := NewMotor(b, Config{ Pins: PinConfig{A: "1", B: "2", EnablePinLow: "3"}, PWMFreq: 4000, - }, logger) + }, mc.Name, logger) test.That(t, err, test.ShouldBeNil) t.Run("motor no encoder GoFor testing", func(t *testing.T) { test.That(t, m.GoFor(ctx, 50, 10, nil), test.ShouldBeError, errors.New("not supported, define max_rpm attribute != 0")) }) + + t.Run("motor no encoder GoTo testing", func(t *testing.T) { + test.That(t, m.GoTo(ctx, 50, 10, nil), test.ShouldBeError, errors.New("motor with name fake_motor does not support GoTo")) + }) + + t.Run("motor no encoder ResetZeroPosition testing", func(t *testing.T) { + test.That(t, m.ResetZeroPosition(ctx, 5.0001, nil), test.ShouldBeError, + errors.New("motor with name fake_motor does not support ResetZeroPosition")) + }) } func TestGoForMath(t *testing.T) { diff --git a/components/motor/gpio/setup.go b/components/motor/gpio/setup.go index f5d79de883c..f72592034a8 100644 --- a/components/motor/gpio/setup.go +++ b/components/motor/gpio/setup.go @@ -70,7 +70,7 @@ func init() { return nil, err } - m, err := NewMotor(actualBoard, *motorConfig, logger) + m, err := NewMotor(actualBoard, *motorConfig, config.Name, logger) if err != nil { return nil, err } diff --git a/components/motor/gpiostepper/gpiostepper.go b/components/motor/gpiostepper/gpiostepper.go index 0d848cefa75..095723074c4 100644 --- a/components/motor/gpiostepper/gpiostepper.go +++ b/components/motor/gpiostepper/gpiostepper.go @@ -58,7 +58,7 @@ func init() { return nil, err } - return newGPIOStepper(ctx, actualBoard, *motorConfig, logger) + return newGPIOStepper(ctx, actualBoard, *motorConfig, config.Name, logger) }, } registry.RegisterComponent(motor.Subtype, modelName, _motor) @@ -88,7 +88,9 @@ func getBoardFromRobotConfig(deps registry.Dependencies, config config.Component return b, motorConfig, nil } -func newGPIOStepper(ctx context.Context, b board.Board, mc Config, logger golog.Logger) (motor.Motor, error) { +func newGPIOStepper(ctx context.Context, b board.Board, mc Config, name string, + logger golog.Logger, +) (motor.Motor, error) { if mc.TicksPerRotation == 0 { return nil, errors.New("expected ticks_per_rotation in config for motor") } @@ -98,6 +100,7 @@ func newGPIOStepper(ctx context.Context, b board.Board, mc Config, logger golog. stepsPerRotation: mc.TicksPerRotation, stepperDelay: mc.StepperDelay, logger: logger, + motorName: name, } if mc.Pins.EnablePinHigh != "" { @@ -145,6 +148,7 @@ type gpioStepper struct { enablePinHigh, enablePinLow board.GPIOPin stepPin, dirPin board.GPIOPin logger golog.Logger + motorName string // state lock sync.Mutex @@ -189,7 +193,7 @@ func (m *gpioStepper) SetPower(ctx context.Context, powerPct float64, extra map[ return nil } - return errors.New("gpioStepper doesn't support raw power mode") + return errors.Errorf("gpioStepper doesn't support raw power mode in motor (%s)", m.motorName) } func (m *gpioStepper) startThread(ctx context.Context) { @@ -268,7 +272,7 @@ func (m *gpioStepper) GoFor(ctx context.Context, rpm, revolutions float64, extra err := m.goForInternal(ctx, rpm, revolutions) if err != nil { - return err + return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) } if revolutions == 0 { @@ -317,7 +321,7 @@ func (m *gpioStepper) goForInternal(ctx context.Context, rpm, revolutions float6 func (m *gpioStepper) GoTo(ctx context.Context, rpm, positionRevolutions float64, extra map[string]interface{}) error { curPos, err := m.Position(ctx, extra) if err != nil { - return err + return errors.Wrapf(err, "error in GoTo from motor (%s)", m.motorName) } moveDistance := positionRevolutions - curPos @@ -337,7 +341,7 @@ func (m *gpioStepper) GoTillStop(ctx context.Context, rpm float64, stopFunc func } defer func() { if err := m.Stop(ctx, nil); err != nil { - m.logger.Errorw("failed to turn off motor", "error", err) + m.logger.Errorw("failed to turn off motor (%s)", m.motorName, "error", err) } }() for { @@ -402,7 +406,7 @@ func (m *gpioStepper) stop() { func (m *gpioStepper) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { on, err := m.IsMoving(ctx) if err != nil { - return on, 0.0, err + return on, 0.0, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) } percent := 0.0 if on { diff --git a/components/motor/gpiostepper/gpiostepper_test.go b/components/motor/gpiostepper/gpiostepper_test.go index 5f3c319e7d1..7a07866ceb1 100644 --- a/components/motor/gpiostepper/gpiostepper_test.go +++ b/components/motor/gpiostepper/gpiostepper_test.go @@ -11,6 +11,7 @@ import ( fakeboard "go.viam.com/rdk/components/board/fake" "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/config" ) func Test1(t *testing.T) { @@ -20,32 +21,35 @@ func Test1(t *testing.T) { b := &fakeboard.Board{GPIOPins: make(map[string]*fakeboard.GPIOPin)} mc := Config{} + c := config.Component{ + Name: "fake_gpiostepper", + } // Create motor with no board and default config t.Run("gpiostepper initializing test with no board and default config", func(t *testing.T) { - _, err := newGPIOStepper(ctx, nil, mc, logger) + _, err := newGPIOStepper(ctx, nil, mc, c.Name, logger) test.That(t, err, test.ShouldNotBeNil) }) // Create motor with board and default config t.Run("gpiostepper initializing test with board and default config", func(t *testing.T) { - _, err := newGPIOStepper(ctx, b, mc, logger) + _, err := newGPIOStepper(ctx, b, mc, c.Name, logger) test.That(t, err, test.ShouldNotBeNil) }) mc.Pins = PinConfig{Direction: "b"} - _, err := newGPIOStepper(ctx, b, mc, logger) + _, err := newGPIOStepper(ctx, b, mc, c.Name, logger) test.That(t, err, test.ShouldNotBeNil) mc.Pins.Step = "c" - _, err = newGPIOStepper(ctx, b, mc, logger) + _, err = newGPIOStepper(ctx, b, mc, c.Name, logger) test.That(t, err, test.ShouldNotBeNil) mc.TicksPerRotation = 200 - mm, err := newGPIOStepper(ctx, b, mc, logger) + mm, err := newGPIOStepper(ctx, b, mc, c.Name, logger) test.That(t, err, test.ShouldBeNil) m := mm.(*gpioStepper) diff --git a/components/motor/i2cmotors/ezopmp.go b/components/motor/i2cmotors/ezopmp.go index 66b4a87c4fb..1680e03e394 100644 --- a/components/motor/i2cmotors/ezopmp.go +++ b/components/motor/i2cmotors/ezopmp.go @@ -59,7 +59,7 @@ const modelName = "ezopmp" func init() { _motor := registry.Component{ Constructor: func(ctx context.Context, deps registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { - return NewMotor(ctx, deps, config.ConvertedAttributes.(*AttrConfig), logger) + return NewMotor(ctx, deps, config.ConvertedAttributes.(*AttrConfig), config.Name, logger) }, } registry.RegisterComponent(motor.Subtype, modelName, _motor) @@ -81,6 +81,7 @@ func init() { // Ezopmp represents a motor connected via the I2C protocol. type Ezopmp struct { + motorName string board board.Board bus board.I2C I2CAddress byte @@ -103,7 +104,9 @@ const ( ) // NewMotor returns a motor(Ezopmp) with I2C protocol. -func NewMotor(ctx context.Context, deps registry.Dependencies, c *AttrConfig, logger golog.Logger) (motor.LocalMotor, error) { +func NewMotor(ctx context.Context, deps registry.Dependencies, c *AttrConfig, name string, + logger golog.Logger, +) (motor.LocalMotor, error) { b, err := board.FromDependencies(deps, c.BoardName) if err != nil { return nil, err @@ -126,6 +129,7 @@ func NewMotor(ctx context.Context, deps registry.Dependencies, c *AttrConfig, lo logger: logger, maxPowerPct: 1.0, powerPct: 0.0, + motorName: name, } flowRate, err := m.findMaxFlowRate(ctx) @@ -283,15 +287,15 @@ func (m *Ezopmp) GoFor(ctx context.Context, mLPerMin, mins float64, extra map[st switch speed := math.Abs(mLPerMin); { case speed < 0.5: - return errors.New("motor cannot move this slowly") + return errors.Errorf("motor (%s) cannot move this slowly", m.motorName) case speed > m.maxFlowRate: - return errors.Errorf("max continuous flow rate is: %f", m.maxFlowRate) + return errors.Errorf("max continuous flow rate of motor (%s) is: %f", m.motorName, m.maxFlowRate) } commandString := "DC," + strconv.FormatFloat(mLPerMin, 'f', -1, 64) + "," + strconv.FormatFloat(mins, 'f', -1, 64) command := []byte(commandString) if err := m.writeRegWithCheck(ctx, command); err != nil { - return err + return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) } return m.opMgr.WaitTillNotPowered(ctx, time.Millisecond, m, m.Stop) @@ -302,15 +306,15 @@ func (m *Ezopmp) GoFor(ctx context.Context, mLPerMin, mins float64, extra map[st func (m *Ezopmp) GoTo(ctx context.Context, mLPerMin, mins float64, extra map[string]interface{}) error { switch speed := math.Abs(mLPerMin); { case speed < 0.5: - return errors.New("motor cannot move this slowly") + return errors.Errorf("motor (%s) cannot move this slowly", m.motorName) case speed > 105: - return errors.New("motor cannot move this fast") + return errors.Errorf("motor (%s) cannot move this fast", m.motorName) } commandString := "D," + strconv.FormatFloat(mLPerMin, 'f', -1, 64) + "," + strconv.FormatFloat(mins, 'f', -1, 64) command := []byte(commandString) if err := m.writeRegWithCheck(ctx, command); err != nil { - return err + return errors.Wrapf(err, "error in GoTo from motor (%s)", m.motorName) } return m.opMgr.WaitTillNotPowered(ctx, time.Millisecond, m, m.Stop) } @@ -326,11 +330,11 @@ func (m *Ezopmp) Position(ctx context.Context, extra map[string]interface{}) (fl command := []byte(totVolDispensed) writeErr := m.writeReg(ctx, command) if writeErr != nil { - return 0, writeErr + return 0, errors.Wrapf(writeErr, "error in Position from motor (%s)", m.motorName) } val, err := m.readReg(ctx) if err != nil { - return 0, err + return 0, errors.Wrapf(err, "error in Position from motor (%s)", m.motorName) } splitMsg := strings.Split(string(val), ",") floatVal, err := strconv.ParseFloat(splitMsg[1], 64) @@ -362,18 +366,18 @@ func (m *Ezopmp) IsPowered(ctx context.Context, extra map[string]interface{}) (b command := []byte(dispenseStatus) writeErr := m.writeReg(ctx, command) if writeErr != nil { - return false, 0, writeErr + return false, 0, errors.Wrapf(writeErr, "error in IsPowered from motor (%s)", m.motorName) } val, err := m.readReg(ctx) if err != nil { - return false, 0, err + return false, 0, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) } splitMsg := strings.Split(string(val), ",") pumpStatus, err := strconv.ParseFloat(splitMsg[2], 64) if err != nil { - return false, 0, err + return false, 0, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) } if pumpStatus == 1 || pumpStatus == -1 { @@ -384,5 +388,5 @@ func (m *Ezopmp) IsPowered(ctx context.Context, extra map[string]interface{}) (b // GoTillStop is unimplemented. func (m *Ezopmp) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx context.Context) bool) error { - return motor.NewGoTillStopUnsupportedError("(name unavailable)") + return motor.NewGoTillStopUnsupportedError(m.motorName) } diff --git a/components/motor/roboclaw/roboclaw.go b/components/motor/roboclaw/roboclaw.go index e27a06a4908..6b30b211882 100644 --- a/components/motor/roboclaw/roboclaw.go +++ b/components/motor/roboclaw/roboclaw.go @@ -118,12 +118,13 @@ func newRoboClaw(deps registry.Dependencies, config config.Component, logger gol return nil, err } - return &roboclawMotor{conn: c, conf: motorConfig, addr: uint8(motorConfig.Address), logger: logger}, nil + return &roboclawMotor{name: config.Name, conn: c, conf: motorConfig, addr: uint8(motorConfig.Address), logger: logger}, nil } var _ = motor.LocalMotor(&roboclawMotor{}) type roboclawMotor struct { + name string conn *roboclaw.Roboclaw conf *AttrConfig @@ -251,5 +252,5 @@ func (m *roboclawMotor) IsPowered(ctx context.Context, extra map[string]interfac } func (m *roboclawMotor) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx context.Context) bool) error { - return motor.NewGoTillStopUnsupportedError("(name unavailable)") + return motor.NewGoTillStopUnsupportedError(m.name) } diff --git a/components/motor/tmcstepper/stepper_motor_tmc.go b/components/motor/tmcstepper/stepper_motor_tmc.go index c79454fba79..1cf4999769e 100644 --- a/components/motor/tmcstepper/stepper_motor_tmc.go +++ b/components/motor/tmcstepper/stepper_motor_tmc.go @@ -71,7 +71,7 @@ const ( func init() { _motor := registry.Component{ Constructor: func(ctx context.Context, deps registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { - return NewMotor(ctx, deps, *config.ConvertedAttributes.(*TMC5072Config), logger) + return NewMotor(ctx, deps, *config.ConvertedAttributes.(*TMC5072Config), config.Name, logger) }, } registry.RegisterComponent(motor.Subtype, modelname, _motor) @@ -107,6 +107,7 @@ type Motor struct { logger golog.Logger opMgr operation.SingleOperationManager powerPct float64 + motorName string } // TMC5072 Values. @@ -151,7 +152,9 @@ const ( ) // NewMotor returns a TMC5072 driven motor. -func NewMotor(ctx context.Context, deps registry.Dependencies, c TMC5072Config, logger golog.Logger) (motor.LocalMotor, error) { +func NewMotor(ctx context.Context, deps registry.Dependencies, c TMC5072Config, name string, + logger golog.Logger, +) (motor.LocalMotor, error) { b, err := board.FromDependencies(deps, c.BoardName) if err != nil { return nil, errors.Errorf("%q is not a board", c.BoardName) @@ -190,6 +193,7 @@ func NewMotor(ctx context.Context, deps registry.Dependencies, c TMC5072Config, maxAcc: c.MaxAcceleration, fClk: baseClk / c.CalFactor, logger: logger, + motorName: name, } rawMaxAcc := m.rpmsToA(m.maxAcc) @@ -389,7 +393,7 @@ func (m *Motor) GetSG(ctx context.Context) (int32, error) { func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (float64, error) { rawPos, err := m.readReg(ctx, xActual) if err != nil { - return 0, err + return 0, errors.Wrapf(err, "error in Position from motor (%s)", m.motorName) } return float64(rawPos) / float64(m.stepsPerRev), nil } @@ -437,7 +441,7 @@ func (m *Motor) GoFor(ctx context.Context, rpm, rotations float64, extra map[str curPos, err := m.Position(ctx, extra) if err != nil { - return err + return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) } var d int64 = 1 @@ -485,7 +489,7 @@ func (m *Motor) GoTo(ctx context.Context, rpm, positionRevolutions float64, extr m.writeReg(ctx, xTarget, int32(positionRevolutions)), ) if err != nil { - return err + return errors.Wrapf(err, "error in GoTo from motor (%s)", m.motorName) } return m.opMgr.WaitForSuccess( @@ -498,6 +502,9 @@ func (m *Motor) GoTo(ctx context.Context, rpm, positionRevolutions float64, extr // IsPowered returns true if the motor is currently moving. func (m *Motor) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { on, err := m.IsMoving(ctx) + if err != nil { + return on, m.powerPct, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) + } return on, m.powerPct, err } @@ -505,7 +512,7 @@ func (m *Motor) IsPowered(ctx context.Context, extra map[string]interface{}) (bo func (m *Motor) IsStopped(ctx context.Context) (bool, error) { stat, err := m.readReg(ctx, rampStat) if err != nil { - return false, err + return false, errors.Wrapf(err, "error in IsStopped from motor (%s)", m.motorName) } // Look for vzero flag return stat&0x400 == 0x400, nil @@ -642,9 +649,9 @@ func (m *Motor) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx c func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { on, _, err := m.IsPowered(ctx, extra) if err != nil { - return err + return errors.Wrapf(err, "error in ResetZeroPosition from motor (%s)", m.motorName) } else if on { - return errors.New("can't zero while moving") + return errors.Errorf("can't zero motor (%s) while moving", m.motorName) } return multierr.Combine( m.writeReg(ctx, rampMode, modeHold),