From d35dd92240da0549be42f71e24885ebe3d018d76 Mon Sep 17 00:00:00 2001 From: martha-johnston <106617924+martha-johnston@users.noreply.github.com> Date: Mon, 18 Nov 2024 08:17:15 -0800 Subject: [PATCH] [RSDK-9013][RSDK-9014][RSDK-9016][RSDK-9017][RSDK-9018] remove all grippers, encoders, and motors (#4529) --- components/encoder/ams/ams_as5048.go | 333 -------- components/encoder/ams/ams_as5048_nonlinux.go | 2 - components/encoder/ams/ams_as5048_test.go | 175 ---- components/encoder/register/register.go | 1 - components/gripper/register/register.go | 1 - components/gripper/robotiq/gripper.go | 307 -------- .../motor/dimensionengineering/common.go | 44 -- .../motor/dimensionengineering/sabertooth.go | 561 ------------- .../dimensionengineering/sabertooth_test.go | 510 ------------ components/motor/register/register.go | 3 - .../motor/tmcstepper/stepper_motor_tmc.go | 740 ----------------- .../tmcstepper/stepper_motor_tmc_nonlinux.go | 2 - .../tmcstepper/stepper_motor_tmc_test.go | 744 ------------------ components/motor/ulnstepper/28byj-48.go | 473 ----------- components/motor/ulnstepper/28byj-48_test.go | 403 ---------- 15 files changed, 4299 deletions(-) delete mode 100644 components/encoder/ams/ams_as5048.go delete mode 100644 components/encoder/ams/ams_as5048_nonlinux.go delete mode 100644 components/encoder/ams/ams_as5048_test.go delete mode 100644 components/gripper/robotiq/gripper.go delete mode 100644 components/motor/dimensionengineering/common.go delete mode 100644 components/motor/dimensionengineering/sabertooth.go delete mode 100644 components/motor/dimensionengineering/sabertooth_test.go delete mode 100644 components/motor/tmcstepper/stepper_motor_tmc.go delete mode 100644 components/motor/tmcstepper/stepper_motor_tmc_nonlinux.go delete mode 100644 components/motor/tmcstepper/stepper_motor_tmc_test.go delete mode 100644 components/motor/ulnstepper/28byj-48.go delete mode 100644 components/motor/ulnstepper/28byj-48_test.go diff --git a/components/encoder/ams/ams_as5048.go b/components/encoder/ams/ams_as5048.go deleted file mode 100644 index 9e3811548e9..00000000000 --- a/components/encoder/ams/ams_as5048.go +++ /dev/null @@ -1,333 +0,0 @@ -//go:build linux - -// Package ams implements the AMS_AS5048 encoder -package ams - -import ( - "context" - "fmt" - "math" - "sync" - "time" - - "github.com/pkg/errors" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board/genericlinux/buses" - "go.viam.com/rdk/components/encoder" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" -) - -const ( - i2cConn = "i2c" - transitionEpsilon = 90 -) - -var ( - model = resource.DefaultModelFamily.WithModel("AMS-AS5048") - scalingFactor = 360.0 / math.Pow(2, 14) - supportedConnections = utils.NewStringSet(i2cConn) -) - -// the wait time necessary to operate the position updating -// loop at 50 Hz. -var waitTimeNano = (1.0 / 50.0) * 1000000000.0 - -func init() { - resource.RegisterComponent( - encoder.API, - model, - resource.Registration[encoder.Encoder, *Config]{ - Constructor: newAS5048Encoder, - }, - ) -} - -// Config contains the connection information for -// configuring an AS5048 encoder. -type Config struct { - // We include connection type here in anticipation for - // future SPI support - ConnectionType string `json:"connection_type"` - *I2CConfig `json:"i2c_attributes,omitempty"` -} - -// Validate checks the attributes of an initialized config -// for proper values. -func (conf *Config) Validate(path string) ([]string, error) { - var deps []string - - connType := conf.ConnectionType - if len(connType) == 0 { - // TODO: stop defaulting to I2C when SPI support is implemented - conf.ConnectionType = i2cConn - // return nil, errors.New("must specify connection type") - } - _, isSupported := supportedConnections[connType] - if !isSupported { - return nil, errors.Errorf("%s is not a supported connection type", connType) - } - if connType == i2cConn { - if conf.I2CConfig == nil { - return nil, errors.New("i2c selected as connection type, but no attributes supplied") - } - err := conf.I2CConfig.ValidateI2C(path) - if err != nil { - return nil, err - } - } - - return deps, nil -} - -// I2CConfig stores the configuration information for I2C connection. -type I2CConfig struct { - I2CBus string `json:"i2c_bus"` - I2CAddr int `json:"i2c_addr"` -} - -// ValidateI2C ensures all parts of the config are valid. -func (cfg *I2CConfig) ValidateI2C(path string) error { - if cfg.I2CBus == "" { - return resource.NewConfigValidationFieldRequiredError(path, "i2c_bus") - } - if cfg.I2CAddr == 0 { - return resource.NewConfigValidationFieldRequiredError(path, "i2c_addr") - } - - return nil -} - -// Encoder is a struct representing an instance of a hardware unit -// in AMS's AS5048 series of Hall-effect encoders. -type Encoder struct { - resource.Named - mu sync.RWMutex - logger logging.Logger - position float64 - positionOffset float64 - rotations int - positionType encoder.PositionType - i2cBus buses.I2C - i2cAddr byte - i2cBusName string // This is nessesary to check whether we need to create a new i2cBus during reconfigure. - workers *utils.StoppableWorkers -} - -func newAS5048Encoder( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger logging.Logger, -) (encoder.Encoder, error) { - return makeAS5048Encoder(ctx, deps, conf, logger, nil) -} - -// This function is separated to inject a mock i2c bus during tests. -func makeAS5048Encoder( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger logging.Logger, - bus buses.I2C, -) (encoder.Encoder, error) { - cfg, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - - res := &Encoder{ - Named: conf.ResourceName().AsNamed(), - logger: logger, - positionType: encoder.PositionTypeTicks, - i2cBus: bus, - i2cBusName: cfg.I2CBus, - } - - if err := res.Reconfigure(ctx, deps, conf); err != nil { - return nil, err - } - if err := res.ResetPosition(ctx, map[string]interface{}{}); err != nil { - return nil, err - } - res.workers = utils.NewBackgroundStoppableWorkers(res.positionLoop) - return res, nil -} - -// Reconfigure reconfigures the encoder atomically. -func (enc *Encoder) Reconfigure( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, -) error { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return err - } - enc.mu.Lock() - defer enc.mu.Unlock() - - if enc.i2cBusName != newConf.I2CBus || enc.i2cBus == nil { - bus, err := buses.NewI2cBus(newConf.I2CBus) - if err != nil { - msg := fmt.Sprintf("can't find I2C bus '%q' for AMS encoder", newConf.I2CBus) - return errors.Wrap(err, msg) - } - enc.i2cBusName = newConf.I2CBus - enc.i2cBus = bus - } - if enc.i2cAddr != byte(newConf.I2CAddr) { - enc.i2cAddr = byte(newConf.I2CAddr) - } - return nil -} - -func (enc *Encoder) positionLoop(cancelCtx context.Context) { - for { - if cancelCtx.Err() != nil { - return - } - if err := enc.updatePosition(cancelCtx); err != nil { - enc.logger.CErrorf(cancelCtx, - "error in position loop (skipping update): %s", err.Error(), - ) - } - time.Sleep(time.Duration(waitTimeNano)) - } -} - -func (enc *Encoder) readPosition(ctx context.Context) (float64, error) { - i2cHandle, err := enc.i2cBus.OpenHandle(enc.i2cAddr) - if err != nil { - return 0, err - } - defer utils.UncheckedErrorFunc(i2cHandle.Close) - - // retrieve the 8 most significant bits of the 14-bit resolution - // position - msB, err := i2cHandle.ReadByteData(ctx, byte(0xFE)) - if err != nil { - return 0, err - } - // retrieve the 6 least significant bits of as a byte (where - // the front two bits are irrelevant) - lsB, err := i2cHandle.ReadByteData(ctx, byte(0xFF)) - if err != nil { - return 0, err - } - return convertBytesToAngle(msB, lsB), nil -} - -func convertBytesToAngle(msB, lsB byte) float64 { - // obtain the 14-bit resolution position, which represents a - // portion of a full rotation. We then scale appropriately - // by (360 / 2^14) to get degrees - byteData := (int(msB) << 6) | int(lsB) - return (float64(byteData) * scalingFactor) -} - -func (enc *Encoder) updatePosition(ctx context.Context) error { - enc.mu.Lock() - defer enc.mu.Unlock() - angleDeg, err := enc.readPosition(ctx) - if err != nil { - return err - } - angleDeg += enc.positionOffset - // in order to keep track of multiple rotations, we increment / decrement - // a rotations counter whenever two subsequent positions are on either side - // of 0 (or 360) within a window of 2 * transitionEpsilon - forwardsTransition := (angleDeg <= transitionEpsilon) && ((360.0 - enc.position) <= transitionEpsilon) - backwardsTransition := (enc.position <= transitionEpsilon) && ((360.0 - angleDeg) <= transitionEpsilon) - if forwardsTransition { - enc.rotations++ - } else if backwardsTransition { - enc.rotations-- - } - enc.position = angleDeg - return nil -} - -// Position returns the total number of rotations detected -// by the encoder (rather than a number of pulse state transitions) -// because this encoder is absolute and not incremental. As a result -// a user MUST set ticks_per_rotation on the config of the corresponding -// motor to 1. Any other value will result in completely incorrect -// position measurements by the motor. -func (enc *Encoder) Position( - ctx context.Context, positionType encoder.PositionType, extra map[string]interface{}, -) (float64, encoder.PositionType, error) { - enc.mu.RLock() - defer enc.mu.RUnlock() - if positionType == encoder.PositionTypeDegrees { - enc.positionType = encoder.PositionTypeDegrees - return enc.position, enc.positionType, nil - } - ticks := float64(enc.rotations) + enc.position/360.0 - enc.positionType = encoder.PositionTypeTicks - return ticks, enc.positionType, nil -} - -// ResetPosition sets the current position measured by the encoder to be -// considered its new zero position. -func (enc *Encoder) ResetPosition( - ctx context.Context, extra map[string]interface{}, -) error { - enc.mu.Lock() - defer enc.mu.Unlock() - // NOTE (GV): potential improvement could be writing the offset position - // to the zero register of the encoder rather than keeping track - // on the struct - enc.position = 0 - enc.rotations = 0 - - i2cHandle, err := enc.i2cBus.OpenHandle(enc.i2cAddr) - if err != nil { - return err - } - defer utils.UncheckedErrorFunc(i2cHandle.Close) - - // clear current zero position - if err := i2cHandle.WriteByteData(ctx, byte(0x16), byte(0)); err != nil { - return err - } - if err := i2cHandle.WriteByteData(ctx, byte(0x17), byte(0)); err != nil { - return err - } - - // read current position - currentMSB, err := i2cHandle.ReadByteData(ctx, byte(0xFE)) - if err != nil { - return err - } - currentLSB, err := i2cHandle.ReadByteData(ctx, byte(0xFF)) - if err != nil { - return err - } - - // write current position to zero register - if err := i2cHandle.WriteByteData(ctx, byte(0x16), currentMSB); err != nil { - return err - } - if err := i2cHandle.WriteByteData(ctx, byte(0x17), currentLSB); err != nil { - return err - } - - return nil -} - -// Properties returns a list of all the position types that are supported by a given encoder. -func (enc *Encoder) Properties(ctx context.Context, extra map[string]interface{}) (encoder.Properties, error) { - return encoder.Properties{ - TicksCountSupported: true, - AngleDegreesSupported: true, - }, nil -} - -// Close stops the position loop of the encoder when the component -// is closed. -func (enc *Encoder) Close(ctx context.Context) error { - enc.workers.Stop() - return nil -} diff --git a/components/encoder/ams/ams_as5048_nonlinux.go b/components/encoder/ams/ams_as5048_nonlinux.go deleted file mode 100644 index 4412b51ffb0..00000000000 --- a/components/encoder/ams/ams_as5048_nonlinux.go +++ /dev/null @@ -1,2 +0,0 @@ -// Package ams is Linux-only -package ams diff --git a/components/encoder/ams/ams_as5048_test.go b/components/encoder/ams/ams_as5048_test.go deleted file mode 100644 index df11080ec44..00000000000 --- a/components/encoder/ams/ams_as5048_test.go +++ /dev/null @@ -1,175 +0,0 @@ -//go:build linux - -package ams - -import ( - "context" - "math" - "testing" - - "go.viam.com/test" - "go.viam.com/utils/testutils" - - "go.viam.com/rdk/components/board/genericlinux/buses" - "go.viam.com/rdk/components/encoder" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/testutils/inject" -) - -func TestConvertBytesToAngle(t *testing.T) { - // 180 degrees - msB := byte(math.Pow(2.0, 7.0)) - lsB := byte(0) - deg := convertBytesToAngle(msB, lsB) - test.That(t, deg, test.ShouldEqual, 180.0) - - // 270 degrees - msB = byte(math.Pow(2.0, 6.0) + math.Pow(2.0, 7.0)) - lsB = byte(0) - deg = convertBytesToAngle(msB, lsB) - test.That(t, deg, test.ShouldEqual, 270.0) - - // 219.990234 degrees - // 10011100011100 in binary, msB = 10011100, lsB = 00011100 - msB = byte(156) - lsB = byte(28) - deg = convertBytesToAngle(msB, lsB) - test.That(t, deg, test.ShouldAlmostEqual, 219.990234, 1e-6) -} - -func setupDependencies(mockData []byte) (resource.Config, resource.Dependencies, buses.I2C) { - i2cConf := &I2CConfig{ - I2CBus: "1", - I2CAddr: 64, - } - - cfg := resource.Config{ - Name: "encoder", - Model: model, - API: encoder.API, - ConvertedAttributes: &Config{ - ConnectionType: "i2c", - I2CConfig: i2cConf, - }, - } - - i2cHandle := &inject.I2CHandle{} - i2cHandle.ReadByteDataFunc = func(ctx context.Context, register byte) (byte, error) { - return mockData[register], nil - } - i2cHandle.WriteByteDataFunc = func(ctx context.Context, b1, b2 byte) error { - return nil - } - i2cHandle.CloseFunc = func() error { return nil } - - i2c := &inject.I2C{} - i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { - return i2cHandle, nil - } - - return cfg, resource.Dependencies{}, i2c -} - -func TestAMSEncoder(t *testing.T) { - ctx := context.Background() - - positionMockData := make([]byte, 256) - positionMockData[0xFE] = 100 - positionMockData[0xFF] = 60 - - logger := logging.NewTestLogger(t) - cfg, deps, bus := setupDependencies(positionMockData) - enc, err := makeAS5048Encoder(ctx, deps, cfg, logger, bus) - test.That(t, err, test.ShouldBeNil) - defer enc.Close(ctx) - - t.Run("test automatically set to type ticks", func(t *testing.T) { - testutils.WaitForAssertion(t, func(tb testing.TB) { - pos, _, _ := enc.Position(ctx, encoder.PositionTypeUnspecified, nil) - test.That(tb, pos, test.ShouldNotEqual, 0.0) - }) - pos, posType, _ := enc.Position(ctx, encoder.PositionTypeUnspecified, nil) - test.That(t, pos, test.ShouldAlmostEqual, 0.4, 0.1) - test.That(t, posType, test.ShouldEqual, 1) - }) - t.Run("test ticks type from input", func(t *testing.T) { - testutils.WaitForAssertion(t, func(tb testing.TB) { - pos, _, _ := enc.Position(ctx, encoder.PositionTypeTicks, nil) - test.That(tb, pos, test.ShouldNotEqual, 0.0) - }) - pos, posType, _ := enc.Position(ctx, encoder.PositionTypeUnspecified, nil) - test.That(t, pos, test.ShouldAlmostEqual, 0.4, 0.1) - test.That(t, posType, test.ShouldEqual, 1) - }) - t.Run("test degrees type from input", func(t *testing.T) { - testutils.WaitForAssertion(t, func(tb testing.TB) { - pos, _, _ := enc.Position(ctx, encoder.PositionTypeTicks, nil) - test.That(tb, pos, test.ShouldNotEqual, 0.0) - }) - pos, posType, _ := enc.Position(ctx, encoder.PositionTypeDegrees, nil) - test.That(t, pos, test.ShouldAlmostEqual, 142, 0.1) - test.That(t, posType, test.ShouldEqual, 2) - }) -} - -func setupDependenciesWithWrite(mockData []byte, writeData map[byte]byte) (resource.Config, resource.Dependencies, buses.I2C) { - i2cConf := &I2CConfig{ - I2CBus: "1", - I2CAddr: 64, - } - - cfg := resource.Config{ - Name: "encoder", - Model: model, - API: encoder.API, - ConvertedAttributes: &Config{ - ConnectionType: "i2c", - I2CConfig: i2cConf, - }, - } - - i2cHandle := &inject.I2CHandle{} - i2cHandle.ReadByteDataFunc = func(ctx context.Context, register byte) (byte, error) { - return mockData[register], nil - } - i2cHandle.WriteByteDataFunc = func(ctx context.Context, b1, b2 byte) error { - writeData[b1] = b2 - return nil - } - i2cHandle.CloseFunc = func() error { return nil } - - i2c := &inject.I2C{} - i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { - return i2cHandle, nil - } - return cfg, resource.Dependencies{}, i2c -} - -func TestAMSEncoderReset(t *testing.T) { - ctx := context.Background() - - positionMockData := make([]byte, 256) - positionMockData[0xFE] = 100 - positionMockData[0xFF] = 60 - - writeData := make(map[byte]byte) - - logger := logging.NewTestLogger(t) - cfg, deps, bus := setupDependenciesWithWrite(positionMockData, writeData) - enc, err := makeAS5048Encoder(ctx, deps, cfg, logger, bus) - test.That(t, err, test.ShouldBeNil) - defer enc.Close(ctx) - - t.Run("test reset", func(t *testing.T) { - testutils.WaitForAssertion(t, func(tb testing.TB) { - enc.ResetPosition(ctx, nil) - pos, posType, _ := enc.Position(ctx, encoder.PositionTypeUnspecified, nil) - test.That(tb, pos, test.ShouldAlmostEqual, 0, 0.1) - test.That(tb, posType, test.ShouldEqual, 1) - }) - - test.That(t, writeData[0x16], test.ShouldEqual, byte(100)) - test.That(t, writeData[0x17], test.ShouldEqual, byte(60)) - }) -} diff --git a/components/encoder/register/register.go b/components/encoder/register/register.go index d164db20859..dca26080743 100644 --- a/components/encoder/register/register.go +++ b/components/encoder/register/register.go @@ -3,7 +3,6 @@ package register import ( // Load all encoders. - _ "go.viam.com/rdk/components/encoder/ams" _ "go.viam.com/rdk/components/encoder/incremental" _ "go.viam.com/rdk/components/encoder/single" ) diff --git a/components/gripper/register/register.go b/components/gripper/register/register.go index b2d34482727..e182c53e5c0 100644 --- a/components/gripper/register/register.go +++ b/components/gripper/register/register.go @@ -4,5 +4,4 @@ package register import ( // for grippers. _ "go.viam.com/rdk/components/gripper/fake" - _ "go.viam.com/rdk/components/gripper/robotiq" ) diff --git a/components/gripper/robotiq/gripper.go b/components/gripper/robotiq/gripper.go deleted file mode 100644 index 9e4704fb329..00000000000 --- a/components/gripper/robotiq/gripper.go +++ /dev/null @@ -1,307 +0,0 @@ -// Package robotiq implements the gripper from robotiq. -// commands found at -// https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_CB-Series_PDF_20190329.pdf -package robotiq - -import ( - "context" - "fmt" - "net" - "strings" - "time" - - "github.com/pkg/errors" - "go.viam.com/utils" - - "go.viam.com/rdk/components/gripper" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" -) - -var model = resource.DefaultModelFamily.WithModel("robotiq") - -// Config is used for converting config attributes. -type Config struct { - Host string `json:"host"` -} - -// Validate ensures all parts of the config are valid. -func (cfg *Config) Validate(path string) ([]string, error) { - if cfg.Host == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "host") - } - return nil, nil -} - -func init() { - resource.RegisterComponent(gripper.API, model, resource.Registration[gripper.Gripper, *Config]{ - Constructor: func( - ctx context.Context, _ resource.Dependencies, conf resource.Config, logger logging.Logger, - ) (gripper.Gripper, error) { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - return newGripper(ctx, conf, newConf.Host, logger) - }, - }) -} - -// robotiqGripper TODO. -type robotiqGripper struct { - resource.Named - resource.AlwaysRebuild - - conn net.Conn - - openLimit string - closeLimit string - logger logging.Logger - opMgr *operation.SingleOperationManager - geometries []spatialmath.Geometry -} - -// newGripper instantiates a new Gripper of robotiqGripper type. -func newGripper(ctx context.Context, conf resource.Config, host string, logger logging.Logger) (gripper.Gripper, error) { - conn, err := net.Dial("tcp", host+":63352") - if err != nil { - return nil, err - } - g := &robotiqGripper{ - conf.ResourceName().AsNamed(), - resource.AlwaysRebuild{}, - conn, - "0", - "255", - logger, - operation.NewSingleOperationManager(), - []spatialmath.Geometry{}, - } - - init := [][]string{ - {"ACT", "1"}, // robot activate - {"GTO", "1"}, // gripper activate - {"FOR", "200"}, // force (0-255) - {"SPE", "255"}, // speed (0-255) - } - err = g.MultiSet(ctx, init) - if err != nil { - return nil, err - } - - err = g.Calibrate(ctx) // TODO(erh): should this live elsewhere? - if err != nil { - return nil, err - } - - if conf.Frame != nil && conf.Frame.Geometry != nil { - geometry, err := conf.Frame.Geometry.ParseConfig() - if err != nil { - return nil, err - } - g.geometries = []spatialmath.Geometry{geometry} - } - - return g, nil -} - -// MultiSet TODO. -func (g *robotiqGripper) MultiSet(ctx context.Context, cmds [][]string) error { - for _, i := range cmds { - err := g.Set(i[0], i[1]) - if err != nil { - return err - } - - // TODO(erh): the next 5 lines are infuriatng, help! - var waitTime time.Duration - if i[0] == "ACT" { - waitTime = 1600 * time.Millisecond - } else { - waitTime = 500 * time.Millisecond - } - if !utils.SelectContextOrWait(ctx, waitTime) { - return ctx.Err() - } - } - - return nil -} - -// Send TODO. -func (g *robotiqGripper) Send(msg string) (string, error) { - _, err := g.conn.Write([]byte(msg)) - if err != nil { - return "", err - } - - res, err := g.read() - if err != nil { - return "", err - } - - return res, err -} - -// Set TODO. -func (g *robotiqGripper) Set(what, to string) error { - res, err := g.Send(fmt.Sprintf("SET %s %s\r\n", what, to)) - if err != nil { - return err - } - if res != "ack" { - return errors.Errorf("didn't get ack back, got [%s]", res) - } - return nil -} - -// Get TODO. -func (g *robotiqGripper) Get(what string) (string, error) { - return g.Send(fmt.Sprintf("GET %s\r\n", what)) -} - -func (g *robotiqGripper) read() (string, error) { - buf := make([]byte, 128) - x, err := g.conn.Read(buf) - if err != nil { - return "", err - } - if x > 100 { - return "", errors.Errorf("read too much: %d", x) - } - if x == 0 { - return "", nil - } - return strings.TrimSpace(string(buf[0:x])), nil -} - -// SetPos returns true iff reached desired position. -func (g *robotiqGripper) SetPos(ctx context.Context, pos string) (bool, error) { - err := g.Set("POS", pos) - if err != nil { - return false, err - } - - prev := "" - prevCount := 0 - - for { - x, err := g.Get("POS") - if err != nil { - return false, err - } - if x == "POS "+pos { - return true, nil - } - - if prev == x { - if prevCount >= 5 { - return false, nil - } - prevCount++ - } else { - prevCount = 0 - } - prev = x - - if !utils.SelectContextOrWait(ctx, 100*time.Millisecond) { - return false, ctx.Err() - } - } -} - -// Open TODO. -func (g *robotiqGripper) Open(ctx context.Context, extra map[string]interface{}) error { - ctx, done := g.opMgr.New(ctx) - defer done() - - _, err := g.SetPos(ctx, g.openLimit) - return err -} - -// Close TODO. -func (g *robotiqGripper) Close(ctx context.Context) error { - ctx, done := g.opMgr.New(ctx) - defer done() - - _, err := g.SetPos(ctx, g.closeLimit) - return err -} - -// Grab returns true iff grabbed something. -func (g *robotiqGripper) Grab(ctx context.Context, extra map[string]interface{}) (bool, error) { - ctx, done := g.opMgr.New(ctx) - defer done() - - res, err := g.SetPos(ctx, g.closeLimit) - if err != nil { - return false, err - } - if res { - // we closed, so didn't grab anything - return false, nil - } - - // we didn't close, let's see if we actually got something - val, err := g.Get("OBJ") - if err != nil { - return false, err - } - return val == "OBJ 2", nil -} - -// Calibrate TODO. -func (g *robotiqGripper) Calibrate(ctx context.Context) error { - err := g.Open(ctx, map[string]interface{}{}) - if err != nil { - return err - } - - x, err := g.Get("POS") - if err != nil { - return err - } - g.openLimit = x[4:] - - err = g.Close(ctx) - if err != nil { - return err - } - - x, err = g.Get("POS") - if err != nil { - return err - } - g.closeLimit = x[4:] - - g.logger.CDebugf(ctx, "limits %s %s", g.openLimit, g.closeLimit) - return nil -} - -// Stop is unimplemented for robotiqGripper. -func (g *robotiqGripper) Stop(ctx context.Context, extra map[string]interface{}) error { - // RSDK-388: Implement Stop - err := g.Set("GTO", "0") - if err != nil { - return err - } - return nil -} - -// IsMoving returns whether the gripper is moving. -func (g *robotiqGripper) IsMoving(ctx context.Context) (bool, error) { - return g.opMgr.OpRunning(), nil -} - -// ModelFrame is unimplemented for robotiqGripper. -func (g *robotiqGripper) ModelFrame() referenceframe.Model { - return nil -} - -// Geometries returns the geometries associated with robotiqGripper. -func (g *robotiqGripper) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { - return g.geometries, nil -} diff --git a/components/motor/dimensionengineering/common.go b/components/motor/dimensionengineering/common.go deleted file mode 100644 index c7221b95003..00000000000 --- a/components/motor/dimensionengineering/common.go +++ /dev/null @@ -1,44 +0,0 @@ -package dimensionengineering - -type ( - commandCode byte - opCode byte -) - -const ( - minSpeed = 0x0 - maxSpeed = 0x7f - - // Commands. - singleForward commandCode = 0 - singleBackwards commandCode = 1 - singleDrive commandCode = 2 - multiForward commandCode = 3 - multiBackward commandCode = 4 - multiDrive commandCode = 5 - multiTurnLeft commandCode = 6 - multiTurnRight commandCode = 7 - multiTurn commandCode = 8 - setRamping commandCode = 20 - setDeadband commandCode = 21 - - // Serial level op-codes. - opMotor1Forward opCode = 0x00 - opMotor1Backwards opCode = 0x01 - opMinVoltage opCode = 0x02 - opMaxVoltage opCode = 0x03 - opMotor2Forward opCode = 0x04 - opMotor2Backwards opCode = 0x05 - opMotor1Drive opCode = 0x06 - opMotor2Drive opCode = 0x07 - opMultiDriveForward opCode = 0x08 - opMultiDriveBackwards opCode = 0x09 - opMultiDriveRight opCode = 0x0a - opMultiDriveLeft opCode = 0x0b - opMultiDrive opCode = 0x0c - opMultiTurn opCode = 0x0d - opSerialTimeout opCode = 0x0e - opSerialBaudRate opCode = 0x0f - opRamping opCode = 0x10 - opDeadband opCode = 0x11 -) diff --git a/components/motor/dimensionengineering/sabertooth.go b/components/motor/dimensionengineering/sabertooth.go deleted file mode 100644 index 189da9fccb2..00000000000 --- a/components/motor/dimensionengineering/sabertooth.go +++ /dev/null @@ -1,561 +0,0 @@ -// Package dimensionengineering contains implementations of the dimensionengineering motor controls -package dimensionengineering - -import ( - "context" - "fmt" - "io" - "math" - "strings" - "sync" - "time" - - "github.com/jacobsa/go-serial/serial" - "github.com/pkg/errors" - - "go.viam.com/rdk/components/motor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/resource" - rutils "go.viam.com/rdk/utils" -) - -// https://www.dimensionengineering.com/datasheets/Sabertooth2x60.pdf -var model = resource.DefaultModelFamily.WithModel("de-sabertooth") - -// controllers is global to all instances, mapped by serial device. -var ( - globalMu sync.Mutex - controllers map[string]*controller - validBaudRates = []uint{115200, 38400, 19200, 9600, 2400} -) - -// controller is common across all Sabertooth motor instances sharing a controller. -type controller struct { - mu sync.Mutex - port io.ReadWriteCloser - serialDevice string - logger logging.Logger - activeAxes map[int]bool - testChan chan []byte - address int // 128-135 -} - -// Motor is a single axis/motor/component instance. -type Motor struct { - resource.Named - resource.AlwaysRebuild - - logger logging.Logger - // A reference to the actual controller that needs to be commanded for the motor to run - c *controller - // which channel the motor is connected to on the controller - Channel int - // Simply indicates if the RDK _thinks_ the motor is moving, because this controller has no feedback, this may not reflect reality - isOn bool - // The current power setting the RDK _thinks_ the motor is running, because this controller has no feedback, this may not reflect reality - currentPowerPct float64 - // dirFlip means that the motor is wired "backwards" from what we expect forward/backward to mean, - // so we need to "flip" the direction sent by control - dirFlip bool - // the minimum power that can be set for the motor to prevent stalls - minPowerPct float64 - // the maximum power that can be set for the motor - maxPowerPct float64 - // the freewheel RPM of the motor - maxRPM float64 - - // 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 -} - -// Config adds DimensionEngineering-specific config options. -type Config struct { - // path to /dev/ttyXXXX file - SerialPath string `json:"serial_path"` - - // The baud rate of the controller - BaudRate int `json:"serial_baud_rate,omitempty"` - - // Valid values are 128-135 - SerialAddress int `json:"serial_address"` - - // Valid values are 1/2 - MotorChannel int `json:"motor_channel"` - - // Flip the direction of the signal sent to the controller. - // Due to wiring/motor orientation, "forward" on the controller may not represent "forward" on the robot - DirectionFlip bool `json:"dir_flip,omitempty"` - - // A value to control how quickly the controller ramps to a particular setpoint - RampValue int `json:"controller_ramp_value,omitempty"` - - // The maximum freewheel rotational velocity of the motor after the final drive (maximum effective wheel speed) - MaxRPM float64 `json:"max_rpm,omitempty"` - - // The name of the encoder used for this motor - Encoder string `json:"encoder,omitempty"` - - // The lowest power percentage to allow for this motor. This is used to prevent motor stalls and overheating. Default is 0.0 - MinPowerPct float64 `json:"min_power_pct,omitempty"` - - // The max power percentage to allow for this motor. Default is 0.0 - MaxPowerPct float64 `json:"max_power_pct,omitempty"` - - // The number of ticks per rotation of this motor from the encoder - TicksPerRotation int `json:"ticks_per_rotation,omitempty"` - - // TestChan is a fake "serial" path for test use only - TestChan chan []byte `json:"-,omitempty"` -} - -// Validate ensures all parts of the config are valid. -func (cfg *Config) Validate(path string) ([]string, error) { - if cfg.SerialPath == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "serial_path") - } - - return nil, nil -} - -func init() { - controllers = make(map[string]*controller) - - resource.RegisterComponent(motor.API, model, resource.Registration[motor.Motor, *Config]{ - Constructor: func( - ctx context.Context, _ resource.Dependencies, conf resource.Config, logger logging.Logger, - ) (motor.Motor, error) { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - return NewMotor(ctx, newConf, conf.ResourceName(), logger) - }, - }) -} - -func newController(c *Config, logger logging.Logger) (*controller, error) { - ctrl := new(controller) - ctrl.activeAxes = make(map[int]bool) - ctrl.serialDevice = c.SerialPath - ctrl.logger = logger - ctrl.address = c.SerialAddress - - if c.TestChan != nil { - ctrl.testChan = c.TestChan - } else { - serialOptions := serial.OpenOptions{ - PortName: c.SerialPath, - BaudRate: uint(c.BaudRate), - DataBits: 8, - StopBits: 1, - MinimumReadSize: 1, - RTSCTSFlowControl: true, - } - - port, err := serial.Open(serialOptions) - if err != nil { - return nil, err - } - ctrl.port = port - } - - ctrl.activeAxes[1] = false - ctrl.activeAxes[2] = false - - return ctrl, nil -} - -func (cfg *Config) populateDefaults() { - if cfg.BaudRate == 0 { - cfg.BaudRate = 9600 - } - - if cfg.MaxPowerPct == 0.0 { - cfg.MaxPowerPct = 1.0 - } -} - -func (cfg *Config) validateValues() error { - errs := make([]string, 0) - if cfg.MotorChannel != 1 && cfg.MotorChannel != 2 { - errs = append(errs, fmt.Sprintf("invalid channel %v, acceptable values are 1 and 2", cfg.MotorChannel)) - } - if cfg.SerialAddress < 128 || cfg.SerialAddress > 135 { - errs = append(errs, "invalid address, acceptable values are 128 thru 135") - } - if !rutils.ValidateBaudRate(validBaudRates, cfg.BaudRate) { - errs = append(errs, fmt.Sprintf("invalid baud_rate, acceptable values are %v", validBaudRates)) - } - if cfg.BaudRate != 2400 && cfg.BaudRate != 9600 && cfg.BaudRate != 19200 && cfg.BaudRate != 38400 && cfg.BaudRate != 115200 { - errs = append(errs, "invalid baud_rate, acceptable values are 2400, 9600, 19200, 38400, 115200") - } - if cfg.MinPowerPct < 0.0 || cfg.MinPowerPct > cfg.MaxPowerPct { - errs = append(errs, "invalid min_power_pct, acceptable values are 0 to max_power_pct") - } - if cfg.MaxPowerPct > 1.0 { - errs = append(errs, "invalid max_power_pct, acceptable values are min_power_pct to 100.0") - } - if len(errs) > 0 { - return fmt.Errorf("error validating sabertooth controller config: %s", strings.Join(errs, "\r\n")) - } - return nil -} - -// NewMotor returns a Sabertooth driven motor. -func NewMotor(ctx context.Context, c *Config, name resource.Name, logger logging.Logger) (motor.Motor, error) { - globalMu.Lock() - defer globalMu.Unlock() - - // populate the default values into the config - c.populateDefaults() - - // Validate the actual config values make sense - err := c.validateValues() - if err != nil { - return nil, err - } - ctrl, ok := controllers[c.SerialPath] - if !ok { - newCtrl, err := newController(c, logger) - if err != nil { - return nil, err - } - controllers[c.SerialPath] = newCtrl - ctrl = newCtrl - } - - ctrl.mu.Lock() - defer ctrl.mu.Unlock() - - // is on a known/supported amplifier only when map entry exists - claimed, ok := ctrl.activeAxes[c.MotorChannel] - if !ok { - return nil, fmt.Errorf("invalid Sabertooth motor axis: %d", c.MotorChannel) - } - if claimed { - return nil, fmt.Errorf("axis %d is already in use", c.MotorChannel) - } - ctrl.activeAxes[c.MotorChannel] = true - - m := &Motor{ - Named: name.AsNamed(), - c: ctrl, - Channel: c.MotorChannel, - dirFlip: c.DirectionFlip, - minPowerPct: c.MinPowerPct, - maxPowerPct: c.MaxPowerPct, - maxRPM: c.MaxRPM, - opMgr: operation.NewSingleOperationManager(), - logger: logger, - } - - if err := m.configure(c); err != nil { - return nil, err - } - - if c.RampValue > 0 { - setRampCmd, err := newCommand(c.SerialAddress, setRamping, c.MotorChannel, byte(c.RampValue)) - if err != nil { - return nil, err - } - - err = m.c.sendCmd(setRampCmd) - if err != nil { - return nil, err - } - } - - return m, nil -} - -// IsPowered returns if the motor is currently on or off. -func (m *Motor) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { - return m.isOn, m.currentPowerPct, nil -} - -// Close stops the motor and marks the axis inactive. -func (m *Motor) Close(ctx context.Context) error { - active := m.isAxisActive() - if !active { - return nil - } - - err := m.Stop(context.Background(), nil) - if err != nil { - m.c.logger.CError(ctx, err) - } - - m.c.mu.Lock() - defer m.c.mu.Unlock() - m.c.activeAxes[m.Channel] = false - for _, active = range m.c.activeAxes { - if active { - return nil - } - } - if m.c.port != nil { - err = m.c.port.Close() - if err != nil { - m.c.logger.CError(ctx, fmt.Errorf("error closing serial connection: %w", err)) - } - } - globalMu.Lock() - defer globalMu.Unlock() - delete(controllers, m.c.serialDevice) - return nil -} - -func (m *Motor) isAxisActive() bool { - m.c.mu.Lock() - defer m.c.mu.Unlock() - return m.c.activeAxes[m.Channel] -} - -// Must be run inside a lock. -func (m *Motor) configure(c *Config) error { - // Turn off the motor with opMixedDrive and a value of 64 (stop) - cmd, err := newCommand(m.c.address, singleForward, c.MotorChannel, 0x00) - if err != nil { - return err - } - err = m.c.sendCmd(cmd) - return err -} - -// Must be run inside a lock. -func (c *controller) sendCmd(cmd *command) error { - packet := cmd.ToPacket() - if c.testChan != nil { - c.testChan <- packet - return nil - } - _, err := c.port.Write(packet) - return err -} - -// SetPower instructs the motor to go in a specific direction at a percentage -// of power between -1 and 1. -func (m *Motor) SetPower(ctx context.Context, powerPct float64, extra map[string]interface{}) error { - if math.Abs(powerPct) < m.minPowerPct { - return m.Stop(ctx, extra) - } - if powerPct > 1 { - powerPct = 1 - } else if powerPct < -1 { - powerPct = -1 - } - - m.opMgr.CancelRunning(ctx) - m.c.mu.Lock() - defer m.c.mu.Unlock() - m.isOn = true - m.currentPowerPct = powerPct - - rawSpeed := powerPct * maxSpeed - warning, err := motor.CheckSpeed(rawSpeed, m.maxRPM) - if warning != "" { - m.logger.CWarn(ctx, warning) - } - if err != nil { - m.logger.CError(ctx, err) - } - if math.Signbit(rawSpeed) { - rawSpeed *= -1 - } - - // Jog - var cmd commandCode - if powerPct < 0 { - // If dirFlip is set, we actually want to reverse the command - if m.dirFlip { - cmd = singleForward - } else { - cmd = singleBackwards - } - } else { - // If dirFlip is set, we actually want to reverse the command - if m.dirFlip { - cmd = singleBackwards - } else { - cmd = singleForward - } - } - c, err := newCommand(m.c.address, cmd, m.Channel, byte(int(rawSpeed))) - if err != nil { - return errors.Wrap(err, "error in SetPower") - } - err = m.c.sendCmd(c) - return err -} - -// GoFor moves an inputted number of revolutions at the given rpm, no encoder is present -// for this so power is determined via a linear relationship with the maxRPM and the distance -// traveled is a time based estimation based on desired RPM. -func (m *Motor) GoFor(ctx context.Context, rpm, revolutions float64, extra map[string]interface{}) error { - if m.maxRPM == 0 || rpm == 0 { - return motor.NewZeroRPMError() - } - - if err := motor.CheckRevolutions(revolutions); err != nil { - return err - } - - powerPct, waitDur := goForMath(m.maxRPM, rpm, revolutions) - err := m.SetPower(ctx, powerPct, extra) - if err != nil { - return errors.Wrap(err, "error in GoFor") - } - - if m.opMgr.NewTimedWaitOp(ctx, waitDur) { - return m.Stop(ctx, extra) - } - return nil -} - -// GoTo instructs the motor to go to a specific position (provided in revolutions from home/zero), -// 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 motor.NewGoToUnsupportedError(fmt.Sprintf("Channel %d on Sabertooth %d", m.Channel, m.c.address)) -} - -// SetRPM instructs the motor to move at the specified RPM indefinitely. -func (m *Motor) SetRPM(ctx context.Context, rpm float64, extra map[string]interface{}) error { - if m.maxRPM == 0 { - return motor.NewZeroRPMError() - } - - powerPct := rpm / m.maxRPM - return m.SetPower(ctx, powerPct, extra) -} - -// ResetZeroPosition defines the current position to be zero (+/- offset). -func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { - return motor.NewResetZeroPositionUnsupportedError(fmt.Sprintf("Channel %d on Sabertooth %d", - m.Channel, m.c.address)) -} - -// Position reports the position in revolutions. -func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, nil -} - -// Stop turns the power to the motor off immediately, without any gradual step down. -func (m *Motor) Stop(ctx context.Context, extra map[string]interface{}) error { - m.c.mu.Lock() - defer m.c.mu.Unlock() - - _, done := m.opMgr.New(ctx) - defer done() - - m.isOn = false - m.currentPowerPct = 0.0 - cmd, err := newCommand(m.c.address, singleForward, m.Channel, 0) - if err != nil { - return err - } - - err = m.c.sendCmd(cmd) - return err -} - -// IsMoving returns whether the motor is currently moving. -func (m *Motor) IsMoving(ctx context.Context) (bool, error) { - return m.isOn, nil -} - -// DoCommand executes additional commands beyond the Motor{} interface. -func (m *Motor) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { - name, ok := cmd["command"] - if !ok { - return nil, errors.New("missing 'command' value") - } - return nil, fmt.Errorf("no such command: %s", name) -} - -// Properties returns the additional properties supported by this motor. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { - return motor.Properties{PositionReporting: false}, nil -} - -type command struct { - Address byte - Op byte - Data byte - Checksum byte -} - -func newCommand(controllerAddress int, motorMode commandCode, channel int, data byte) (*command, error) { - var opcode opCode - switch motorMode { - case singleForward: - switch channel { - case 1: - opcode = opMotor1Forward - case 2: - opcode = opMotor2Forward - default: - return nil, errors.New("invalid motor channel") - } - case singleBackwards: - switch channel { - case 1: - opcode = opMotor1Backwards - case 2: - opcode = opMotor2Backwards - default: - return nil, errors.New("invalid motor channel") - } - case singleDrive: - switch channel { - case 1: - opcode = opMotor1Drive - case 2: - opcode = opMotor2Drive - default: - return nil, errors.New("invalid motor channel") - } - case multiForward: - opcode = opMultiDriveForward - case multiBackward: - opcode = opMultiDriveForward - case multiDrive: - opcode = opMultiDrive - case setRamping: - opcode = opRamping - case setDeadband: - case multiTurnRight: - case multiTurnLeft: - case multiTurn: - default: - return nil, fmt.Errorf("opcode %x not implemented", opcode) - } - sum := byte(controllerAddress) + byte(opcode) + data - checksum := sum & 0x7F - return &command{ - Address: byte(controllerAddress), - Op: byte(opcode), - Data: data, - Checksum: checksum, - }, nil -} - -func (c *command) ToPacket() []byte { - return []byte{c.Address, c.Op, c.Data, c.Checksum} -} - -func goForMath(maxRPM, rpm, revolutions float64) (float64, time.Duration) { - // need to do this so time is reasonable - if rpm > maxRPM { - rpm = maxRPM - } else if rpm < -1*maxRPM { - rpm = -1 * maxRPM - } - - dir := motor.GetRequestedDirection(rpm, revolutions) - - powerPct := math.Abs(rpm) / maxRPM * dir - waitDur := time.Duration(math.Abs(revolutions/rpm)*60*1000) * time.Millisecond - return powerPct, waitDur -} diff --git a/components/motor/dimensionengineering/sabertooth_test.go b/components/motor/dimensionengineering/sabertooth_test.go deleted file mode 100644 index bf632c4512b..00000000000 --- a/components/motor/dimensionengineering/sabertooth_test.go +++ /dev/null @@ -1,510 +0,0 @@ -package dimensionengineering_test - -import ( - "bytes" - "context" - "fmt" - "testing" - - "go.viam.com/test" - - "go.viam.com/rdk/components/motor" - "go.viam.com/rdk/components/motor/dimensionengineering" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" -) - -var sabertoothModel = resource.DefaultModelFamily.WithModel("de-sabertooth") - -func checkTx(t *testing.T, resChan chan string, c chan []byte, expects []byte) { - t.Helper() - message := <-c - t.Logf("Expected: %b, Actual %b", expects, message) - test.That(t, bytes.Compare(message, expects), test.ShouldBeZeroValue) - resChan <- "DONE" -} - -//nolint:dupl -func TestSabertoothMotor(t *testing.T) { - ctx := context.Background() - logger, obs := logging.NewObservedTestLogger(t) - c := make(chan []byte, 1024) - resChan := make(chan string, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 128, - DirectionFlip: false, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - m1, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err, test.ShouldBeNil) - defer m1.Close(ctx) - - // This should be the stop command - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - - motor1, ok := m1.(motor.Motor) - test.That(t, ok, test.ShouldBeTrue) - - t.Run("motor supports position reporting", func(t *testing.T) { - properties, err := motor1.Properties(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, properties.PositionReporting, test.ShouldBeFalse) - }) - - t.Run("motor SetPower testing", func(t *testing.T) { - // Test 0 (aka "stop") - test.That(t, motor1.SetPower(ctx, 0.0001, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - // Test 0.5 of max power - test.That(t, motor1.SetPower(ctx, 0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x3f, 0x3f}) - - // Test -0.5 of max power - test.That(t, motor1.SetPower(ctx, -0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x01, 0x3f, 0x40}) - - // Test max power - test.That(t, motor1.SetPower(ctx, 1, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x7f, 0x7f}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - - // Test 0 (aka "stop") - test.That(t, motor1.SetPower(ctx, 0, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - }) - - t.Run("motor SetRPM testing", func(t *testing.T) { - // Test 0 (aka "stop") - test.That(t, motor1.SetRPM(ctx, 0.0001, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - // Test 0.5 of max power - test.That(t, motor1.SetRPM(ctx, 0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x3f, 0x3f}) - - // Test -0.5 of max power - test.That(t, motor1.SetRPM(ctx, -0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x01, 0x3f, 0x40}) - - // Test max power - test.That(t, motor1.SetRPM(ctx, 1, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x7f, 0x7f}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - - // Test 0 (aka "stop") - test.That(t, motor1.SetRPM(ctx, 0, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - }) - - t.Run("motor GoFor testing", func(t *testing.T) { - // zero rpm error - test.That(t, motor1.GoFor(ctx, 0, 10, nil), test.ShouldBeError, motor.NewZeroRPMError()) - - // zero revolutions error - test.That(t, motor1.GoFor(ctx, 10, 0, nil), test.ShouldBeError, motor.NewZeroRevsError()) - }) - - mc2 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 2, - TestChan: c, - SerialAddress: 128, - DirectionFlip: false, - MaxRPM: 1, - } - - m2, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor2", ConvertedAttributes: &mc2}, logger) - test.That(t, err, test.ShouldBeNil) - defer m2.Close(ctx) - - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) - - motor2, ok := m2.(motor.Motor) - test.That(t, ok, test.ShouldBeTrue) - - t.Run("motor supports position reporting", func(t *testing.T) { - properties, err := motor2.Properties(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, properties.PositionReporting, test.ShouldBeFalse) - }) - - t.Run("motor SetPower testing", func(t *testing.T) { - // Test 0 (aka "stop") - test.That(t, motor2.SetPower(ctx, 0.0001, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - // Test 0.5 of max power - test.That(t, motor2.SetPower(ctx, 0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x3f, 0x43}) - - // Test -0.5 of max power - test.That(t, motor2.SetPower(ctx, -0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x05, 0x3f, 0x44}) - - // Test max power - test.That(t, motor2.SetPower(ctx, 1, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x7f, 0x03}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - - // Test 0 (aka "stop") - test.That(t, motor2.SetPower(ctx, 0, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - }) -} - -//nolint:dupl -func TestSabertoothMotorDirectionFlip(t *testing.T) { - ctx := context.Background() - logger, obs := logging.NewObservedTestLogger(t) - c := make(chan []byte, 1024) - resChan := make(chan string, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 128, - DirectionFlip: true, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - m1, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err, test.ShouldBeNil) - defer m1.Close(ctx) - - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - - motor1, ok := m1.(motor.Motor) - test.That(t, ok, test.ShouldBeTrue) - - t.Run("motor SetPower testing", func(t *testing.T) { - // Test 0 (aka "stop") - test.That(t, motor1.SetPower(ctx, 0.0001, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x01, 0x00, 0x01}) - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - // Test 0.5 of max power - test.That(t, motor1.SetPower(ctx, 0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x01, 0x3f, 0x40}) - - // Test -0.5 of max power - test.That(t, motor1.SetPower(ctx, -0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x3f, 0x3f}) - - // Test max power - test.That(t, motor1.SetPower(ctx, 1, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x01, 0x7f, 0x00}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - - // Test 0 (aka "stop") - test.That(t, motor1.SetPower(ctx, 0, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x01, 0x00, 0x01}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - }) - - mc2 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 2, - TestChan: c, - SerialAddress: 128, - DirectionFlip: true, - MaxRPM: 1, - } - - m2, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor2", ConvertedAttributes: &mc2}, logger) - test.That(t, err, test.ShouldBeNil) - defer m2.Close(ctx) - - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) - - motor2, ok := m2.(motor.Motor) - test.That(t, ok, test.ShouldBeTrue) - - t.Run("motor supports position reporting", func(t *testing.T) { - properties, err := motor2.Properties(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, properties.PositionReporting, test.ShouldBeFalse) - }) - - t.Run("motor SetPower testing", func(t *testing.T) { - // Test 0 (aka "stop") - test.That(t, motor2.SetPower(ctx, 0.0001, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x05, 0x00, 0x05}) - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - // Test 0.5 of max power - test.That(t, motor2.SetPower(ctx, 0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x05, 0x3f, 0x44}) - - // Test -0.5 of max power - test.That(t, motor2.SetPower(ctx, -0.5, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x04, 0x3f, 0x43}) - - // Test max power - test.That(t, motor2.SetPower(ctx, 1, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x05, 0x7f, 0x04}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - - // Test 0 (aka "stop") - test.That(t, motor2.SetPower(ctx, 0, nil), test.ShouldBeNil) - checkTx(t, resChan, c, []byte{0x80, 0x05, 0x00, 0x05}) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - }) -} - -func TestSabertoothRampConfig(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - resChan := make(chan string, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 128, - RampValue: 100, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - m1, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err, test.ShouldBeNil) - defer m1.Close(ctx) - - checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) - checkTx(t, resChan, c, []byte{0x80, 0x10, 0x64, 0x74}) - - _, ok = m1.(motor.Motor) - test.That(t, ok, test.ShouldBeTrue) -} - -func TestSabertoothAddressMapping(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - resChan := make(chan string, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 129, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - m1, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err, test.ShouldBeNil) - defer m1.Close(ctx) - - checkTx(t, resChan, c, []byte{0x81, 0x00, 0x00, 0x01}) -} - -func TestInvalidMotorChannel(t *testing.T) { - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 3, - TestChan: c, - SerialAddress: 129, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - _, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid channel") -} - -func TestInvalidBaudRate(t *testing.T) { - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 129, - BaudRate: 1, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - _, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid baud_rate") -} - -func TestInvalidSerialAddress(t *testing.T) { - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 140, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - _, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid address") -} - -func TestInvalidMinPowerPct(t *testing.T) { - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 129, - MinPowerPct: 0.7, - MaxPowerPct: 0.5, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - _, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid min_power_pct") -} - -func TestInvalidMaxPowerPct(t *testing.T) { - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 1, - TestChan: c, - SerialAddress: 129, - MinPowerPct: 0.7, - MaxPowerPct: 1.5, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - _, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid max_power_pct") -} - -func TestMultipleInvalidParameters(t *testing.T) { - logger := logging.NewTestLogger(t) - c := make(chan []byte, 1024) - deps := make(resource.Dependencies) - - mc1 := dimensionengineering.Config{ - SerialPath: "testchan", - MotorChannel: 3, - TestChan: c, - BaudRate: 10, - SerialAddress: 140, - MinPowerPct: 1.7, - MaxPowerPct: 1.5, - MaxRPM: 1, - } - - motorReg, ok := resource.LookupRegistration(motor.API, sabertoothModel) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, motorReg, test.ShouldNotBeNil) - - // These are the setup register writes - _, err := motorReg.Constructor(context.Background(), deps, resource.Config{Name: "motor1", ConvertedAttributes: &mc1}, logger) - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid channel") - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid address") - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid baud_rate") - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid min_power_pct") - test.That(t, err.Error(), test.ShouldContainSubstring, "invalid max_power_pct") -} diff --git a/components/motor/register/register.go b/components/motor/register/register.go index 62a7592c284..dbecc41b3a9 100644 --- a/components/motor/register/register.go +++ b/components/motor/register/register.go @@ -3,10 +3,7 @@ package register import ( // for motors. - _ "go.viam.com/rdk/components/motor/dimensionengineering" _ "go.viam.com/rdk/components/motor/fake" _ "go.viam.com/rdk/components/motor/gpio" _ "go.viam.com/rdk/components/motor/gpiostepper" - _ "go.viam.com/rdk/components/motor/tmcstepper" - _ "go.viam.com/rdk/components/motor/ulnstepper" ) diff --git a/components/motor/tmcstepper/stepper_motor_tmc.go b/components/motor/tmcstepper/stepper_motor_tmc.go deleted file mode 100644 index 61cf5c36183..00000000000 --- a/components/motor/tmcstepper/stepper_motor_tmc.go +++ /dev/null @@ -1,740 +0,0 @@ -//go:build linux - -// Package tmcstepper implements a TMC stepper motor. -package tmcstepper - -import ( - "context" - "math" - "sync" - "time" - - "github.com/pkg/errors" - "go.uber.org/multierr" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/board/genericlinux/buses" - "go.viam.com/rdk/components/motor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/resource" -) - -// PinConfig defines the mapping of where motor are wired. -type PinConfig struct { - EnablePinLow string `json:"en_low,omitempty"` -} - -// TMC5072Config describes the configuration of a motor. -type TMC5072Config struct { - Pins PinConfig `json:"pins,omitempty"` - BoardName string `json:"board,omitempty"` // used solely for the PinConfig - MaxRPM float64 `json:"max_rpm,omitempty"` - MaxAcceleration float64 `json:"max_acceleration_rpm_per_sec,omitempty"` - TicksPerRotation int `json:"ticks_per_rotation"` - SPIBus string `json:"spi_bus"` - ChipSelect string `json:"chip_select"` - Index int `json:"index"` - SGThresh int32 `json:"sg_thresh,omitempty"` - HomeRPM float64 `json:"home_rpm,omitempty"` - CalFactor float64 `json:"cal_factor,omitempty"` - RunCurrent int32 `json:"run_current,omitempty"` // 1-32 as a percentage of rsense voltage, 15 default - HoldCurrent int32 `json:"hold_current,omitempty"` // 1-32 as a percentage of rsense voltage, 8 default - HoldDelay int32 `json:"hold_delay,omitempty"` // 0=instant powerdown, 1-15=delay * 2^18 clocks, 6 default -} - -var model = resource.DefaultModelFamily.WithModel("TMC5072") - -// Validate ensures all parts of the config are valid. -func (config *TMC5072Config) Validate(path string) ([]string, error) { - var deps []string - if config.Pins.EnablePinLow != "" { - if config.BoardName == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "board") - } - deps = append(deps, config.BoardName) - } - if config.SPIBus == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "spi_bus") - } - if config.ChipSelect == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "chip_select") - } - if config.Index <= 0 { - return nil, resource.NewConfigValidationFieldRequiredError(path, "index") - } - if config.Index > 2 { - return nil, errors.New("tmcstepper motor index should be 1 or 2") - } - if config.TicksPerRotation <= 0 { - return nil, resource.NewConfigValidationFieldRequiredError(path, "ticks_per_rotation") - } - return deps, nil -} - -func init() { - resource.RegisterComponent(motor.API, model, resource.Registration[motor.Motor, *TMC5072Config]{ - Constructor: newMotor, - }) -} - -// A Motor represents a brushless motor connected via a TMC controller chip (ex: TMC5072). -type Motor struct { - resource.Named - resource.AlwaysRebuild - resource.TriviallyCloseable - bus buses.SPI - csPin string - index int - enLowPin board.GPIOPin - stepsPerRev int - homeRPM float64 - maxRPM float64 - maxAcc float64 - fClk float64 - logger logging.Logger - opMgr *operation.SingleOperationManager - powerPct float64 - motorName string -} - -// TMC5072 Values. -const ( - baseClk = 13200000 // Nominal 13.2mhz internal clock speed - uSteps = 256 // Microsteps per fullstep -) - -// SNEAKY TRICK ALERT! The TMC5072 always returns the value of the register from the *previous* -// command, not the current one. For an example, see the top of page 18 of -// https://www.analog.com/media/en/technical-documentation/data-sheets/TMC5072_datasheet_rev1.26.pdf -// So, to get accurate reads, request the read twice. Use a global mutex to ensure no race -// conditions when multiple components access the chip. -var globalMu sync.Mutex - -// TMC5072 Register Addressses (for motor index 1) -// TODO full register set. -const ( - // add 0x10 for motor 2. - chopConf = 0x6C - coolConf = 0x6D - drvStatus = 0x6F - - // add 0x20 for motor 2. - rampMode = 0x20 - xActual = 0x21 - // vActual = 0x22. - vStart = 0x23 - a1 = 0x24 - v1 = 0x25 - aMax = 0x26 - vMax = 0x27 - dMax = 0x28 - d1 = 0x2A - vStop = 0x2B - xTarget = 0x2D - iHoldIRun = 0x30 - vCoolThres = 0x31 - swMode = 0x34 - rampStat = 0x35 -) - -// TMC5072 ramp modes. -const ( - modePosition = int32(0) - modeVelPos = int32(1) - modeVelNeg = int32(2) - modeHold = int32(3) -) - -// newMotor returns a TMC5072 driven motor. -func newMotor(ctx context.Context, deps resource.Dependencies, c resource.Config, logger logging.Logger, -) (motor.Motor, error) { - conf, err := resource.NativeConfig[*TMC5072Config](c) - if err != nil { - return nil, err - } - bus := buses.NewSpiBus(conf.SPIBus) - return makeMotor(ctx, deps, *conf, c.ResourceName(), logger, bus) -} - -// makeMotor returns a TMC5072 driven motor. It is separate from NewMotor, above, so you can inject -// a mock SPI bus in here during testing. -func makeMotor(ctx context.Context, deps resource.Dependencies, c TMC5072Config, name resource.Name, - logger logging.Logger, bus buses.SPI, -) (motor.Motor, error) { - if c.MaxRPM == 0 { - logger.CWarn(ctx, "max_rpm not set, setting to 200 rpm") - c.MaxRPM = 200 - } - if c.MaxAcceleration == 0 { - logger.CWarn(ctx, "max_acceleration_rpm_per_sec not set, setting to 200 rpm/sec") - c.MaxAcceleration = 200 - } - if c.CalFactor == 0 { - c.CalFactor = 1.0 - } - - if c.TicksPerRotation == 0 { - return nil, errors.New("ticks_per_rotation isn't set") - } - - if c.HomeRPM == 0 { - logger.CWarn(ctx, "home_rpm not set: defaulting to 1/4 of max_rpm") - c.HomeRPM = c.MaxRPM / 4 - } - c.HomeRPM *= -1 - - m := &Motor{ - Named: name.AsNamed(), - bus: bus, - csPin: c.ChipSelect, - index: c.Index, - stepsPerRev: c.TicksPerRotation * uSteps, - homeRPM: c.HomeRPM, - maxRPM: c.MaxRPM, - maxAcc: c.MaxAcceleration, - fClk: baseClk / c.CalFactor, - logger: logger, - opMgr: operation.NewSingleOperationManager(), - motorName: name.ShortName(), - } - - rawMaxAcc := m.rpmsToA(m.maxAcc) - - if c.SGThresh > 63 { - c.SGThresh = 63 - } else if c.SGThresh < -64 { - c.SGThresh = -64 - } - // The register is a 6 bit signed int - if c.SGThresh < 0 { - c.SGThresh = int32(64 + math.Abs(float64(c.SGThresh))) - } - - // Hold/Run currents are 0-31 (linear scale), - // but we'll take 1-32 so zero can remain default - if c.RunCurrent == 0 { - c.RunCurrent = 15 // Default - } else { - c.RunCurrent-- - } - - if c.RunCurrent > 31 { - c.RunCurrent = 31 - } else if c.RunCurrent < 0 { - c.RunCurrent = 0 - } - - if c.HoldCurrent == 0 { - c.HoldCurrent = 8 // Default - } else { - c.HoldCurrent-- - } - - if c.HoldCurrent > 31 { - c.HoldCurrent = 31 - } else if c.HoldCurrent < 0 { - c.HoldCurrent = 0 - } - - // HoldDelay is 2^18 clocks per step between current stepdown phases - // Approximately 1/16th of a second for default 16mhz clock - // Repurposing zero for default, and -1 for "instant" - if c.HoldDelay == 0 { - c.HoldDelay = 6 // default - } else if c.HoldDelay < 0 { - c.HoldDelay = 0 - } - - if c.HoldDelay > 15 { - c.HoldDelay = 15 - } - - coolConfig := c.SGThresh << 16 - - iCfg := c.HoldDelay<<16 | c.RunCurrent<<8 | c.HoldCurrent - - err := multierr.Combine( - m.writeReg(ctx, chopConf, 0x000100C3), // TOFF=3, HSTRT=4, HEND=1, TBL=2, CHM=0 (spreadCycle) - m.writeReg(ctx, iHoldIRun, iCfg), - m.writeReg(ctx, coolConf, coolConfig), // Sets just the SGThreshold (for now) - - // Set max acceleration and decceleration - m.writeReg(ctx, a1, rawMaxAcc), - m.writeReg(ctx, aMax, rawMaxAcc), - m.writeReg(ctx, d1, rawMaxAcc), - m.writeReg(ctx, dMax, rawMaxAcc), - - m.writeReg(ctx, vStart, 1), // Always start at min speed - m.writeReg(ctx, vStop, 10), // Always count a stop as LOW speed, but where vStop > vStart - m.writeReg(ctx, v1, m.rpmToV(m.maxRPM/4)), // Transition ramp at 25% speed (if d1 and a1 are set different) - m.writeReg(ctx, vCoolThres, m.rpmToV(m.maxRPM/20)), // Set minimum speed for stall detection and coolstep - m.writeReg(ctx, vMax, m.rpmToV(0)), // Max velocity to zero, we don't want to move - - m.writeReg(ctx, rampMode, modeVelPos), // Lastly, set velocity mode to force a stop in case chip was left in moving state - m.writeReg(ctx, xActual, 0), // Zero the position - ) - if err != nil { - return nil, err - } - - if c.Pins.EnablePinLow != "" { - b, err := board.FromDependencies(deps, c.BoardName) - if err != nil { - return nil, errors.Errorf("%q is not a board", c.BoardName) - } - - m.enLowPin, err = b.GPIOPinByName(c.Pins.EnablePinLow) - if err != nil { - return nil, err - } - err = m.Enable(ctx, true) - if err != nil { - return nil, err - } - } - - return m, nil -} - -func (m *Motor) shiftAddr(addr uint8) uint8 { - // Shift register address for motor 2 instead of motor 1 - if m.index == 2 { - switch { - case addr >= 0x10 && addr <= 0x11: - addr += 0x08 - case addr >= 0x20 && addr <= 0x3C: - addr += 0x20 - case addr >= 0x6A && addr <= 0x6F: - addr += 0x10 - } - } - return addr -} - -func (m *Motor) writeReg(ctx context.Context, addr uint8, value int32) error { - addr = m.shiftAddr(addr) - - var buf [5]byte - buf[0] = addr | 0x80 - buf[1] = 0xFF & byte(value>>24) - buf[2] = 0xFF & byte(value>>16) - buf[3] = 0xFF & byte(value>>8) - buf[4] = 0xFF & byte(value) - - handle, err := m.bus.OpenHandle() - if err != nil { - return err - } - defer func() { - if err := handle.Close(); err != nil { - m.logger.CError(ctx, err) - } - }() - - m.logger.Debugf("Write to 0x%x: %v", addr, buf[1:]) - - // Ensure we're not writing in the middle of another component attempting to read (which would - // otherwise be non-atomic). - globalMu.Lock() - defer globalMu.Unlock() - - _, err = handle.Xfer(ctx, 1000000, m.csPin, 3, buf[:]) // SPI Mode 3, 1mhz - if err != nil { - return err - } - - return nil -} - -func (m *Motor) readReg(ctx context.Context, addr uint8) (int32, error) { - addr = m.shiftAddr(addr) - - var tbuf [5]byte - tbuf[0] = addr - - handle, err := m.bus.OpenHandle() - if err != nil { - return 0, err - } - defer func() { - if err := handle.Close(); err != nil { - m.logger.CError(ctx, err) - } - }() - - // Read access returns data from the address sent in the PREVIOUS "packet," so we transmit, - // then read. Ensure that another component can't interact with the chip in between our two - // commands. - globalMu.Lock() - defer globalMu.Unlock() - - _, err = handle.Xfer(ctx, 1000000, m.csPin, 3, tbuf[:]) // SPI Mode 3, 1mhz - if err != nil { - return 0, err - } - - rbuf, err := handle.Xfer(ctx, 1000000, m.csPin, 3, tbuf[:]) - if err != nil { - return 0, err - } - - var value int32 - value = int32(rbuf[1]) - value <<= 8 - value |= int32(rbuf[2]) - value <<= 8 - value |= int32(rbuf[3]) - value <<= 8 - value |= int32(rbuf[4]) - - m.logger.Debugf("Read from 0x%x: %d (%v)", addr, value, rbuf[1:]) - - return value, nil -} - -// GetSG returns the current StallGuard reading (effectively an indication of motor load.) -func (m *Motor) GetSG(ctx context.Context) (int32, error) { - rawRead, err := m.readReg(ctx, drvStatus) - if err != nil { - return 0, err - } - - rawRead &= 1023 - return rawRead, nil -} - -// Position gives the current motor position. -func (m *Motor) Position(ctx context.Context, extra map[string]interface{}) (float64, error) { - rawPos, err := m.readReg(ctx, xActual) - if err != nil { - return 0, errors.Wrapf(err, "error in Position from motor (%s)", m.motorName) - } - return float64(rawPos) / float64(m.stepsPerRev), nil -} - -// Properties returns the status of optional properties on the motor. -func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { - return motor.Properties{ - PositionReporting: true, - }, nil -} - -// SetPower sets the motor at a particular rpm based on the percent of -// maxRPM supplied by powerPct (between -1 and 1). -func (m *Motor) SetPower(ctx context.Context, powerPct float64, extra map[string]interface{}) error { - m.opMgr.CancelRunning(ctx) - m.powerPct = powerPct - return m.doJog(ctx, powerPct*m.maxRPM) -} - -// Jog sets a fixed RPM. -func (m *Motor) Jog(ctx context.Context, rpm float64) error { - m.opMgr.CancelRunning(ctx) - return m.doJog(ctx, rpm) -} - -func (m *Motor) doJog(ctx context.Context, rpm float64) error { - mode := modeVelPos - if rpm < 0 { - mode = modeVelNeg - } - - warning, err := motor.CheckSpeed(rpm, m.maxRPM) - // only display warnings if rpm != 0 because Stop calls doJog with an rpm of 0 - if rpm != 0 { - if warning != "" { - m.logger.CWarn(ctx, warning) - } - if err != nil { - m.logger.CError(ctx, err) - } - } - - speed := m.rpmToV(math.Abs(rpm)) - return multierr.Combine( - m.writeReg(ctx, rampMode, mode), - m.writeReg(ctx, vMax, speed), - ) -} - -// GoFor turns in the given direction the given number of times at the given speed. -// Both the RPM and the revolutions can be assigned negative values to move in a backwards direction. -// Note: if both are negative the motor will spin in the forward direction. -func (m *Motor) GoFor(ctx context.Context, rpm, rotations float64, extra map[string]interface{}) error { - warning, err := motor.CheckSpeed(rpm, m.maxRPM) - if warning != "" { - m.logger.CWarn(ctx, warning) - } - if err != nil { - return err - } - - curPos, err := m.Position(ctx, extra) - if err != nil { - return errors.Wrapf(err, "error in GoFor from motor (%s)", m.motorName) - } - - var d int64 = 1 - if math.Signbit(rotations) != math.Signbit(rpm) { - d *= -1 - } - - rotations = math.Abs(rotations) * float64(d) - rpm = math.Abs(rpm) - - target := curPos + rotations - return m.GoTo(ctx, rpm, target, extra) -} - -// Convert rpm to TMC5072 steps/s. -func (m *Motor) rpmToV(rpm float64) int32 { - if rpm > m.maxRPM { - rpm = m.maxRPM - } - // Time constant for velocities in TMC5072 - tConst := m.fClk / math.Pow(2, 24) - speed := rpm / 60 * float64(m.stepsPerRev) / tConst - return int32(speed) -} - -// Convert rpm/s to TMC5072 steps/taConst^2. -func (m *Motor) rpmsToA(acc float64) int32 { - // Time constant for accelerations in TMC5072 - taConst := math.Pow(2, 41) / math.Pow(m.fClk, 2) - rawMaxAcc := acc / 60 * float64(m.stepsPerRev) * taConst - return int32(rawMaxAcc) -} - -// GoTo moves to the specified position in terms of (provided in revolutions from home/zero), -// at a specific speed. Regardless of the directionality of the RPM this function will move the -// motor towards the specified target. -func (m *Motor) GoTo(ctx context.Context, rpm, positionRevolutions float64, extra map[string]interface{}) error { - ctx, done := m.opMgr.New(ctx) - defer done() - - positionRevolutions *= float64(m.stepsPerRev) - - warning, err := motor.CheckSpeed(rpm, m.maxRPM) - if warning != "" { - m.logger.CWarn(ctx, warning) - } - if err != nil { - m.logger.CError(ctx, err) - } - - err = multierr.Combine( - m.writeReg(ctx, rampMode, modePosition), - m.writeReg(ctx, vMax, m.rpmToV(math.Abs(rpm))), - m.writeReg(ctx, xTarget, int32(positionRevolutions)), - ) - if err != nil { - return errors.Wrapf(err, "error in GoTo from motor (%s)", m.motorName) - } - - return m.opMgr.WaitForSuccess( - ctx, - time.Millisecond*10, - m.IsStopped, - ) -} - -// SetRPM instructs the motor to move at the specified RPM indefinitely. -func (m *Motor) SetRPM(ctx context.Context, rpm float64, extra map[string]interface{}) error { - m.opMgr.CancelRunning(ctx) - return m.doJog(ctx, rpm) -} - -// 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 -} - -// IsStopped returns true if the motor is NOT moving. -func (m *Motor) IsStopped(ctx context.Context) (bool, error) { - stat, err := m.readReg(ctx, rampStat) - if err != nil { - return false, errors.Wrapf(err, "error in IsStopped from motor (%s)", m.motorName) - } - // Look for vzero flag - return stat&0x400 == 0x400, nil -} - -// AtVelocity returns true if the motor has reached the requested velocity. -func (m *Motor) AtVelocity(ctx context.Context) (bool, error) { - stat, err := m.readReg(ctx, rampStat) - if err != nil { - return false, err - } - // Look for velocity reached flag - return stat&0x100 == 0x100, nil -} - -// Enable pulls down the hardware enable pin, activating the power stage of the chip. -func (m *Motor) Enable(ctx context.Context, turnOn bool) error { - if m.enLowPin == nil { - return errors.New("no enable pin configured") - } - return m.enLowPin.Set(ctx, !turnOn, nil) -} - -// Stop stops the motor. -func (m *Motor) Stop(ctx context.Context, extra map[string]interface{}) error { - m.opMgr.CancelRunning(ctx) - return m.doJog(ctx, 0) -} - -// IsMoving returns true if the motor is currently moving. -func (m *Motor) IsMoving(ctx context.Context) (bool, error) { - stop, err := m.IsStopped(ctx) - return !stop, err -} - -// home homes the motor using stallguard. -func (m *Motor) home(ctx context.Context) error { - err := m.goTillStop(ctx, m.homeRPM, nil) - if err != nil { - return err - } - for { - stopped, err := m.IsStopped(ctx) - if err != nil { - return err - } - if stopped { - break - } - } - - return m.ResetZeroPosition(ctx, 0, nil) -} - -// goTillStop enables StallGuard detection, then moves in the direction/speed given until resistance (endstop) is detected. -func (m *Motor) goTillStop(ctx context.Context, rpm float64, stopFunc func(ctx context.Context) bool) error { - if err := m.Jog(ctx, rpm); err != nil { - return err - } - ctx, done := m.opMgr.New(ctx) - defer done() - - // Disable stallguard and turn off if we fail homing - defer func() { - if err := multierr.Combine( - m.writeReg(ctx, swMode, 0x000), - m.doJog(ctx, 0), - ); err != nil { - m.logger.CError(ctx, err) - } - }() - - // Get up to speed - var fails int - for { - if !utils.SelectContextOrWait(ctx, 10*time.Millisecond) { - return errors.New("context cancelled: duration timeout trying to get up to speed while homing") - } - - if stopFunc != nil && stopFunc(ctx) { - return nil - } - - ready, err := m.AtVelocity(ctx) - if err != nil { - return err - } - - if ready { - break - } - - if fails >= 500 { - return errors.New("over 500 failures trying to get up to speed while homing") - } - fails++ - } - - // Now enable stallguard - if err := m.writeReg(ctx, swMode, 0x400); err != nil { - return err - } - - // Wait for motion to stop at endstop - fails = 0 - for { - if !utils.SelectContextOrWait(ctx, 10*time.Millisecond) { - return errors.New("context cancelled: duration timeout trying to stop at the endstop while homing") - } - - if stopFunc != nil && stopFunc(ctx) { - return nil - } - - stopped, err := m.IsStopped(ctx) - if err != nil { - return err - } - if stopped { - break - } - - if fails >= 10000 { - return errors.New("over 1000 failures trying to stop at endstop while homing") - } - fails++ - } - - return nil -} - -// ResetZeroPosition sets the current position of the motor specified by the request -// (adjusted by a given offset) to be its new zero position. -func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { - on, _, err := m.IsPowered(ctx, extra) - if err != nil { - return errors.Wrapf(err, "error in ResetZeroPosition from motor (%s)", m.motorName) - } else if on { - return errors.Errorf("can't zero motor (%s) while moving", m.motorName) - } - return multierr.Combine( - m.writeReg(ctx, rampMode, modeHold), - m.writeReg(ctx, xTarget, int32(-1*offset*float64(m.stepsPerRev))), - m.writeReg(ctx, xActual, int32(-1*offset*float64(m.stepsPerRev))), - ) -} - -// DoCommand() related constants. -const ( - Command = "command" - Home = "home" - Jog = "jog" - RPMVal = "rpm" -) - -// DoCommand executes additional commands beyond the Motor{} interface. -func (m *Motor) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { - name, ok := cmd["command"] - if !ok { - return nil, errors.Errorf("missing %s value", Command) - } - switch name { - case Home: - return nil, m.home(ctx) - case Jog: - rpmRaw, ok := cmd[RPMVal] - if !ok { - return nil, errors.Errorf("need %s value for jog", RPMVal) - } - rpm, ok := rpmRaw.(float64) - if !ok { - return nil, errors.New("rpm value must be floating point") - } - return nil, m.Jog(ctx, rpm) - default: - return nil, errors.Errorf("no such command: %s", name) - } -} diff --git a/components/motor/tmcstepper/stepper_motor_tmc_nonlinux.go b/components/motor/tmcstepper/stepper_motor_tmc_nonlinux.go deleted file mode 100644 index 4773a90f92b..00000000000 --- a/components/motor/tmcstepper/stepper_motor_tmc_nonlinux.go +++ /dev/null @@ -1,2 +0,0 @@ -// Package tmcstepper is only implemented on Linux. -package tmcstepper diff --git a/components/motor/tmcstepper/stepper_motor_tmc_test.go b/components/motor/tmcstepper/stepper_motor_tmc_test.go deleted file mode 100644 index 7da9ab234cf..00000000000 --- a/components/motor/tmcstepper/stepper_motor_tmc_test.go +++ /dev/null @@ -1,744 +0,0 @@ -//go:build linux - -// Package tmcstepper contains the TMC stepper motor driver. This file contains unit tests for it. -package tmcstepper - -import ( - "context" - "fmt" - "strings" - "testing" - - "go.viam.com/test" - - "go.viam.com/rdk/components/board/genericlinux/buses" - "go.viam.com/rdk/components/motor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/testutils/inject" -) - -type fakeSpiHandle struct { - tx, rx [][]byte // tx and rx must have the same length - i int // Index of the next tx/rx pair to use - tb testing.TB -} - -func newFakeSpiHandle(tb testing.TB) fakeSpiHandle { - h := fakeSpiHandle{} - h.rx = [][]byte{} - h.tx = [][]byte{} - h.i = 0 - h.tb = tb - return h -} - -func (h *fakeSpiHandle) Xfer( - ctx context.Context, - baud uint, - chipSelect string, - mode uint, - tx []byte, -) ([]byte, error) { - test.That(h.tb, tx, test.ShouldResemble, h.tx[h.i]) - result := h.rx[h.i] - h.i++ - return result, nil -} - -func (h *fakeSpiHandle) Close() error { - return nil -} - -func (h *fakeSpiHandle) AddExpectedTx(expects [][]byte) { - for _, line := range expects { - h.tx = append(h.tx, line) - h.rx = append(h.rx, make([]byte, len(line))) - } -} - -func (h *fakeSpiHandle) AddExpectedRx(expects, sends [][]byte) { - h.tx = append(h.tx, expects...) - h.rx = append(h.rx, sends...) -} - -func (h *fakeSpiHandle) ExpectDone() { - // Assert that all expected data was transmitted - test.That(h.tb, h.i, test.ShouldEqual, len(h.tx)) -} - -func newFakeSpi(tb testing.TB) (*fakeSpiHandle, buses.SPI) { - handle := newFakeSpiHandle(tb) - fakeSpi := inject.SPI{} - fakeSpi.OpenHandleFunc = func() (buses.SPIHandle, error) { - return &handle, nil - } - - return &handle, &fakeSpi -} - -const maxRpm = 500 - -func TestRPMBounds(t *testing.T) { - ctx := context.Background() - logger, obs := logging.NewObservedTestLogger(t) - - getLastLogLine := func() string { - lastLine := "" - // Filter out the debug logs that just indicate what we sent/received on the SPI bus. - for _, lineVal := range obs.All() { - line := fmt.Sprint(lineVal) - if strings.Contains(line, "Read from ") || strings.Contains(line, "Write to ") { - continue - } - lastLine = line - } - return lastLine - } - - fakeSpiHandle, fakeSpi := newFakeSpi(t) - var deps resource.Dependencies - - mc := TMC5072Config{ - SPIBus: "3", - ChipSelect: "40", - Index: 1, - SGThresh: 0, - CalFactor: 1.0, - MaxAcceleration: 500, - MaxRPM: maxRpm, - TicksPerRotation: 200, - } - - // These are the setup register writes - fakeSpiHandle.AddExpectedTx([][]byte{ - {236, 0, 1, 0, 195}, - {176, 0, 6, 15, 8}, - {237, 0, 0, 0, 0}, - {164, 0, 0, 21, 8}, - {166, 0, 0, 21, 8}, - {170, 0, 0, 21, 8}, - {168, 0, 0, 21, 8}, - {163, 0, 0, 0, 1}, - {171, 0, 0, 0, 10}, - {165, 0, 2, 17, 149}, - {177, 0, 0, 105, 234}, - {167, 0, 0, 0, 0}, - {160, 0, 0, 0, 1}, - {161, 0, 0, 0, 0}, - }) - - name := resource.NewName(motor.API, "motor1") - motorDep, err := makeMotor(ctx, deps, mc, name, logger, fakeSpi) - test.That(t, err, test.ShouldBeNil) - defer func() { - fakeSpiHandle.ExpectDone() - test.That(t, motorDep.Close(context.Background()), test.ShouldBeNil) - }() - - test.That(t, motorDep.GoFor(ctx, 0.05, 6.6, nil), test.ShouldBeError, motor.NewZeroRPMError()) - test.That(t, getLastLogLine(), test.ShouldContainSubstring, "nearly 0") - - // Check with position at 0.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 8, 70, 85}, - {173, 0, 5, 40, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 500, 6.6, nil), test.ShouldBeNil) - test.That(t, getLastLogLine(), test.ShouldContainSubstring, "nearly the max") -} - -func TestTMCStepperMotor(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - - fakeSpiHandle, fakeSpi := newFakeSpi(t) - var deps resource.Dependencies - - mc := TMC5072Config{ - SPIBus: "main", - ChipSelect: "40", - Index: 1, - SGThresh: 0, - CalFactor: 1.0, - MaxAcceleration: 500, - MaxRPM: maxRpm, - TicksPerRotation: 200, - } - - // These are the setup register writes - fakeSpiHandle.AddExpectedTx([][]byte{ - {236, 0, 1, 0, 195}, - {176, 0, 6, 15, 8}, - {237, 0, 0, 0, 0}, - {164, 0, 0, 21, 8}, - {166, 0, 0, 21, 8}, - {170, 0, 0, 21, 8}, - {168, 0, 0, 21, 8}, - {163, 0, 0, 0, 1}, - {171, 0, 0, 0, 10}, - {165, 0, 2, 17, 149}, - {177, 0, 0, 105, 234}, - {167, 0, 0, 0, 0}, - {160, 0, 0, 0, 1}, - {161, 0, 0, 0, 0}, - }) - - name := resource.NewName(motor.API, "motor1") - motorDep, err := makeMotor(ctx, deps, mc, name, logger, fakeSpi) - test.That(t, err, test.ShouldBeNil) - defer func() { - fakeSpiHandle.ExpectDone() - test.That(t, motorDep.Close(context.Background()), test.ShouldBeNil) - }() - - t.Run("motor supports position reporting", func(t *testing.T) { - properties, err := motorDep.Properties(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, properties.PositionReporting, test.ShouldBeTrue) - }) - - t.Run("motor SetPower testing", func(t *testing.T) { - // Test Go forward at half speed - fakeSpiHandle.AddExpectedTx([][]byte{ - {160, 0, 0, 0, 1}, - {167, 0, 4, 35, 42}, - }) - test.That(t, motorDep.SetPower(ctx, 0.5, nil), test.ShouldBeNil) - - // Test Go backward at quarter speed - fakeSpiHandle.AddExpectedTx([][]byte{ - {160, 0, 0, 0, 2}, - {167, 0, 2, 17, 149}, - }) - test.That(t, motorDep.SetPower(ctx, -0.25, nil), test.ShouldBeNil) - }) - - t.Run("motor SetRPM testing", func(t *testing.T) { - // Test Go forward at half speed - fakeSpiHandle.AddExpectedTx([][]byte{ - {160, 0, 0, 0, 1}, - {167, 0, 4, 35, 42}, - }) - test.That(t, motorDep.SetRPM(ctx, 250, nil), test.ShouldBeNil) - - // Test Go backward at quarter speed - fakeSpiHandle.AddExpectedTx([][]byte{ - {160, 0, 0, 0, 2}, - {167, 0, 2, 17, 149}, - }) - test.That(t, motorDep.SetRPM(ctx, -125, nil), test.ShouldBeNil) - }) - - t.Run("motor Off testing", func(t *testing.T) { - fakeSpiHandle.AddExpectedTx([][]byte{ - {160, 0, 0, 0, 1}, - {167, 0, 0, 0, 0}, - }) - test.That(t, motorDep.Stop(ctx, nil), test.ShouldBeNil) - }) - - t.Run("motor position testing", func(t *testing.T) { - // Check at 4.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 1, 6, 18, 3}, // Can be gibberish, only second register is valid - {0, 0, 3, 32, 0}, - }, - ) - pos, err := motorDep.Position(ctx, nil) - test.That(t, pos, test.ShouldEqual, 4.0) - test.That(t, err, test.ShouldBeNil) - }) - - t.Run("motor GoFor with positive rpm and positive revolutions", func(t *testing.T) { - // Check with position at 0.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 2, 128, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 50.0, 3.2, nil), test.ShouldBeNil) - - // Check with position at 4.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 5, 160, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 8, 98, 98, 7}, // Can be gibberish - {0, 0, 3, 32, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 50.0, 3.2, nil), test.ShouldBeNil) - - // Check with position at 1.2 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 6, 24, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 240, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 50.0, 6.6, nil), test.ShouldBeNil) - }) - - t.Run("motor GoFor with negative rpm and positive revolutions", func(t *testing.T) { - // Check with position at 0.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 255, 253, 128, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, -50.0, 3.2, nil), test.ShouldBeNil) - - // Check with position at 4.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 0, 159, 255}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 8, 98, 98, 7}, // Can be gibberish - {0, 0, 3, 32, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, -50.0, 3.2, nil), test.ShouldBeNil) - - // Check with position at 1.2 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 255, 251, 200, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 240, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, -50.0, 6.6, nil), test.ShouldBeNil) - }) - - t.Run("motor GoFor with positive rpm and negative revolutions", func(t *testing.T) { - // Check with position at 0.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 255, 253, 128, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 50.0, -3.2, nil), test.ShouldBeNil) - - // Check with position at 4.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 0, 159, 255}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 8, 98, 98, 7}, // Can be gibberish - {0, 0, 3, 32, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 50.0, -3.2, nil), test.ShouldBeNil) - - // Check with position at 1.2 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 255, 251, 200, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 240, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, 50.0, -6.6, nil), test.ShouldBeNil) - }) - - t.Run("motor GoFor with negative rpm and negative revolutions", func(t *testing.T) { - // Check with position at 0.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 2, 128, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, -50.0, -3.2, nil), test.ShouldBeNil) - - // Check with position at 4.0 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 5, 160, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 8, 98, 98, 7}, // Can be gibberish - {0, 0, 3, 32, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, -50.0, -3.2, nil), test.ShouldBeNil) - - // Check with position at 1.2 revolutions - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {33, 0, 0, 0, 0}, - {33, 0, 0, 0, 0}, - {160, 0, 0, 0, 0}, - {167, 0, 0, 211, 213}, - {173, 0, 6, 24, 0}, - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 0, 0}, - {0, 0, 0, 240, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - test.That(t, motorDep.GoFor(ctx, -50.0, -6.6, nil), test.ShouldBeNil) - }) - - t.Run("motor GoFor with zero rpm", func(t *testing.T) { - test.That(t, motorDep.GoFor(ctx, 0, 1, nil), test.ShouldBeError, motor.NewZeroRPMError()) - }) - - t.Run("motor is on testing", func(t *testing.T) { - // Off - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - }, - ) - on, powerPct, err := motorDep.IsPowered(ctx, nil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, -0.25) - test.That(t, err, test.ShouldBeNil) - - // On - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 5, 0}, - {0, 0, 0, 0, 0}, - }, - ) - on, powerPct, err = motorDep.IsPowered(ctx, nil) - test.That(t, on, test.ShouldEqual, true) - test.That(t, powerPct, test.ShouldEqual, -0.25) - test.That(t, err, test.ShouldBeNil) - }) - - t.Run("motor zero testing", func(t *testing.T) { - // No offset (and when actually off) - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - {160, 0, 0, 0, 3}, - {173, 0, 0, 0, 0}, - {161, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - }, - ) - test.That(t, motorDep.ResetZeroPosition(ctx, 0, nil), test.ShouldBeNil) - - // No offset (and when actually on) - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - }, - [][]byte{ - {0, 0, 128, 0, 4}, - {0, 0, 0, 0, 0}, - }, - ) - test.That(t, motorDep.ResetZeroPosition(ctx, 0, nil), test.ShouldNotBeNil) - - // 3.1 offset (and when actually off) - fakeSpiHandle.AddExpectedRx( - [][]byte{ - {53, 0, 0, 0, 0}, - {53, 0, 0, 0, 0}, - {160, 0, 0, 0, 3}, - {173, 255, 253, 148, 0}, - {161, 255, 253, 148, 0}, - }, - [][]byte{ - {0, 0, 0, 4, 0}, - {0, 0, 0, 4, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, - }, - ) - test.That(t, motorDep.ResetZeroPosition(ctx, 3.1, nil), test.ShouldBeNil) - }) - - //nolint:dupl - t.Run("test over-limit current settings", func(*testing.T) { - mc.HoldDelay = 9999 - mc.RunCurrent = 9999 - mc.HoldCurrent = 9999 - - fakeSpiHandle, fakeSpi := newFakeSpi(t) - - // These are the setup register writes - fakeSpiHandle.AddExpectedTx([][]byte{ - {236, 0, 1, 0, 195}, - {176, 0, 15, 31, 31}, // Last three are delay, run, and hold - {237, 0, 0, 0, 0}, - {164, 0, 0, 21, 8}, - {166, 0, 0, 21, 8}, - {170, 0, 0, 21, 8}, - {168, 0, 0, 21, 8}, - {163, 0, 0, 0, 1}, - {171, 0, 0, 0, 10}, - {165, 0, 2, 17, 149}, - {177, 0, 0, 105, 234}, - {167, 0, 0, 0, 0}, - {160, 0, 0, 0, 1}, - {161, 0, 0, 0, 0}, - }) - - m, err := makeMotor(ctx, deps, mc, name, logger, fakeSpi) - test.That(t, err, test.ShouldBeNil) - fakeSpiHandle.ExpectDone() - test.That(t, m.Close(context.Background()), test.ShouldBeNil) - }) - - t.Run("test under-limit current settings", func(*testing.T) { - mc.HoldDelay = -9999 - mc.RunCurrent = -9999 - mc.HoldCurrent = -9999 - - fakeSpiHandle, fakeSpi := newFakeSpi(t) - - // These are the setup register writes - fakeSpiHandle.AddExpectedTx([][]byte{ - {236, 0, 1, 0, 195}, - {176, 0, 0, 0, 0}, // Last three are delay, run, and hold - {237, 0, 0, 0, 0}, - {164, 0, 0, 21, 8}, - {166, 0, 0, 21, 8}, - {170, 0, 0, 21, 8}, - {168, 0, 0, 21, 8}, - {163, 0, 0, 0, 1}, - {171, 0, 0, 0, 10}, - {165, 0, 2, 17, 149}, - {177, 0, 0, 105, 234}, - {167, 0, 0, 0, 0}, - {160, 0, 0, 0, 1}, - {161, 0, 0, 0, 0}, - }) - - m, err := makeMotor(ctx, deps, mc, name, logger, fakeSpi) - test.That(t, err, test.ShouldBeNil) - fakeSpiHandle.ExpectDone() - test.That(t, m.Close(context.Background()), test.ShouldBeNil) - }) - - //nolint:dupl - t.Run("test explicit current settings", func(*testing.T) { - mc.HoldDelay = 12 - // Currents will be passed as one less, as default is repurposed - mc.RunCurrent = 27 - mc.HoldCurrent = 14 - - fakeSpiHandle, fakeSpi := newFakeSpi(t) - - // These are the setup register writes - fakeSpiHandle.AddExpectedTx([][]byte{ - {236, 0, 1, 0, 195}, - {176, 0, 12, 26, 13}, // Last three are delay, run, and hold - {237, 0, 0, 0, 0}, - {164, 0, 0, 21, 8}, - {166, 0, 0, 21, 8}, - {170, 0, 0, 21, 8}, - {168, 0, 0, 21, 8}, - {163, 0, 0, 0, 1}, - {171, 0, 0, 0, 10}, - {165, 0, 2, 17, 149}, - {177, 0, 0, 105, 234}, - {167, 0, 0, 0, 0}, - {160, 0, 0, 0, 1}, - {161, 0, 0, 0, 0}, - }) - - m, err := makeMotor(ctx, deps, mc, name, logger, fakeSpi) - test.That(t, err, test.ShouldBeNil) - fakeSpiHandle.ExpectDone() - test.That(t, m.Close(context.Background()), test.ShouldBeNil) - }) -} diff --git a/components/motor/ulnstepper/28byj-48.go b/components/motor/ulnstepper/28byj-48.go deleted file mode 100644 index 462449de270..00000000000 --- a/components/motor/ulnstepper/28byj-48.go +++ /dev/null @@ -1,473 +0,0 @@ -// Package uln28byj implements a GPIO based -// stepper motor (model: 28byj-48) with uln2003 controler. -package uln28byj - -/* - Motor Name: 28byj-48 - Motor Controler: ULN2003 - Datasheet: - ULN2003: https://www.makerguides.com/wp-content/uploads/2019/04/ULN2003-Datasheet.pdf - 28byj-48: https://components101.com/sites/default/files/component_datasheet/28byj48-step-motor-datasheet.pdf - - This driver will drive the motor with half-step driving method (instead of full-step drive) for higher resolutions. - In half-step the current vector divides a circle into eight parts. The eight step switching sequence is shown in - stepSequence below. The motor takes 5.625*(1/64)° per step. For 360° the motor will take 4096 steps. - - The motor can run at a max speed of ~146rpm. Though it is recommended to not run the motor at max speed as it can - damage the gears. The max rpm of the motor shaft after gear reduction is ~15rpm. -*/ - -import ( - "context" - "math" - "sync" - "time" - - "github.com/pkg/errors" - "go.uber.org/multierr" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/components/motor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/resource" -) - -var ( - model = resource.DefaultModelFamily.WithModel("28byj48") - minDelayBetweenTicks = 100 * time.Microsecond // minimum sleep time between each ticks - maxRPM = 15.0 // max rpm of the 28byj-48 motor after gear reduction -) - -// stepSequence contains switching signal for uln2003 pins. -// Treversing through stepSequence once is one step. -var stepSequence = [8][4]bool{ - {false, false, false, true}, - {true, false, false, true}, - {true, false, false, false}, - {true, true, false, false}, - {false, true, false, false}, - {false, true, true, false}, - {false, false, true, false}, - {false, false, true, true}, -} - -// PinConfig defines the mapping of where motor are wired. -type PinConfig struct { - In1 string `json:"in1"` - In2 string `json:"in2"` - In3 string `json:"in3"` - In4 string `json:"in4"` -} - -// Config describes the configuration of a motor. -type Config struct { - Pins PinConfig `json:"pins"` - BoardName string `json:"board"` - TicksPerRotation int `json:"ticks_per_rotation"` -} - -// Validate ensures all parts of the config are valid. -func (conf *Config) Validate(path string) ([]string, error) { - var deps []string - if conf.BoardName == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "board") - } - - if conf.Pins.In1 == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "in1") - } - - if conf.Pins.In2 == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "in2") - } - - if conf.Pins.In3 == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "in3") - } - - if conf.Pins.In4 == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "in4") - } - - deps = append(deps, conf.BoardName) - return deps, nil -} - -func init() { - resource.RegisterComponent(motor.API, model, resource.Registration[motor.Motor, *Config]{ - Constructor: new28byj, - }) -} - -func new28byj( - ctx context.Context, deps resource.Dependencies, conf resource.Config, logger logging.Logger, -) (motor.Motor, error) { - mc, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - - b, err := board.FromDependencies(deps, mc.BoardName) - if err != nil { - return nil, errors.Wrap(err, "expected board name in config for motor") - } - - if mc.TicksPerRotation <= 0 { - return nil, errors.New("expected ticks_per_rotation to be greater than zero in config for motor") - } - - m := &uln28byj{ - Named: conf.ResourceName().AsNamed(), - theBoard: b, - ticksPerRotation: mc.TicksPerRotation, - logger: logger, - motorName: conf.Name, - opMgr: operation.NewSingleOperationManager(), - } - - in1, err := b.GPIOPinByName(mc.Pins.In1) - if err != nil { - return nil, errors.Wrapf(err, "in in1 in motor (%s)", m.motorName) - } - m.in1 = in1 - - in2, err := b.GPIOPinByName(mc.Pins.In2) - if err != nil { - return nil, errors.Wrapf(err, "in in2 in motor (%s)", m.motorName) - } - m.in2 = in2 - - in3, err := b.GPIOPinByName(mc.Pins.In3) - if err != nil { - return nil, errors.Wrapf(err, "in in3 in motor (%s)", m.motorName) - } - m.in3 = in3 - - in4, err := b.GPIOPinByName(mc.Pins.In4) - if err != nil { - return nil, errors.Wrapf(err, "in in4 in motor (%s)", m.motorName) - } - m.in4 = in4 - - return m, nil -} - -// struct is named after the controler uln28byj. -type uln28byj struct { - resource.Named - resource.AlwaysRebuild - theBoard board.Board - ticksPerRotation int - in1, in2, in3, in4 board.GPIOPin - logger logging.Logger - motorName string - - // state - workers *utils.StoppableWorkers - lock sync.Mutex - opMgr *operation.SingleOperationManager - doRunDone func() - - stepPosition int64 - stepperDelay time.Duration - targetStepPosition int64 -} - -// doRun runs the motor till it reaches target step position. -func (m *uln28byj) doRun() { - // cancel doRun if it already exists - if m.doRunDone != nil { - m.doRunDone() - } - - // start a new doRun - var doRunCtx context.Context - doRunCtx, m.doRunDone = context.WithCancel(context.Background()) - m.workers = utils.NewBackgroundStoppableWorkers(func(ctx context.Context) { - for { - select { - case <-doRunCtx.Done(): - return - default: - } - - if m.getStepPosition() == m.getTargetStepPosition() { - if err := m.doStop(doRunCtx); err != nil { - m.logger.Errorf("error setting pins to zero %v", err) - return - } - } else { - err := m.doStep(doRunCtx, m.getStepPosition() < m.getTargetStepPosition()) - if err != nil { - m.logger.Errorf("error stepping %v", err) - return - } - } - } - }) -} - -// doStop sets all the pins to 0 to stop the motor. -func (m *uln28byj) doStop(ctx context.Context) error { - m.lock.Lock() - defer m.lock.Unlock() - return m.setPins(ctx, [4]bool{false, false, false, false}) -} - -// Depending on the direction, doStep will either treverse the stepSequence array in ascending -// or descending order. -func (m *uln28byj) doStep(ctx context.Context, forward bool) error { - m.lock.Lock() - defer m.lock.Unlock() - if forward { - m.stepPosition++ - } else { - m.stepPosition-- - } - - var nextStepSequence int - if m.stepPosition < 0 { - nextStepSequence = 7 + int(m.stepPosition%8) - } else { - nextStepSequence = int(m.stepPosition % 8) - } - - err := m.setPins(ctx, stepSequence[nextStepSequence]) - if err != nil { - return err - } - - time.Sleep(m.stepperDelay) - return nil -} - -// doTicks sets all 4 pins. -// must be called in locked context. -func (m *uln28byj) setPins(ctx context.Context, pins [4]bool) error { - err := multierr.Combine( - m.in1.Set(ctx, pins[0], nil), - m.in2.Set(ctx, pins[1], nil), - m.in3.Set(ctx, pins[2], nil), - m.in4.Set(ctx, pins[3], nil), - ) - - return err -} - -func (m *uln28byj) getTargetStepPosition() int64 { - m.lock.Lock() - defer m.lock.Unlock() - return m.targetStepPosition -} - -func (m *uln28byj) setTargetStepPosition(targetPos int64) { - m.lock.Lock() - defer m.lock.Unlock() - m.targetStepPosition = targetPos -} - -func (m *uln28byj) getStepPosition() int64 { - m.lock.Lock() - defer m.lock.Unlock() - return m.stepPosition -} - -func (m *uln28byj) setStepperDelay(delay time.Duration) { - m.lock.Lock() - defer m.lock.Unlock() - m.stepperDelay = delay -} - -// GoFor instructs the motor to go in a specific direction for a specific amount of -// revolutions at a given speed in revolutions per minute. Both the RPM and the revolutions -// can be assigned negative values to move in a backwards direction. Note: if both are negative -// the motor will spin in the forward direction. -func (m *uln28byj) GoFor(ctx context.Context, rpm, revolutions float64, extra map[string]interface{}) error { - ctx, done := m.opMgr.New(ctx) - defer done() - - warning, err := motor.CheckSpeed(rpm, maxRPM) - if warning != "" { - m.logger.CWarn(ctx, warning) - if err != nil { - m.logger.CError(ctx, err) - // only stop if we receive a zero RPM error - return m.Stop(ctx, extra) - } - // we do not get the zeroRPM error, but still want - // the motor to move at the maximum rpm - m.logger.CWarnf(ctx, "can only move at maxRPM of %v", maxRPM) - rpm = maxRPM * motor.GetSign(rpm) - } - - targetStepPosition, stepperDelay := m.goMath(rpm, revolutions) - m.setTargetStepPosition(targetStepPosition) - m.setStepperDelay(stepperDelay) - m.doRun() - - positionReached := func(ctx context.Context) (bool, error) { - return m.getTargetStepPosition() == m.getStepPosition(), nil - } - - err = m.opMgr.WaitForSuccess( - ctx, - m.stepperDelay, - positionReached, - ) - // Ignore the context canceled error - this occurs when the motor is stopped - // at the beginning of goForInternal - if !errors.Is(err, context.Canceled) { - return err - } - - return nil -} - -func (m *uln28byj) goMath(rpm, revolutions float64) (int64, time.Duration) { - var d int64 = 1 - - if math.Signbit(revolutions) != math.Signbit(rpm) { - d = -1 - } - - revolutions = math.Abs(revolutions) - rpm = math.Abs(rpm) * float64(d) - - targetPosition := m.getStepPosition() + int64(float64(d)*revolutions*float64(m.ticksPerRotation)) - stepperDelay := m.calcStepperDelay(rpm) - - return targetPosition, stepperDelay -} - -func (m *uln28byj) calcStepperDelay(rpm float64) time.Duration { - stepperDelay := time.Duration(int64((1/(math.Abs(rpm)*float64(m.ticksPerRotation)/60.0))*1000000)) * time.Microsecond - if stepperDelay < minDelayBetweenTicks { - m.logger.Debugf("Computed sleep time between ticks (%v) too short. Defaulting to %v", stepperDelay, minDelayBetweenTicks) - stepperDelay = minDelayBetweenTicks - } - return stepperDelay -} - -// GoTo instructs the motor to go to a specific position (provided in revolutions from home/zero), -// at a specific RPM. Regardless of the directionality of the RPM this function will move the motor -// towards the specified target. -func (m *uln28byj) GoTo(ctx context.Context, rpm, positionRevolutions float64, extra map[string]interface{}) error { - curPos, err := m.Position(ctx, extra) - if err != nil { - return errors.Wrapf(err, "error in GoTo from motor (%s)", m.motorName) - } - moveDistance := positionRevolutions - curPos - - m.logger.CDebugf(ctx, "Moving %v ticks at %v rpm", moveDistance, rpm) - - if moveDistance == 0 { - return nil - } - - return m.GoFor(ctx, math.Abs(rpm), moveDistance, extra) -} - -// SetRPM instructs the motor to move at the specified RPM indefinitely. -func (m *uln28byj) SetRPM(ctx context.Context, rpm float64, extra map[string]interface{}) error { - powerPct := rpm / maxRPM - return m.SetPower(ctx, powerPct, extra) -} - -// Set the current position (+/- offset) to be the new zero (home) position. -func (m *uln28byj) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { - newPosition := int64(-1 * offset * float64(m.ticksPerRotation)) - // use Stop to set the target position to the current position again - if err := m.Stop(ctx, extra); err != nil { - return err - } - m.lock.Lock() - defer m.lock.Unlock() - m.stepPosition = newPosition - m.targetStepPosition = newPosition - return nil -} - -// SetPower is invalid for this motor. -func (m *uln28byj) SetPower(ctx context.Context, powerPct float64, extra map[string]interface{}) error { - ctx, done := m.opMgr.New(ctx) - defer done() - - warning, err := motor.CheckSpeed(powerPct*maxRPM, maxRPM) - if warning != "" { - m.logger.CWarn(ctx, warning) - if err != nil { - m.logger.CError(ctx, err) - // only stop if we receive a zero RPM error - return m.Stop(ctx, extra) - } - } - - m.lock.Lock() - defer m.lock.Unlock() - direction := motor.GetSign(powerPct) // get the direction to set target to -ve/+ve Inf - m.targetStepPosition = int64(math.Inf(int(direction))) - powerPct = motor.ClampPower(powerPct) // ensure 1.0 max and -1.0 min - m.stepperDelay = m.calcStepperDelay(powerPct * maxRPM) - - m.doRun() - - return nil -} - -// Position reports the current step position of the motor. If it's not supported, the returned -// data is undefined. -func (m *uln28byj) Position(ctx context.Context, extra map[string]interface{}) (float64, error) { - m.lock.Lock() - defer m.lock.Unlock() - return float64(m.stepPosition) / float64(m.ticksPerRotation), nil -} - -// Properties returns the status of whether the motor supports certain optional properties. -func (m *uln28byj) Properties(ctx context.Context, extra map[string]interface{}) (motor.Properties, error) { - return motor.Properties{ - PositionReporting: true, - }, nil -} - -// IsMoving returns if the motor is currently moving. -func (m *uln28byj) IsMoving(ctx context.Context) (bool, error) { - m.lock.Lock() - defer m.lock.Unlock() - return m.stepPosition != m.targetStepPosition, nil -} - -// Stop turns the power to the motor off immediately, without any gradual step down. -func (m *uln28byj) Stop(ctx context.Context, extra map[string]interface{}) error { - if m.doRunDone != nil { - m.doRunDone() - } - m.lock.Lock() - defer m.lock.Unlock() - m.targetStepPosition = m.stepPosition - return nil -} - -// IsPowered returns whether or not the motor is currently on. It also returns the percent power -// that the motor has, but stepper motors only have this set to 0% or 100%, so it's a little -// redundant. -func (m *uln28byj) IsPowered(ctx context.Context, extra map[string]interface{}) (bool, float64, error) { - on, err := m.IsMoving(ctx) - if err != nil { - return on, 0.0, errors.Wrapf(err, "error in IsPowered from motor (%s)", m.motorName) - } - percent := 0.0 - if on { - percent = 1.0 - } - return on, percent, err -} - -func (m *uln28byj) Close(ctx context.Context) error { - if err := m.Stop(ctx, nil); err != nil { - return err - } - m.workers.Stop() - return nil -} diff --git a/components/motor/ulnstepper/28byj-48_test.go b/components/motor/ulnstepper/28byj-48_test.go deleted file mode 100644 index 48f63d083d1..00000000000 --- a/components/motor/ulnstepper/28byj-48_test.go +++ /dev/null @@ -1,403 +0,0 @@ -package uln28byj - -import ( - "context" - "errors" - "fmt" - "testing" - "time" - - "go.viam.com/test" - - "go.viam.com/rdk/components/board" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/testutils/inject" -) - -const ( - testBoardName = "fake_board" -) - -func setupDependencies(t *testing.T) resource.Dependencies { - t.Helper() - - testBoard := &inject.Board{} - in1 := &mockGPIOPin{} - in2 := &mockGPIOPin{} - in3 := &mockGPIOPin{} - in4 := &mockGPIOPin{} - - testBoard.GPIOPinByNameFunc = func(pin string) (board.GPIOPin, error) { - switch pin { - case "1": - return in1, nil - case "2": - return in2, nil - case "3": - return in3, nil - case "4": - return in4, nil - } - return nil, errors.New("pin name not found") - } - deps := make(resource.Dependencies) - deps[board.Named(testBoardName)] = testBoard - return deps -} - -func TestValid(t *testing.T) { - ctx, cancel := context.WithCancel(context.Background()) - logger := logging.NewTestLogger(t) - deps := setupDependencies(t) - - mc := Config{ - Pins: PinConfig{ - In1: "1", - In2: "2", - In3: "3", - In4: "4", - }, - BoardName: testBoardName, - } - - c := resource.Config{ - Name: "fake_28byj", - ConvertedAttributes: &mc, - } - - // Create motor with no board and default config - t.Run("motor initializing test with no board and default config", func(t *testing.T) { - _, err := new28byj(ctx, deps, c, 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 := new28byj(ctx, deps, c, logger) - test.That(t, err, test.ShouldNotBeNil) - }) - _, err := new28byj(ctx, deps, c, logger) - test.That(t, err, test.ShouldNotBeNil) - - mc.TicksPerRotation = 200 - - mm, err := new28byj(ctx, deps, c, logger) - test.That(t, err, test.ShouldBeNil) - - m := mm.(*uln28byj) - - t.Run("motor test supports position reporting", func(t *testing.T) { - properties, err := m.Properties(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, properties.PositionReporting, test.ShouldBeTrue) - }) - - t.Run("motor test isOn functionality", func(t *testing.T) { - on, powerPct, err := m.IsPowered(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, 0.0) - }) - - t.Run("motor testing with positive rpm and positive revolutions", func(t *testing.T) { - on, powerPct, err := m.IsPowered(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, 0.0) - - pos, err := m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) - }) - - t.Run("motor testing with negative rpm and positive revolutions", func(t *testing.T) { - on, powerPct, err := m.IsPowered(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, 0.0) - - pos, err := m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) - }) - - t.Run("motor testing with positive rpm and negative revolutions", func(t *testing.T) { - on, powerPct, err := m.IsPowered(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, 0.0) - - pos, err := m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) - }) - - t.Run("motor testing with negative rpm and negative revolutions", func(t *testing.T) { - on, powerPct, err := m.IsPowered(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, 0.0) - - pos, err := m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) - }) - - t.Run("motor testing with large # of revolutions", func(t *testing.T) { - on, powerPct, err := m.IsPowered(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, on, test.ShouldEqual, false) - test.That(t, powerPct, test.ShouldEqual, 0.0) - - err = m.Stop(ctx, nil) - test.That(t, err, test.ShouldBeNil) - - pos, err := m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldBeGreaterThanOrEqualTo, 0) - test.That(t, pos, test.ShouldBeLessThan, 202) - }) - - cancel() -} - -func TestFunctions(t *testing.T) { - ctx, cancel := context.WithCancel(context.Background()) - logger, obs := logging.NewObservedTestLogger(t) - deps := setupDependencies(t) - - mc := Config{ - Pins: PinConfig{ - In1: "1", - In2: "2", - In3: "3", - In4: "4", - }, - BoardName: testBoardName, - TicksPerRotation: 100, - } - - c := resource.Config{ - Name: "fake_28byj", - ConvertedAttributes: &mc, - } - mm, _ := new28byj(ctx, deps, c, logger) - m := mm.(*uln28byj) - - t.Run("test goMath", func(t *testing.T) { - targetPos, stepperdelay := m.goMath(100, 100) - test.That(t, targetPos, test.ShouldEqual, 10000) - test.That(t, stepperdelay, test.ShouldEqual, (6 * time.Millisecond)) - - targetPos, stepperdelay = m.goMath(-100, 100) - test.That(t, targetPos, test.ShouldEqual, -10000) - test.That(t, stepperdelay, test.ShouldEqual, (6 * time.Millisecond)) - - targetPos, stepperdelay = m.goMath(-100, -100) - test.That(t, targetPos, test.ShouldEqual, 10000) - test.That(t, stepperdelay, test.ShouldEqual, (6 * time.Millisecond)) - - targetPos, stepperdelay = m.goMath(-2, 50) - test.That(t, targetPos, test.ShouldEqual, -5000) - test.That(t, stepperdelay, test.ShouldEqual, (300 * time.Millisecond)) - - targetPos, stepperdelay = m.goMath(1, 400) - test.That(t, targetPos, test.ShouldEqual, 40000) - test.That(t, stepperdelay, test.ShouldEqual, (600 * time.Millisecond)) - - targetPos, stepperdelay = m.goMath(400, 2) - test.That(t, targetPos, test.ShouldEqual, 200) - test.That(t, stepperdelay, test.ShouldEqual, (1500 * time.Microsecond)) - - targetPos, stepperdelay = m.goMath(0, 2) - test.That(t, targetPos, test.ShouldEqual, 200) - test.That(t, stepperdelay, test.ShouldEqual, (100 * time.Microsecond)) - }) - - t.Run("test calcStepperDelay", func(t *testing.T) { - stepperdelay := m.calcStepperDelay(100) - test.That(t, stepperdelay, test.ShouldEqual, (6 * time.Millisecond)) - - stepperdelay = m.calcStepperDelay(-100) - test.That(t, stepperdelay, test.ShouldEqual, (6 * time.Millisecond)) - - stepperdelay = m.calcStepperDelay(-2) - test.That(t, stepperdelay, test.ShouldEqual, (300 * time.Millisecond)) - - stepperdelay = m.calcStepperDelay(1) - test.That(t, stepperdelay, test.ShouldEqual, (600 * time.Millisecond)) - - stepperdelay = m.calcStepperDelay(400) - test.That(t, stepperdelay, test.ShouldEqual, (1500 * time.Microsecond)) - - stepperdelay = m.calcStepperDelay(0) - test.That(t, stepperdelay, test.ShouldEqual, (100 * time.Microsecond)) - }) - - t.Run("test position", func(t *testing.T) { - err := m.ResetZeroPosition(ctx, -0.03, nil) - test.That(t, err, test.ShouldBeNil) - pos, err := m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0.03) - - err = m.ResetZeroPosition(ctx, 0.03, nil) - test.That(t, err, test.ShouldBeNil) - pos, err = m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, -0.03) - - err = m.ResetZeroPosition(ctx, 0, nil) - test.That(t, err, test.ShouldBeNil) - pos, err = m.Position(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, pos, test.ShouldEqual, 0) - }) - - t.Run("test GoFor", func(t *testing.T) { - err := m.GoFor(ctx, 0, 1, nil) - test.That(t, err, test.ShouldBeNil) - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - err = m.GoFor(ctx, -.009, 1, nil) - test.That(t, err, test.ShouldBeNil) - - err = m.GoFor(ctx, 146, 1, nil) - test.That(t, err, test.ShouldBeNil) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-2] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - }) - - t.Run("test SetRPM", func(t *testing.T) { - err := m.SetRPM(ctx, 0, nil) - test.That(t, err, test.ShouldBeNil) - - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - err = m.SetRPM(ctx, -.009, nil) - test.That(t, err, test.ShouldBeNil) - - err = m.SetRPM(ctx, 146, nil) - test.That(t, err, test.ShouldBeNil) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - }) - - t.Run("test SetPower", func(t *testing.T) { - err := m.SetPower(ctx, 0, nil) - test.That(t, err, test.ShouldBeNil) - - allObs := obs.All() - latestLoggedEntry := allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly 0") - - err = m.SetPower(ctx, -.009, nil) - test.That(t, err, test.ShouldBeNil) - - err = m.SetPower(ctx, 146, nil) - test.That(t, err, test.ShouldBeNil) - allObs = obs.All() - latestLoggedEntry = allObs[len(allObs)-1] - test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max") - }) - - cancel() -} - -func TestState(t *testing.T) { - ctx, cancel := context.WithCancel(context.Background()) - logger := logging.NewTestLogger(t) - deps := setupDependencies(t) - - mc := Config{ - Pins: PinConfig{ - In1: "1", - In2: "2", - In3: "3", - In4: "4", - }, - BoardName: testBoardName, - TicksPerRotation: 100, - } - - c := resource.Config{ - Name: "fake_28byj", - ConvertedAttributes: &mc, - } - mm, _ := new28byj(ctx, deps, c, logger) - m := mm.(*uln28byj) - - t.Run("test state", func(t *testing.T) { - err := m.ResetZeroPosition(ctx, -0.09, nil) - test.That(t, err, test.ShouldBeNil) - b := m.theBoard - var pin1Arr []bool - var pin2Arr []bool - var pin3Arr []bool - var pin4Arr []bool - - arrIn1 := []bool{true, true, false, false, false, false, false, true} - arrIn2 := []bool{false, true, true, true, false, false, false, false} - arrIn3 := []bool{false, false, false, true, true, true, false, false} - arrIn4 := []bool{false, false, false, false, false, true, true, true} - - for i := 0; i < 8; i++ { - // moving forward. - err := m.doStep(ctx, true) - test.That(t, err, test.ShouldBeNil) - - PinOut1, err := b.GPIOPinByName("1") - test.That(t, err, test.ShouldBeNil) - pinStruct, ok := PinOut1.(*mockGPIOPin) - test.That(t, ok, test.ShouldBeTrue) - pin1Arr = pinStruct.pinStates - - PinOut2, err := b.GPIOPinByName("2") - test.That(t, err, test.ShouldBeNil) - pinStruct2, ok := PinOut2.(*mockGPIOPin) - test.That(t, ok, test.ShouldBeTrue) - pin2Arr = pinStruct2.pinStates - - PinOut3, err := b.GPIOPinByName("3") - test.That(t, err, test.ShouldBeNil) - pinStruct3, ok := PinOut3.(*mockGPIOPin) - test.That(t, ok, test.ShouldBeTrue) - pin3Arr = pinStruct3.pinStates - - PinOut4, err := b.GPIOPinByName("4") - test.That(t, err, test.ShouldBeNil) - pinStruct4, ok := PinOut4.(*mockGPIOPin) - test.That(t, ok, test.ShouldBeTrue) - pin4Arr = pinStruct4.pinStates - } - - m.logger.Info("pin1Arr ", pin1Arr) - m.logger.Info("pin2Arr ", pin2Arr) - m.logger.Info("pin3Arr ", pin3Arr) - m.logger.Info("pin4Arr ", pin4Arr) - - test.That(t, pin1Arr, test.ShouldResemble, arrIn1) - test.That(t, pin2Arr, test.ShouldResemble, arrIn2) - test.That(t, pin3Arr, test.ShouldResemble, arrIn3) - test.That(t, pin4Arr, test.ShouldResemble, arrIn4) - }) - - cancel() -} - -type mockGPIOPin struct { - board.GPIOPin - pinStates []bool -} - -func (m *mockGPIOPin) Set(ctx context.Context, high bool, extra map[string]interface{}) error { - m.pinStates = append(m.pinStates, high) - return nil -}