[RSDK-9603] Move enable pin logic into doCycle to allow SetPower and SetRPM to enable the configured EN pins (viamrobotics#4666)

Author: randhid

.gitignore

@@ -92,3 +92,6 @@ web/cmd/server/*.core
 
 # direnv (optional dev tool)
 .envrc
+
+# codegpt
+.codegpt/*

components/motor/gpiostepper/gpiostepper.go

@@ -85,62 +85,56 @@ func (cfg *Config) Validate(path string) ([]string, error) {
 
 func init() {
 	resource.RegisterComponent(motor.API, model, resource.Registration[motor.Motor, *Config]{
-		Constructor: func(
-			ctx context.Context,
-			deps resource.Dependencies,
-			conf resource.Config,
-			logger logging.Logger,
-		) (motor.Motor, error) {
-			actualBoard, motorConfig, err := getBoardFromRobotConfig(deps, conf)
-			if err != nil {
-				return nil, err
-			}
-
-			return newGPIOStepper(ctx, actualBoard, *motorConfig, conf.ResourceName(), logger)
-		},
-	})
+		Constructor: newGPIOStepper,
+	},
+	)
 }
 
-func getBoardFromRobotConfig(deps resource.Dependencies, conf resource.Config) (board.Board, *Config, error) {
-	motorConfig, err := resource.NativeConfig[*Config](conf)
-	if err != nil {
-		return nil, nil, err
-	}
-	if motorConfig.BoardName == "" {
-		return nil, nil, errors.New("expected board name in config for motor")
+// TODO (rh) refactor this driver so that the enable and direction logic is at the beginning of each API call
+// and the step -> position logic is the only thing being handled by the background thread.
+// right now too many things can be called out of lock, this function is only called from the constructor, CLose
+// the doCycle step routine, and should not be called elsewhere since there's no lock in to ptoect the enable pins.
+func (m *gpioStepper) enable(ctx context.Context, high bool) error {
+	var err error
+	if m.enablePinHigh != nil {
+		err = multierr.Combine(err, m.enablePinHigh.Set(ctx, high, nil))
 	}
-	b, err := board.FromDependencies(deps, motorConfig.BoardName)
-	if err != nil {
-		return nil, nil, err
+
+	if m.enablePinLow != nil {
+		err = multierr.Combine(err, m.enablePinLow.Set(ctx, !high, nil))
 	}
-	return b, motorConfig, nil
+
+	return err
 }
 
 func newGPIOStepper(
 	ctx context.Context,
-	b board.Board,
-	mc Config,
-	name resource.Name,
+	deps resource.Dependencies,
+	conf resource.Config,
 	logger logging.Logger,
 ) (motor.Motor, error) {
-	if b == nil {
-		return nil, errors.New("board is required")
+	mc, err := resource.NativeConfig[*Config](conf)
+	if err != nil {
+		return nil, err
+	}
+
+	b, err := board.FromDependencies(deps, mc.BoardName)
+	if err != nil {
+		return nil, err
 	}
 
 	if mc.TicksPerRotation == 0 {
 		return nil, errors.New("expected ticks_per_rotation in config for motor")
 	}
 
 	m := &gpioStepper{
-		Named:            name.AsNamed(),
+		Named:            conf.ResourceName().AsNamed(),
 		theBoard:         b,
 		stepsPerRotation: mc.TicksPerRotation,
 		logger:           logger,
 		opMgr:            operation.NewSingleOperationManager(),
 	}
 
-	var err error
-
 	// only set enable pins if they exist
 	if mc.Pins.EnablePinHigh != "" {
 		m.enablePinHigh, err = b.GPIOPinByName(mc.Pins.EnablePinHigh)
@@ -208,8 +202,7 @@ type gpioStepper struct {
 // SetPower sets the percentage of power the motor should employ between 0-1.
 func (m *gpioStepper) SetPower(ctx context.Context, powerPct float64, extra map[string]interface{}) error {
 	if math.Abs(powerPct) <= .0001 {
-		m.stop()
-		return nil
+		return m.Stop(ctx, nil)
 	}
 
 	if m.minDelay == 0 {
@@ -219,7 +212,10 @@ func (m *gpioStepper) SetPower(ctx context.Context, powerPct float64, extra map[
 			m.Name().Name)
 	}
 
+	// lock added here to prevent race with doStep
+	m.lock.Lock()
 	m.stepperDelay = time.Duration(float64(m.minDelay) / math.Abs(powerPct))
+	m.lock.Unlock()
 
 	if powerPct < 0 {
 		m.targetStepPosition = math.MinInt64
@@ -288,7 +284,9 @@ func (m *gpioStepper) doCycle(ctx context.Context) (time.Duration, error) {
 func (m *gpioStepper) doStep(ctx context.Context, forward bool) error {
 	err := multierr.Combine(
 		m.dirPin.Set(ctx, forward, nil),
-		m.stepPin.Set(ctx, true, nil))
+		m.stepPin.Set(ctx, true, nil),
+		m.enable(ctx, true),
+	)
 	if err != nil {
 		return err
 	}
@@ -319,13 +317,7 @@ func (m *gpioStepper) GoFor(ctx context.Context, rpm, revolutions float64, extra
 	ctx, done := m.opMgr.New(ctx)
 	defer done()
 
-	err := m.enable(ctx, true)
-	if err != nil {
-		return errors.Wrapf(err, "error enabling motor in GoFor from motor (%s)", m.Name().Name)
-	}
-
-	err = m.goForInternal(ctx, rpm, revolutions)
-	if err != nil {
+	if err := m.goForInternal(ctx, rpm, revolutions); err != nil {
 		return multierr.Combine(
 			m.enable(ctx, false),
 			errors.Wrapf(err, "error in GoFor from motor (%s)", m.Name().Name))
@@ -406,8 +398,7 @@ func (m *gpioStepper) SetRPM(ctx context.Context, rpm float64, extra map[string]
 	defer m.lock.Unlock()
 
 	if math.Abs(rpm) <= .0001 {
-		m.stop()
-		return nil
+		return m.Stop(ctx, nil)
 	}
 
 	// calculate delay between steps for the thread in the goroutine that we started in component creation.
@@ -488,19 +479,6 @@ func (m *gpioStepper) IsPowered(ctx context.Context, extra map[string]interface{
 	return on, percent, err
 }
 
-func (m *gpioStepper) enable(ctx context.Context, on bool) error {
-	var err error
-	if m.enablePinHigh != nil {
-		err = multierr.Combine(err, m.enablePinHigh.Set(ctx, on, nil))
-	}
-
-	if m.enablePinLow != nil {
-		err = multierr.Combine(err, m.enablePinLow.Set(ctx, !on, nil))
-	}
-
-	return err
-}
-
 func (m *gpioStepper) Close(ctx context.Context) error {
 	err := m.Stop(ctx, nil)