From 54c9b348ecb98abc829e2f0322106606b3615059 Mon Sep 17 00:00:00 2001 From: Pete Garafano Date: Fri, 4 Nov 2022 16:51:14 -0400 Subject: [PATCH] RSDK-779: Add Support For Dimension Engineering Sabertooth Motor Controller (#1498) --- .../motor/dimensionengineering/common.go | 44 ++ .../motor/dimensionengineering/sabertooth.go | 562 ++++++++++++++++++ .../dimensionengineering/sabertooth_test.go | 418 +++++++++++++ components/motor/gpio/setup.go | 2 +- components/motor/register/register.go | 1 + 5 files changed, 1026 insertions(+), 1 deletion(-) create mode 100644 components/motor/dimensionengineering/common.go create mode 100644 components/motor/dimensionengineering/sabertooth.go create mode 100644 components/motor/dimensionengineering/sabertooth_test.go diff --git a/components/motor/dimensionengineering/common.go b/components/motor/dimensionengineering/common.go new file mode 100644 index 00000000000..c7221b95003 --- /dev/null +++ b/components/motor/dimensionengineering/common.go @@ -0,0 +1,44 @@ +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 new file mode 100644 index 00000000000..a536c91a723 --- /dev/null +++ b/components/motor/dimensionengineering/sabertooth.go @@ -0,0 +1,562 @@ +// Package dimensionengineering contains implementations of the dimensionengineering motor controls +package dimensionengineering + +import ( + "context" + "errors" + "fmt" + "io" + "math" + "strings" + "sync" + "time" + + "github.com/edaniels/golog" + "github.com/jacobsa/go-serial/serial" + "github.com/mitchellh/mapstructure" + utils "go.viam.com/utils" + + "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/config" + "go.viam.com/rdk/operation" + "go.viam.com/rdk/registry" + rdkutils "go.viam.com/rdk/utils" +) + +const ( + // https://www.dimensionengineering.com/datasheets/Sabertooth2x60.pdf + modelName = "de-sabertooth" +) + +// controllers is global to all instances, mapped by serial device. +var ( + globalMu sync.Mutex + controllers map[string]*controller +) + +// controller is common across all Sabertooth motor instances sharing a controller. +type controller struct { + mu sync.Mutex + port io.ReadWriteCloser + serialDevice string + logger golog.Logger + activeAxes map[int]bool + testChan chan []byte + address int // 128-135 +} + +// Motor is a single axis/motor/component instance. +type Motor struct { + // 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) error { + if cfg.SerialPath == "" { + return utils.NewConfigValidationFieldRequiredError(path, "serial_path") + } + + return nil +} + +func init() { + controllers = make(map[string]*controller) + + _motor := registry.Component{ + Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + conf, ok := config.ConvertedAttributes.(*Config) + if !ok { + return nil, rdkutils.NewUnexpectedTypeError(conf, config.ConvertedAttributes) + } + return NewMotor(ctx, conf, logger) + }, + } + registry.RegisterComponent(motor.Subtype, modelName, _motor) + + config.RegisterComponentAttributeMapConverter( + motor.SubtypeName, + modelName, + func(attributes config.AttributeMap) (interface{}, error) { + var conf Config + decoder, err := mapstructure.NewDecoder(&mapstructure.DecoderConfig{TagName: "json", Squash: true, Result: &conf}) + if err != nil { + return nil, err + } + if err := decoder.Decode(attributes); err != nil { + return nil, err + } + return &conf, nil + }, + &Config{}, + ) +} + +func newController(c *Config, logger golog.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 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, logger golog.Logger) (motor.LocalMotor, 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{ + c: ctrl, + Channel: c.MotorChannel, + dirFlip: c.DirectionFlip, + minPowerPct: c.MinPowerPct, + maxPowerPct: c.MaxPowerPct, + } + + 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() { + active := m.isAxisActive() + if !active { + return + } + + err := m.Stop(context.Background(), nil) + if err != nil { + m.c.logger.Error(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 + } + } + if m.c.port != nil { + err = m.c.port.Close() + if err != nil { + m.c.logger.Error(fmt.Errorf("error closing serial connection: %w", err)) + } + } + globalMu.Lock() + defer globalMu.Unlock() + delete(controllers, m.c.serialDevice) +} + +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 + 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 err + } + 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 { + return motor.NewZeroRPMError() + } + + powerPct, waitDur := goForMath(m.maxRPM, rpm, revolutions) + err := m.SetPower(ctx, powerPct, extra) + if err != nil { + return err + } + + if revolutions == 0 { + return nil + } + + 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 errors.New("not supported") +} + +// GoTillStop moves a motor until stopped by the controller (due to switch or function) or stopFunc. +func (m *Motor) GoTillStop(ctx context.Context, rpm float64, stopFunc func(ctx context.Context) bool) error { + return motor.NewGoTillStopUnsupportedError(fmt.Sprintf("Channel %d on Sabertooth %d", m.Channel, m.c.address)) +} + +// ResetZeroPosition defines the current position to be zero (+/- offset). +func (m *Motor) ResetZeroPosition(ctx context.Context, offset float64, extra map[string]interface{}) error { + return errors.New("not supported") +} + +// 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 features supported by this motor. +func (m *Motor) Properties(ctx context.Context, extra map[string]interface{}) (map[motor.Feature]bool, error) { + return map[motor.Feature]bool{"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} +} + +// If revolutions is 0, the returned wait duration will be 0 representing that +// the motor should run indefinitely. +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 + } + + if revolutions == 0 { + powerPct := rpm / maxRPM + return powerPct, 0 + } + + dir := rpm * revolutions / math.Abs(revolutions*rpm) + 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 new file mode 100644 index 00000000000..db0aac91ad3 --- /dev/null +++ b/components/motor/dimensionengineering/sabertooth_test.go @@ -0,0 +1,418 @@ +package dimensionengineering_test + +import ( + "bytes" + "context" + "testing" + + "github.com/edaniels/golog" + "go.viam.com/test" + "go.viam.com/utils" + + "go.viam.com/rdk/components/motor" + "go.viam.com/rdk/components/motor/dimensionengineering" + "go.viam.com/rdk/config" + "go.viam.com/rdk/registry" +) + +// var txMu sync.Mutex + +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" +} + +func TestSabertoothMotor(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + resChan := make(chan string, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 128, + DirectionFlip: false, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err, test.ShouldBeNil) + defer utils.TryClose(ctx, m1) + + // 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) + _, ok = motor1.(motor.LocalMotor) + test.That(t, ok, test.ShouldBeTrue) + + t.Run("motor supports position reporting", func(t *testing.T) { + features, err := motor1.Properties(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + }) + + t.Run("motor SetPower testing", func(t *testing.T) { + // Test 0 (aka "stop") + test.That(t, motor1.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) + + // 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 0 (aka "stop") + test.That(t, motor1.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) + }) + + mc2 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 2, + TestChan: c, + SerialAddress: 128, + DirectionFlip: false, + } + + m2, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor2", ConvertedAttributes: &mc2}, logger) + test.That(t, err, test.ShouldBeNil) + defer utils.TryClose(ctx, m2) + + checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) + + motor2, ok := m2.(motor.Motor) + test.That(t, ok, test.ShouldBeTrue) + _, ok = motor2.(motor.LocalMotor) + test.That(t, ok, test.ShouldBeTrue) + + t.Run("motor supports position reporting", func(t *testing.T) { + features, err := motor2.Properties(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + }) + + t.Run("motor SetPower testing", func(t *testing.T) { + // Test 0 (aka "stop") + test.That(t, motor2.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) + + // 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 0 (aka "stop") + test.That(t, motor2.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) + }) +} + +func TestSabertoothMotorDirectionFlip(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + resChan := make(chan string, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 128, + DirectionFlip: true, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err, test.ShouldBeNil) + defer utils.TryClose(ctx, m1) + + checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) + + motor1, ok := m1.(motor.Motor) + test.That(t, ok, test.ShouldBeTrue) + _, ok = motor1.(motor.LocalMotor) + 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, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x01, 0x00, 0x01}) + + // 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 0 (aka "stop") + test.That(t, motor1.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x01, 0x00, 0x01}) + }) + + mc2 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 2, + TestChan: c, + SerialAddress: 128, + DirectionFlip: true, + } + + m2, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor2", ConvertedAttributes: &mc2}, logger) + test.That(t, err, test.ShouldBeNil) + defer utils.TryClose(ctx, m2) + + checkTx(t, resChan, c, []byte{0x80, 0x04, 0x00, 0x04}) + + motor2, ok := m2.(motor.Motor) + test.That(t, ok, test.ShouldBeTrue) + _, ok = motor2.(motor.LocalMotor) + test.That(t, ok, test.ShouldBeTrue) + + t.Run("motor supports position reporting", func(t *testing.T) { + features, err := motor2.Properties(ctx, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, features[motor.PositionReporting], test.ShouldBeFalse) + }) + + t.Run("motor SetPower testing", func(t *testing.T) { + // Test 0 (aka "stop") + test.That(t, motor2.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x05, 0x00, 0x05}) + + // 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 0 (aka "stop") + test.That(t, motor2.SetPower(ctx, 0, nil), test.ShouldBeNil) + checkTx(t, resChan, c, []byte{0x80, 0x05, 0x00, 0x05}) + }) +} + +func TestSabertoothRampConfig(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + resChan := make(chan string, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 128, + RampValue: 100, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err, test.ShouldBeNil) + defer utils.TryClose(ctx, m1) + + checkTx(t, resChan, c, []byte{0x80, 0x00, 0x00, 0x00}) + checkTx(t, resChan, c, []byte{0x80, 0x10, 0x64, 0x74}) + + motor1, ok := m1.(motor.Motor) + test.That(t, ok, test.ShouldBeTrue) + _, ok = motor1.(motor.LocalMotor) + test.That(t, ok, test.ShouldBeTrue) +} + +func TestSabertoothAddressMapping(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + resChan := make(chan string, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 129, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err, test.ShouldBeNil) + defer utils.TryClose(ctx, m1) + + checkTx(t, resChan, c, []byte{0x81, 0x00, 0x00, 0x01}) +} + +func TestInvalidMotorChannel(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 3, + TestChan: c, + SerialAddress: 129, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err.Error(), test.ShouldContainSubstring, "invalid channel") + defer utils.TryClose(ctx, m1) +} + +func TestInvalidBaudRate(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 129, + BaudRate: 1, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err.Error(), test.ShouldContainSubstring, "invalid baud_rate") + defer utils.TryClose(ctx, m1) +} + +func TestInvalidSerialAddress(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 140, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err.Error(), test.ShouldContainSubstring, "invalid address") + defer utils.TryClose(ctx, m1) +} + +func TestInvalidMinPowerPct(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 129, + MinPowerPct: 0.7, + MaxPowerPct: 0.5, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err.Error(), test.ShouldContainSubstring, "invalid min_power_pct") + defer utils.TryClose(ctx, m1) +} + +func TestInvalidMaxPowerPct(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 1, + TestChan: c, + SerialAddress: 129, + MinPowerPct: 0.7, + MaxPowerPct: 1.5, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{Name: "motor1", ConvertedAttributes: &mc1}, logger) + test.That(t, err.Error(), test.ShouldContainSubstring, "invalid max_power_pct") + defer utils.TryClose(ctx, m1) +} + +func TestMultipleInvalidParameters(t *testing.T) { + ctx := context.Background() + logger := golog.NewTestLogger(t) + c := make(chan []byte, 1024) + deps := make(registry.Dependencies) + + mc1 := dimensionengineering.Config{ + SerialPath: "testchan", + MotorChannel: 3, + TestChan: c, + BaudRate: 10, + SerialAddress: 140, + MinPowerPct: 1.7, + MaxPowerPct: 1.5, + } + + motorReg := registry.ComponentLookup(motor.Subtype, "de-sabertooth") + test.That(t, motorReg, test.ShouldNotBeNil) + + // These are the setup register writes + m1, err := motorReg.Constructor(context.Background(), deps, config.Component{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") + defer utils.TryClose(ctx, m1) +} diff --git a/components/motor/gpio/setup.go b/components/motor/gpio/setup.go index eb8b83389ba..e23a8e34938 100644 --- a/components/motor/gpio/setup.go +++ b/components/motor/gpio/setup.go @@ -31,7 +31,7 @@ type PinConfig struct { // Config describes the configuration of a motor. type Config struct { Pins PinConfig `json:"pins"` - BoardName string `json:"board"` // used to get encoders + BoardName string `json:"board"` MinPowerPct float64 `json:"min_power_pct,omitempty"` // min power percentage to allow for this motor default is 0.0 MaxPowerPct float64 `json:"max_power_pct,omitempty"` // max power percentage to allow for this motor (0.06 - 1.0) PWMFreq uint `json:"pwm_freq,omitempty"` diff --git a/components/motor/register/register.go b/components/motor/register/register.go index 61474858989..14de67cf16a 100644 --- a/components/motor/register/register.go +++ b/components/motor/register/register.go @@ -3,6 +3,7 @@ package register import ( // for motors. + _ "go.viam.com/rdk/components/motor/dimensionengineering" _ "go.viam.com/rdk/components/motor/dmc4000" _ "go.viam.com/rdk/components/motor/fake" _ "go.viam.com/rdk/components/motor/gpio"