[RSDK-8486] Fix 28byj-48 control issues

components/motor/motor.go

@@ -218,3 +218,22 @@ func GetRequestedDirection(rpm, revolutions float64) float64 {
 	}
 	return dir
 }
+
+// GetSign returns the sign of the float as a helper for getting
+// the intended direction of travel of a motor.
+func GetSign(x float64) float64 {
+	if x == 0 {
+		return 0
+	}
+	if math.Signbit(x) {
+		return -1.0
+	}
+	return 1.0
+}
+
+// ClampPower clamps a percentage power to 1.0 or -1.0.
+func ClampPower(pwr float64) float64 {
+	pwr = math.Min(pwr, 1.0)
+	pwr = math.Max(pwr, -1.0)
+	return pwr
+}

components/motor/ulnstepper/28byj-48.go

@@ -293,8 +293,13 @@ func (m *uln28byj) GoFor(ctx context.Context, rpm, revolutions float64, extra ma
 		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)
 		}
-		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)
@@ -372,12 +377,15 @@ func (m *uln28byj) SetRPM(ctx context.Context, rpm float64, extra map[string]int
 
 // 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))
 	if err := m.Stop(ctx, extra); err != nil {
 		return err
 	}
 	m.lock.Lock()
 	defer m.lock.Unlock()
-	m.stepPosition = int64(-1 * offset * float64(m.ticksPerRotation))
+	m.stepPosition = newPosition
+	m.targetStepPosition = newPosition
+	// use Stop to set the target position to the current position again
 	return nil
 }
 
@@ -391,13 +399,16 @@ func (m *uln28byj) SetPower(ctx context.Context, powerPct float64, extra map[str
 		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)
 		}
-		return m.Stop(ctx, extra)
 	}
 
 	m.lock.Lock()
 	defer m.lock.Unlock()
-	m.targetStepPosition = int64(math.Inf(int(powerPct)))
+	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()
@@ -432,6 +443,9 @@ 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
 }
 

components/motor/ulnstepper/28byj-48_test.go

@@ -268,7 +268,7 @@ func TestFunctions(t *testing.T) {
 		err = m.GoFor(ctx, 146, 1, nil)
 		test.That(t, err, test.ShouldBeNil)
 		allObs = obs.All()
-		latestLoggedEntry = allObs[len(allObs)-1]
+		latestLoggedEntry = allObs[len(allObs)-2]
 		test.That(t, fmt.Sprint(latestLoggedEntry), test.ShouldContainSubstring, "nearly the max")
 	})