From 4416021553691fefbc1bb741340dd0bdee5ea374 Mon Sep 17 00:00:00 2001 From: Troye Stonich Date: Sun, 26 Apr 2026 16:33:02 -0400 Subject: [PATCH 1/5] initial check in --- .github/copilot-instructions.md | 2 +- .../Config/SynchronizedDualAxisConfig.cs | 11 +- MotorController/Docs/spec.md | 12 +- .../ISynchronizedDualAxisController.cs | 19 + .../SynchronizedDualAxisController.cs | 666 ++++++++++++++++++ Test/StepperMotorControllerTests.cs | 450 ++++++++++++ UI/Program.cs | 2 +- 7 files changed, 1148 insertions(+), 14 deletions(-) create mode 100644 MotorController/ISynchronizedDualAxisController.cs create mode 100644 MotorController/SynchronizedDualAxisController.cs diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index 0f5db5d..915ec6f 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -74,7 +74,7 @@ The UI application automatically selects: - `specs.md` - Detailed specification document (read this for requirements) - `IStepperMotorController.cs` - Public interface for motor controller (~75 lines with XML docs) - `StepperMotorController.cs` - Main controller with David Austin algorithm (~492 lines) -- `ControllerConfig.cs` - Configuration settings class with XML documentation (~64 lines) +- `LinearAxixConfig.cs` - Configuration settings class with XML documentation (~64 lines) - Includes `StepsPerRevolution` enum with values: 200, 400, 800, 1600, 3200, 6400, 12800 - `LimitSwitch.cs` - Enum specifying limit switch direction (Min/Max) (~18 lines) - `GpioControllerWrapper.cs` - Contains: diff --git a/MotorController/Config/SynchronizedDualAxisConfig.cs b/MotorController/Config/SynchronizedDualAxisConfig.cs index 57c12f8..40950fa 100644 --- a/MotorController/Config/SynchronizedDualAxisConfig.cs +++ b/MotorController/Config/SynchronizedDualAxisConfig.cs @@ -18,15 +18,10 @@ public class SynchronizedDualAxisConfig public RotaryAxisConfig RotaryAxisConfig { get; set; } = new RotaryAxisConfig(); /// - /// Gets or sets the gear ratio between the rotary axis and the linear axis. - /// This represents the number of rotations of the rotary axis per inch of linear travel. + /// Gets or sets the ratio between the rotary axis and the linear axis. + /// This represents the number of rotations of the rotary axis per rotation of the linear axis. /// - /// - /// For example, if the rotary axis should make 2 full rotations per inch of linear movement, - /// set this value to 2.0. The controller will automatically synchronize the rotary motor's - /// step timing to maintain this ratio throughout acceleration, constant speed, and deceleration phases. - /// - public double GearRatio { get; set; } = 2.0; + public double GearRatio { get; set; } = 0.4; } diff --git a/MotorController/Docs/spec.md b/MotorController/Docs/spec.md index 56a5b17..e519fba 100644 --- a/MotorController/Docs/spec.md +++ b/MotorController/Docs/spec.md @@ -1,4 +1,4 @@ -Currently there is a rotary axis that is driven by gears off the lead screw. +Currently there is a rotary axis that is driven by gears off the lead screw from the linear axis that is controlled by the StepperMotorController. It rotates relative to the rotation of the lead screw. @@ -6,10 +6,14 @@ I want to change this and drive the rotary axis using a stepper motor so that I Create a class that incorporates this new stepper motor, as if the rotary axis was driven by gears. -The linear movoment must be syncronized with the rotary movement with no drift. +The rotary movoment must be syncronized with the linear movement with no drift. -This should be done by scaling the delay of the rotary axis by the gear ratio. +This should be done by scaling the delay of the rotary axis by the gear ratio. + +Combine the 2 stepper motor pulse generation similar to how LinuxCNC has a stepgen thread that generates pulses for both the linear and rotary axis. Since the new stepper only turns it won't have a min and max limit switch. -The rotary axis should automatically follow the linear axis's acceleration profile through the synchronization mechanism. \ No newline at end of file +The rotary axis should automatically follow the linear axis's acceleration profile through the synchronization mechanism. + +There is already a class for the RotaryAxisConfig, and SynchronizedDualAxisConfig which combines the linear axis configuration and the rotary axis configuration with the gear ratio. \ No newline at end of file diff --git a/MotorController/ISynchronizedDualAxisController.cs b/MotorController/ISynchronizedDualAxisController.cs new file mode 100644 index 0000000..d39734b --- /dev/null +++ b/MotorController/ISynchronizedDualAxisController.cs @@ -0,0 +1,19 @@ +namespace MotorControllerApp; + +/// +/// Extends with rotary axis position feedback for a +/// synchronized dual-axis controller that drives a linear axis and a rotary axis simultaneously. +/// +/// +/// The rotary axis follows the linear axis acceleration profile automatically through the +/// synchronization mechanism. Its speed is derived by scaling the linear axis step delay by +/// the configured gear ratio, with a DDA (Digital Differential Analyzer) accumulator ensuring +/// zero long-term drift between the two axes. +/// +public interface ISynchronizedDualAxisController : IStepperMotorController +{ + /// + /// Gets the current angular position of the rotary axis in degrees. + /// + double CurrentRotaryPositionDegrees { get; } +} diff --git a/MotorController/SynchronizedDualAxisController.cs b/MotorController/SynchronizedDualAxisController.cs new file mode 100644 index 0000000..eb93029 --- /dev/null +++ b/MotorController/SynchronizedDualAxisController.cs @@ -0,0 +1,666 @@ +using System.Device.Gpio; +using System.Diagnostics; +using Microsoft.Extensions.Logging; + +namespace MotorControllerApp; + +/// +/// Controls two stepper motors — a linear axis and a synchronized rotary axis — in a single +/// step-generation thread, analogous to LinuxCNC's stepgen thread. +/// +/// +/// +/// The rotary axis is treated as if it were geared to the linear axis lead screw. +/// A DDA (Digital Differential Analyzer) accumulator advances the rotary motor by exactly +/// GearRatio × (RotarySPR / LinearSPR) rotary steps per linear step, with sub-step +/// residual carried forward so there is zero long-term drift. +/// +/// +/// Both motors share the same David Austin delay value at every iteration; the rotary axis +/// inherits the linear axis's acceleration and deceleration profile automatically. +/// Rotary pulses are fired within the same step period using stopwatch-anchored timing so the +/// linear pulse timing is never disturbed. +/// +/// +/// The rotary axis does not have limit switches because it rotates continuously. +/// +/// +public class SynchronizedDualAxisController : ISynchronizedDualAxisController +{ + private readonly IGpioController _gpio; + private readonly SynchronizedDualAxisConfig _config; + private readonly ILogger _logger; + + // --- linear axis state --- + private readonly SemaphoreSlim _positionLock = new(1, 1); + private readonly SemaphoreSlim _motionLock = new(1, 1); + private CancellationTokenSource _stopTokenSource = new(); + private double _currentLinearPositionSteps; + private bool _disposed; + private volatile bool _stopRequested; + private double _targetRpm; + + // --- rotary axis state --- + private readonly SemaphoreSlim _rotaryPositionLock = new(1, 1); + private double _currentRotaryPositionSteps; + + // ----------------------------------------------------------------------- + // Properties + // ----------------------------------------------------------------------- + + /// + public double CurrentPositionInches + { + get + { + _positionLock.Wait(); + try + { + var linear = _config.LinearAxisConfig; + return _currentLinearPositionSteps / (int)linear.StepsPerRevolution / linear.LeadScrewThreadsPerInch; + } + finally + { + _positionLock.Release(); + } + } + } + + /// + public double CurrentRotaryPositionDegrees + { + get + { + _rotaryPositionLock.Wait(); + try + { + return _currentRotaryPositionSteps / (int)_config.RotaryAxisConfig.StepsPerRevolution * 360.0; + } + finally + { + _rotaryPositionLock.Release(); + } + } + } + + /// + public bool IsMinLimitSwitchTriggered { get; private set; } + + /// + public bool IsMaxLimitSwitchTriggered { get; private set; } + + /// + public event EventHandler? MinLimitSwitchTriggered; + + /// + public event EventHandler? MaxLimitSwitchTriggered; + + // ----------------------------------------------------------------------- + // Constructor + // ----------------------------------------------------------------------- + + /// + /// Initializes a new instance of . + /// + /// GPIO controller (real or fake for development). + /// Synchronized dual-axis configuration. + /// Logger instance. + public SynchronizedDualAxisController( + IGpioController gpio, + SynchronizedDualAxisConfig config, + ILogger logger) + { + _gpio = gpio ?? throw new ArgumentNullException(nameof(gpio)); + _config = config ?? throw new ArgumentNullException(nameof(config)); + _logger = logger ?? throw new ArgumentNullException(nameof(logger)); + + var lin = _config.LinearAxisConfig; + var rot = _config.RotaryAxisConfig; + + _logger.LogInformation( + "Initializing SynchronizedDualAxisController | " + + "Linear: PulsePin={LP}, DirPin={LD}, EnablePin={LE}, MinLimit={Mn}, MaxLimit={Mx}, SPR={LSPR}, TPI={TPI}, Accel={Accel} | " + + "Rotary: PulsePin={RP}, DirPin={RD}, EnablePin={RE}, SPR={RSPR} | GearRatio={GR}", + lin.PulsePin, lin.DirectionPin, lin.EnablePin, lin.MinLimitSwitchPin, lin.MaxLimitSwitchPin, + lin.StepsPerRevolution, lin.LeadScrewThreadsPerInch, lin.Acceleration, + rot.PulsePin, rot.DirectionPin, rot.EnablePin, rot.StepsPerRevolution, + _config.GearRatio); + + InitializePins(); + } + + // ----------------------------------------------------------------------- + // GPIO initialisation + // ----------------------------------------------------------------------- + + private void InitializePins() + { + var lin = _config.LinearAxisConfig; + var rot = _config.RotaryAxisConfig; + + // ---- linear axis ---- + CloseIfOpen(lin.PulsePin); + CloseIfOpen(lin.DirectionPin); + if (lin.EnablePin.HasValue) CloseIfOpen(lin.EnablePin.Value); + CloseIfOpen(lin.MinLimitSwitchPin); + CloseIfOpen(lin.MaxLimitSwitchPin); + + _gpio.OpenPin(lin.PulsePin, PinMode.Output); + _gpio.OpenPin(lin.DirectionPin, PinMode.Output); + + if (lin.EnablePin.HasValue) + { + _gpio.OpenPin(lin.EnablePin.Value, PinMode.Output); + _gpio.Write(lin.EnablePin.Value, PinValue.High); // Disabled by default + } + + _gpio.OpenPin(lin.MinLimitSwitchPin, PinMode.Input); + _gpio.OpenPin(lin.MaxLimitSwitchPin, PinMode.Input); + + _gpio.RegisterCallbackForPinValueChangedEvent(lin.MinLimitSwitchPin, PinEventTypes.Falling | PinEventTypes.Rising, OnMinLimitSwitchChanged); + _gpio.RegisterCallbackForPinValueChangedEvent(lin.MaxLimitSwitchPin, PinEventTypes.Falling | PinEventTypes.Rising, OnMaxLimitSwitchChanged); + + IsMinLimitSwitchTriggered = _gpio.Read(lin.MinLimitSwitchPin) == PinValue.Low; + IsMaxLimitSwitchTriggered = _gpio.Read(lin.MaxLimitSwitchPin) == PinValue.Low; + + // ---- rotary axis ---- + CloseIfOpen(rot.PulsePin); + CloseIfOpen(rot.DirectionPin); + if (rot.EnablePin.HasValue) CloseIfOpen(rot.EnablePin.Value); + + _gpio.OpenPin(rot.PulsePin, PinMode.Output); + _gpio.OpenPin(rot.DirectionPin, PinMode.Output); + + if (rot.EnablePin.HasValue) + { + _gpio.OpenPin(rot.EnablePin.Value, PinMode.Output); + _gpio.Write(rot.EnablePin.Value, PinValue.High); // Disabled by default + } + } + + private void CloseIfOpen(int pin) + { + if (_gpio.IsPinOpen(pin)) _gpio.ClosePin(pin); + } + + // ----------------------------------------------------------------------- + // Limit switch callbacks + // ----------------------------------------------------------------------- + + private void OnMinLimitSwitchChanged(object sender, PinValueChangedEventArgs e) + { + IsMinLimitSwitchTriggered = e.ChangeType == PinEventTypes.Falling; + _logger.LogInformation("Min limit switch {Status} (Pin {Pin})", + IsMinLimitSwitchTriggered ? "triggered" : "released", + _config.LinearAxisConfig.MinLimitSwitchPin); + MinLimitSwitchTriggered?.Invoke(this, EventArgs.Empty); + } + + private void OnMaxLimitSwitchChanged(object sender, PinValueChangedEventArgs e) + { + IsMaxLimitSwitchTriggered = e.ChangeType == PinEventTypes.Falling; + _logger.LogInformation("Max limit switch {Status} (Pin {Pin})", + IsMaxLimitSwitchTriggered ? "triggered" : "released", + _config.LinearAxisConfig.MaxLimitSwitchPin); + MaxLimitSwitchTriggered?.Invoke(this, EventArgs.Empty); + } + + // ----------------------------------------------------------------------- + // Public API + // ----------------------------------------------------------------------- + + /// + public async Task MoveInchesAsync(double inches, double rpm, CancellationToken cancellationToken = default) + { + if (rpm <= 0) + throw new ArgumentException("RPM must be greater than zero", nameof(rpm)); + + await _motionLock.WaitAsync(cancellationToken); + try + { + _stopRequested = false; + + var lin = _config.LinearAxisConfig; + var totalSteps = (int)(inches * lin.LeadScrewThreadsPerInch * (int)lin.StepsPerRevolution); + bool forward = totalSteps >= 0; + + SetLinearDirection(forward); + SetRotaryDirection(forward); + EnableMotors(); + + try + { + await ExecuteMotionAsync(Math.Abs(totalSteps), rpm, forward, cancellationToken); + } + catch (OperationCanceledException) + { + // Expected when motion is cancelled + } + finally + { + DisableMotors(); + } + } + finally + { + _motionLock.Release(); + } + } + + /// + public async Task RunToLimitSwitchAsync(LimitSwitch direction, double rpm, CancellationToken cancellationToken = default) + { + if (rpm <= 0) + throw new ArgumentException("RPM must be greater than zero", nameof(rpm)); + + await _motionLock.WaitAsync(cancellationToken); + try + { + _stopRequested = false; + + bool toMax = direction == LimitSwitch.Max; + SetLinearDirection(toMax); + SetRotaryDirection(toMax); + EnableMotors(); + + try + { + await ExecuteMotionInternalAsync( + direction: toMax, + initialRpm: rpm, + maxSteps: null, + shouldStartDeceleration: (step, accelSteps) => + { + if (step < accelSteps) return false; + return toMax ? IsMaxLimitSwitchTriggered : IsMinLimitSwitchTriggered; + }, + maxDecelerationSteps: 300, + cancellationToken: cancellationToken); + } + catch (OperationCanceledException) + { + // Expected when stopped + } + finally + { + DisableMotors(); + } + } + finally + { + _motionLock.Release(); + } + } + + /// + public async Task StopAsync() + { + _stopRequested = true; + await Task.CompletedTask; + } + + /// + public void SetTargetSpeed(double rpm) + { + if (rpm <= 0) + { + _logger.LogWarning("SetTargetSpeed called with invalid RPM value: {Rpm}. Ignoring.", rpm); + return; + } + + Interlocked.Exchange(ref _targetRpm, rpm); + _logger.LogInformation("Target RPM updated to {Rpm}", rpm); + } + + /// + public async Task ResetPositionAsync() + { + await _positionLock.WaitAsync(); + try + { + _currentLinearPositionSteps = 0; + } + finally + { + _positionLock.Release(); + } + } + + // ----------------------------------------------------------------------- + // Enable / direction helpers + // ----------------------------------------------------------------------- + + private void SetLinearDirection(bool forward) + { + _gpio.Write(_config.LinearAxisConfig.DirectionPin, forward ? PinValue.Low : PinValue.High); + } + + private void SetRotaryDirection(bool forward) + { + _gpio.Write(_config.RotaryAxisConfig.DirectionPin, forward ? PinValue.Low : PinValue.High); + } + + private void EnableMotors() + { + var lin = _config.LinearAxisConfig; + var rot = _config.RotaryAxisConfig; + + if (lin.EnablePin.HasValue) + _gpio.Write(lin.EnablePin.Value, PinValue.Low); + + if (rot.EnablePin.HasValue) + _gpio.Write(rot.EnablePin.Value, PinValue.Low); + } + + private void DisableMotors() + { + var lin = _config.LinearAxisConfig; + var rot = _config.RotaryAxisConfig; + + if (lin.EnablePin.HasValue) + _gpio.Write(lin.EnablePin.Value, PinValue.High); + + if (rot.EnablePin.HasValue) + _gpio.Write(rot.EnablePin.Value, PinValue.High); + } + + // ----------------------------------------------------------------------- + // Motion execution + // ----------------------------------------------------------------------- + + /// + /// Executes a fixed-step motion sequence with full acceleration / deceleration profile. + /// + internal Task ExecuteMotionAsync(int steps, double rpm, bool direction, CancellationToken cancellationToken) + { + return ExecuteMotionInternalAsync( + direction: direction, + initialRpm: rpm, + maxSteps: steps, + shouldStartDeceleration: (_, __) => false, + maxDecelerationSteps: null, + cancellationToken: cancellationToken); + } + + /// + /// Unified motion execution engine that drives both the linear and rotary steppers in a single + /// step-generation loop. + /// + /// + /// The rotary axis uses a DDA accumulator to fire the correct number of pulses per linear step + /// without any long-term drift. The per-step delay is identical for both axes; rotary pulses are + /// inserted within the step period using stopwatch-anchored timing so the linear step timing is + /// never disturbed. + /// + private Task ExecuteMotionInternalAsync( + bool direction, + double initialRpm, + int? maxSteps, + Func shouldStartDeceleration, + int? maxDecelerationSteps, + CancellationToken cancellationToken) + { + Interlocked.Exchange(ref _targetRpm, initialRpm); + + return Task.Run(async () => + { + using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _stopTokenSource.Token); + + var lin = _config.LinearAxisConfig; + var rot = _config.RotaryAxisConfig; + + var currentRpm = Interlocked.CompareExchange(ref _targetRpm, 0, 0); + var maxLinStepsPerSec = (currentRpm * (int)lin.StepsPerRevolution) / 60.0; + var targetDelayMicroseconds = 1_000_000.0 / maxLinStepsPerSec; + + // David Austin initial delay: c0 = 0.676 * sqrt(2/α) * 10^6 + var initialDelayMicroseconds = 0.676 * Math.Sqrt(2.0 / lin.Acceleration) * 1_000_000.0; + + var accelerationSteps = (int)((maxLinStepsPerSec * maxLinStepsPerSec) / (2.0 * lin.Acceleration)); + var decelerationSteps = accelerationSteps; + + if (maxDecelerationSteps.HasValue) + decelerationSteps = Math.Min(decelerationSteps, maxDecelerationSteps.Value); + + if (maxSteps.HasValue && accelerationSteps + decelerationSteps > maxSteps.Value) + { + accelerationSteps = maxSteps.Value / 2; + decelerationSteps = maxSteps.Value - accelerationSteps; + } + + // DDA: rotary steps per linear step = gearRatio × rotarySPR / linearSPR + // Using an accumulator so the sub-step fraction is never lost → zero drift. + double rotaryStepsPerLinearStep = _config.GearRatio + * (int)rot.StepsPerRevolution + / (double)(int)lin.StepsPerRevolution; + + if (maxSteps.HasValue) + { + _logger.LogDebug("Synchronized motion | LinearSteps={Steps}, MaxLinStepsPerSec={MaxSPS:n1}, " + + "AccelSteps={Accel}, DecelSteps={Decel}, InitDelay={Init:n1}µs, TargetDelay={Target:n1}µs, " + + "RotaryStepsPerLinearStep={Ratio:n4}", + maxSteps.Value, maxLinStepsPerSec, accelerationSteps, decelerationSteps, + initialDelayMicroseconds, targetDelayMicroseconds, rotaryStepsPerLinearStep); + } + + double delayMicroseconds = initialDelayMicroseconds; + double rotaryAccumulator = 0.0; + bool stopRequested = false; + bool externalDecelTrigger = false; + int decelStepCounter = 0; + int step = 0; + + while (true) + { + // Hard cancellation — emergency stop, throw immediately without deceleration + if (linkedCts.Token.IsCancellationRequested) + linkedCts.Token.ThrowIfCancellationRequested(); + + // Completed all requested steps? + if (maxSteps.HasValue && step >= maxSteps.Value) + break; + + // Dynamic speed update while in constant-speed phase + if (!externalDecelTrigger && !stopRequested && step >= accelerationSteps) + { + var latestRpm = Interlocked.CompareExchange(ref _targetRpm, 0, 0); + if (Math.Abs(currentRpm - latestRpm) > 0.01) + { + currentRpm = latestRpm; + maxLinStepsPerSec = (currentRpm * (int)lin.StepsPerRevolution) / 60.0; + targetDelayMicroseconds = 1_000_000.0 / maxLinStepsPerSec; + + var newDecelSteps = (int)((maxLinStepsPerSec * maxLinStepsPerSec) / (2.0 * lin.Acceleration)); + if (maxDecelerationSteps.HasValue) + newDecelSteps = Math.Min(newDecelSteps, maxDecelerationSteps.Value); + + if (maxSteps.HasValue) + { + var remaining = maxSteps.Value - step; + decelerationSteps = newDecelSteps < remaining ? newDecelSteps : Math.Max(1, remaining / 2); + _logger.LogInformation("Speed changed to {Rpm} RPM (step {Step}/{Total}, decel={Decel})", + currentRpm, step, maxSteps.Value, decelerationSteps); + } + else + { + decelerationSteps = newDecelSteps; + _logger.LogInformation("Speed changed to {Rpm} RPM (decel={Decel})", currentRpm, decelerationSteps); + } + } + } + + // Graceful stop requested via StopAsync() + if (!stopRequested && _stopRequested) + { + stopRequested = true; + + if (step < accelerationSteps) + { + decelerationSteps = Math.Max(1, step); + _logger.LogInformation("Stop during acceleration at step {Step} → decel for {Decel} steps.", step, decelerationSteps); + } + else + { + _logger.LogInformation("Stop during constant speed → decel for {Decel} steps.", decelerationSteps); + } + } + + // External trigger (e.g. limit switch) + if (!externalDecelTrigger && !stopRequested && shouldStartDeceleration(step, accelerationSteps)) + externalDecelTrigger = true; + + // ---- compute delay for this step ---- + if (externalDecelTrigger || stopRequested) + { + if (decelStepCounter >= decelerationSteps) + break; + + int decelStep = decelerationSteps - decelStepCounter; + if (decelStep > 0) + delayMicroseconds += (2.0 * delayMicroseconds) / ((4.0 * decelStep) - 1.0); + + decelStepCounter++; + } + else if (step < accelerationSteps) + { + // Acceleration — David Austin: cn = cn-1 - (2·cn-1)/(4n+1) + if (step > 0) + delayMicroseconds -= (2.0 * delayMicroseconds) / ((4.0 * step) + 1.0); + } + else + { + // Constant speed / planned deceleration for fixed-step moves + if (maxSteps.HasValue && step >= maxSteps.Value - decelerationSteps) + { + int decelStep = maxSteps.Value - step; + if (decelStep > 0) + delayMicroseconds += (2.0 * delayMicroseconds) / ((4.0 * decelStep) - 1.0); + } + else + { + delayMicroseconds = targetDelayMicroseconds; + } + } + + _logger.LogDebug("Step {Step}: delay={Delay:n1}µs, rotaryAcc={Acc:n4}", step, delayMicroseconds, rotaryAccumulator); + + // ---- fire the linear pulse and any rotary pulses within one step period ---- + FireStepPeriod((int)delayMicroseconds, ref rotaryAccumulator, rotaryStepsPerLinearStep, direction); + + // ---- update positions (thread-safe) ---- + await _positionLock.WaitAsync(linkedCts.Token); + try + { + _currentLinearPositionSteps += direction ? 1 : -1; + } + finally + { + _positionLock.Release(); + } + + step++; + } + + _stopRequested = false; + }); + } + + /// + /// Executes one linear step pulse and all rotary step pulses that fall within its period. + /// + /// + /// The entire period of is consumed here using + /// stopwatch-anchored timing so the outer loop always advances at the correct rate. + /// Rotary pulses are spaced evenly inside the period. + /// + private void FireStepPeriod(int totalPeriodMicroseconds, ref double rotaryAccumulator, double rotaryStepsPerLinearStep, bool direction) + { + var rot = _config.RotaryAxisConfig; + + // Advance DDA accumulator + rotaryAccumulator += rotaryStepsPerLinearStep; + int rotaryPulsesThisStep = (int)rotaryAccumulator; + rotaryAccumulator -= rotaryPulsesThisStep; // keep fractional remainder + + // Update rotary position counter (best-effort; not awaited here to keep timing tight) + if (rotaryPulsesThisStep > 0) + { + _rotaryPositionLock.Wait(); + try + { + _currentRotaryPositionSteps += direction ? rotaryPulsesThisStep : -rotaryPulsesThisStep; + } + finally + { + _rotaryPositionLock.Release(); + } + } + + long freq = Stopwatch.Frequency; + long periodTicks = (long)(totalPeriodMicroseconds * (freq / 1_000_000.0)); + + if (rotaryPulsesThisStep == 0) + { + // Simple case — just fire the linear pulse and consume the full period + long start = Stopwatch.GetTimestamp(); + + _gpio.Write(_config.LinearAxisConfig.PulsePin, PinValue.High); + SpinUntil(start, periodTicks / 2); + + _gpio.Write(_config.LinearAxisConfig.PulsePin, PinValue.Low); + SpinUntil(start, periodTicks); + } + else + { + // Interleaved case — divide period into (rotaryPulsesThisStep + 1) sub-slots. + // Linear pulse fires at the start; rotary pulses are evenly spread within the window. + long slotTicks = periodTicks / (rotaryPulsesThisStep + 1); + long start = Stopwatch.GetTimestamp(); + + // Linear HIGH for first half-slot + _gpio.Write(_config.LinearAxisConfig.PulsePin, PinValue.High); + SpinUntil(start, slotTicks / 2); + + _gpio.Write(_config.LinearAxisConfig.PulsePin, PinValue.Low); + SpinUntil(start, slotTicks); + + // Rotary pulses fill the remaining slots + for (int r = 0; r < rotaryPulsesThisStep; r++) + { + long slotStart = start + slotTicks * (r + 1); + long halfSlot = slotTicks / 2; + + _gpio.Write(rot.PulsePin, PinValue.High); + SpinUntil(slotStart, halfSlot); + + _gpio.Write(rot.PulsePin, PinValue.Low); + SpinUntil(slotStart, slotTicks); + } + } + } + + /// Busy-waits until have passed since . + private static void SpinUntil(long origin, long elapsedTicks) + { + while (Stopwatch.GetTimestamp() - origin < elapsedTicks) { } + } + + // ----------------------------------------------------------------------- + // Dispose + // ----------------------------------------------------------------------- + + /// + public void Dispose() + { + if (_disposed) return; + + _stopTokenSource?.Cancel(); + _stopTokenSource?.Dispose(); + _positionLock?.Dispose(); + _rotaryPositionLock?.Dispose(); + _motionLock?.Dispose(); + _gpio?.Dispose(); + + _disposed = true; + } +} diff --git a/Test/StepperMotorControllerTests.cs b/Test/StepperMotorControllerTests.cs index d4d5317..57a7921 100644 --- a/Test/StepperMotorControllerTests.cs +++ b/Test/StepperMotorControllerTests.cs @@ -1217,3 +1217,453 @@ public async Task SetTargetRpm_ShouldHandleMultipleSpeedChanges() #endregion } + +public class SynchronizedDualAxisControllerTests : IDisposable +{ + private readonly IGpioController _mockGpio; + private readonly SynchronizedDualAxisConfig _config; + private readonly ILogger _mockLogger; + private readonly SynchronizedDualAxisController _controller; + + public SynchronizedDualAxisControllerTests() + { + _mockGpio = Substitute.For(); + _mockLogger = Substitute.For>(); + + _config = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + }, + RotaryAxisConfig = new RotaryAxisConfig + { + PulsePin = 16, + DirectionPin = 12, + StepsPerRevolution = StepsPerRevolution.SPR_400 + }, + GearRatio = 1.0 // 1:1 simplifies pulse-count assertions + }; + + _mockGpio.Read(_config.LinearAxisConfig.MinLimitSwitchPin).Returns(PinValue.High); + _mockGpio.Read(_config.LinearAxisConfig.MaxLimitSwitchPin).Returns(PinValue.High); + + _controller = new SynchronizedDualAxisController(_mockGpio, _config, _mockLogger); + } + + public void Dispose() => _controller?.Dispose(); + + // ----------------------------------------------------------------------- + // Constructor + // ----------------------------------------------------------------------- + + [Fact] + public void Constructor_ShouldThrowArgumentNullException_WhenGpioIsNull() + { + Assert.Throws(() => + new SynchronizedDualAxisController(null!, _config, _mockLogger)); + } + + [Fact] + public void Constructor_ShouldThrowArgumentNullException_WhenConfigIsNull() + { + Assert.Throws(() => + new SynchronizedDualAxisController(_mockGpio, null!, _mockLogger)); + } + + [Fact] + public void Constructor_ShouldOpenLinearAxisPins() + { + var lin = _config.LinearAxisConfig; + _mockGpio.Received(1).OpenPin(lin.PulsePin, PinMode.Output); + _mockGpio.Received(1).OpenPin(lin.DirectionPin, PinMode.Output); + _mockGpio.Received(1).OpenPin(lin.MinLimitSwitchPin, PinMode.Input); + _mockGpio.Received(1).OpenPin(lin.MaxLimitSwitchPin, PinMode.Input); + } + + [Fact] + public void Constructor_ShouldOpenRotaryAxisPins() + { + var rot = _config.RotaryAxisConfig; + _mockGpio.Received(1).OpenPin(rot.PulsePin, PinMode.Output); + _mockGpio.Received(1).OpenPin(rot.DirectionPin, PinMode.Output); + } + + [Fact] + public void Constructor_ShouldRegisterLimitSwitchCallbacks() + { + var lin = _config.LinearAxisConfig; + _mockGpio.Received(1).RegisterCallbackForPinValueChangedEvent( + lin.MinLimitSwitchPin, + PinEventTypes.Falling | PinEventTypes.Rising, + Arg.Any()); + _mockGpio.Received(1).RegisterCallbackForPinValueChangedEvent( + lin.MaxLimitSwitchPin, + PinEventTypes.Falling | PinEventTypes.Rising, + Arg.Any()); + } + + [Fact] + public void Constructor_ShouldOpenEnablePins_WhenConfigured() + { + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + + var cfg = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, DirectionPin = 20, EnablePin = 19, + MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23 + }, + RotaryAxisConfig = new RotaryAxisConfig + { + PulsePin = 16, DirectionPin = 12, EnablePin = 13 + }, + GearRatio = 1.0 + }; + + using var c = new SynchronizedDualAxisController(mockGpio, cfg, _mockLogger); + + mockGpio.Received(1).OpenPin(19, PinMode.Output); + mockGpio.Received(1).Write(19, PinValue.High); // Disabled by default + mockGpio.Received(1).OpenPin(13, PinMode.Output); + mockGpio.Received(1).Write(13, PinValue.High); + } + + // ----------------------------------------------------------------------- + // Initial property values + // ----------------------------------------------------------------------- + + [Fact] + public void CurrentPositionInches_ShouldReturnZero_Initially() + { + Assert.Equal(0.0, _controller.CurrentPositionInches); + } + + [Fact] + public void CurrentRotaryPositionDegrees_ShouldReturnZero_Initially() + { + Assert.Equal(0.0, _controller.CurrentRotaryPositionDegrees); + } + + [Fact] + public void IsMinLimitSwitchTriggered_ShouldBeFalse_Initially() + { + Assert.False(_controller.IsMinLimitSwitchTriggered); + } + + [Fact] + public void IsMaxLimitSwitchTriggered_ShouldBeFalse_Initially() + { + Assert.False(_controller.IsMaxLimitSwitchTriggered); + } + + // ----------------------------------------------------------------------- + // MoveInchesAsync + // ----------------------------------------------------------------------- + + [Fact] + public async Task MoveInchesAsync_ShouldThrowArgumentException_WhenRpmIsZero() + { + await Assert.ThrowsAsync(() => _controller.MoveInchesAsync(1.0, 0)); + } + + [Fact] + public async Task MoveInchesAsync_ShouldThrowArgumentException_WhenRpmIsNegative() + { + await Assert.ThrowsAsync(() => _controller.MoveInchesAsync(1.0, -1)); + } + + [Fact] + public async Task MoveInchesAsync_ShouldSetLinearDirectionLow_WhenMovingPositive() + { + await _controller.MoveInchesAsync(0.1, 60); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.Low); + } + + [Fact] + public async Task MoveInchesAsync_ShouldSetLinearDirectionHigh_WhenMovingNegative() + { + await _controller.MoveInchesAsync(-0.1, 60); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.High); + } + + [Fact] + public async Task MoveInchesAsync_ShouldSetRotaryDirectionLow_WhenMovingPositive() + { + await _controller.MoveInchesAsync(0.1, 60); + _mockGpio.Received().Write(_config.RotaryAxisConfig.DirectionPin, PinValue.Low); + } + + [Fact] + public async Task MoveInchesAsync_ShouldSetRotaryDirectionHigh_WhenMovingNegative() + { + await _controller.MoveInchesAsync(-0.1, 60); + _mockGpio.Received().Write(_config.RotaryAxisConfig.DirectionPin, PinValue.High); + } + + [Fact] + public async Task MoveInchesAsync_ShouldGenerateLinearPulses() + { + await _controller.MoveInchesAsync(0.01, 60); + _mockGpio.Received().Write(_config.LinearAxisConfig.PulsePin, PinValue.High); + _mockGpio.Received().Write(_config.LinearAxisConfig.PulsePin, PinValue.Low); + } + + [Fact] + public async Task MoveInchesAsync_ShouldUpdateLinearPosition() + { + var cfg = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + }, + RotaryAxisConfig = new RotaryAxisConfig { PulsePin = 16, DirectionPin = 12 }, + GearRatio = 1.0 + }; + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + using var c = new SynchronizedDualAxisController(mockGpio, cfg, _mockLogger); + + await c.MoveInchesAsync(0.1, 60); + + Assert.Equal(0.1, c.CurrentPositionInches, 2); + } + + // ----------------------------------------------------------------------- + // Rotary synchronization & DDA accuracy + // ----------------------------------------------------------------------- + + [Fact] + public async Task MoveInchesAsync_RotaryPulseCount_ShouldMatchGearRatioScaling_OneToOne() + { + // GearRatio = 1.0, SPR equal → rotary pulses should equal linear pulses + var cfg = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + }, + RotaryAxisConfig = new RotaryAxisConfig + { + PulsePin = 16, DirectionPin = 12, + StepsPerRevolution = StepsPerRevolution.SPR_400 + }, + GearRatio = 1.0 + }; + + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + using var c = new SynchronizedDualAxisController(mockGpio, cfg, _mockLogger); + + int totalLinearSteps = 400; // 1 inch at 1 TPI × 400 SPR + await c.MoveInchesAsync(1.0, 60); + + // With 1:1 gear ratio and same SPR, rotary pulses == linear pulses + mockGpio.Received(totalLinearSteps).Write(cfg.LinearAxisConfig.PulsePin, PinValue.High); + mockGpio.Received(totalLinearSteps).Write(cfg.RotaryAxisConfig.PulsePin, PinValue.High); + } + + [Fact] + public async Task MoveInchesAsync_RotaryPulseCount_ShouldScaleByGearRatio() + { + // GearRatio = 0.5 → rotary fires half as many pulses per linear step + var cfg = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + }, + RotaryAxisConfig = new RotaryAxisConfig + { + PulsePin = 16, DirectionPin = 12, + StepsPerRevolution = StepsPerRevolution.SPR_400 + }, + GearRatio = 0.5 + }; + + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + using var c = new SynchronizedDualAxisController(mockGpio, cfg, _mockLogger); + + await c.MoveInchesAsync(1.0, 60); // 400 linear steps + + // 0.5 × (400/400) × 400 linear steps = 200 rotary pulses + mockGpio.Received(400).Write(cfg.LinearAxisConfig.PulsePin, PinValue.High); + mockGpio.Received(200).Write(cfg.RotaryAxisConfig.PulsePin, PinValue.High); + } + + [Fact] + public async Task MoveInchesAsync_RotaryPositionDegrees_ShouldBeNonZeroAfterMove() + { + var cfg = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + }, + RotaryAxisConfig = new RotaryAxisConfig + { + PulsePin = 16, DirectionPin = 12, + StepsPerRevolution = StepsPerRevolution.SPR_400 + }, + GearRatio = 1.0 + }; + + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + using var c = new SynchronizedDualAxisController(mockGpio, cfg, _mockLogger); + + await c.MoveInchesAsync(1.0, 60); + + // 1:1, 1 TPI, 400 SPR → 400 rotary steps = 360° + Assert.Equal(360.0, c.CurrentRotaryPositionDegrees, 1); + } + + // ----------------------------------------------------------------------- + // StopAsync + // ----------------------------------------------------------------------- + + [Fact] + public async Task StopAsync_ShouldCompleteMotionGracefully() + { + var moveTask = _controller.MoveInchesAsync(10.0, 60); + await Task.Delay(50); + await _controller.StopAsync(); + await Task.WhenAny(moveTask, Task.Delay(2000)); + Assert.True(moveTask.IsCompleted); + } + + [Fact] + public async Task StopAsync_ShouldAllowSubsequentMoves() + { + var moveTask = _controller.MoveInchesAsync(5.0, 60); + await Task.Delay(50); + await _controller.StopAsync(); + await moveTask; + + // Should not throw + await _controller.MoveInchesAsync(0.1, 60); + } + + // ----------------------------------------------------------------------- + // RunToLimitSwitchAsync + // ----------------------------------------------------------------------- + + [Fact] + public async Task RunToLimitSwitchAsync_ShouldThrowArgumentException_WhenRpmIsZero() + { + await Assert.ThrowsAsync(() => + _controller.RunToLimitSwitchAsync(LimitSwitch.Max, 0)); + } + + [Fact] + public async Task RunToLimitSwitchAsync_ShouldSetLinearDirectionLow_WhenMovingToMaxLimit() + { + var cts = new CancellationTokenSource(100); + try { await _controller.RunToLimitSwitchAsync(LimitSwitch.Max, 60, cts.Token); } + catch (OperationCanceledException) { } + + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.Low); + } + + [Fact] + public async Task RunToLimitSwitchAsync_ShouldSetLinearDirectionHigh_WhenMovingToMinLimit() + { + var cts = new CancellationTokenSource(100); + try { await _controller.RunToLimitSwitchAsync(LimitSwitch.Min, 60, cts.Token); } + catch (OperationCanceledException) { } + + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.High); + } + + // ----------------------------------------------------------------------- + // ResetPositionAsync + // ----------------------------------------------------------------------- + + [Fact] + public async Task ResetPositionAsync_ShouldSetLinearPositionToZero() + { + var cfg = new SynchronizedDualAxisConfig + { + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + }, + RotaryAxisConfig = new RotaryAxisConfig { PulsePin = 16, DirectionPin = 12 }, + GearRatio = 1.0 + }; + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + using var c = new SynchronizedDualAxisController(mockGpio, cfg, _mockLogger); + + await c.MoveInchesAsync(0.5, 60); + Assert.NotEqual(0.0, c.CurrentPositionInches); + + await c.ResetPositionAsync(); + Assert.Equal(0.0, c.CurrentPositionInches); + } + + // ----------------------------------------------------------------------- + // Limit switches + // ----------------------------------------------------------------------- + + [Fact] + public void IsMinLimitSwitchTriggered_ShouldBeTrueWhenPinIsLowOnInit() + { + var mockGpio = Substitute.For(); + mockGpio.Read(_config.LinearAxisConfig.MinLimitSwitchPin).Returns(PinValue.Low); + mockGpio.Read(_config.LinearAxisConfig.MaxLimitSwitchPin).Returns(PinValue.High); + + using var c = new SynchronizedDualAxisController(mockGpio, _config, _mockLogger); + Assert.True(c.IsMinLimitSwitchTriggered); + } + + // ----------------------------------------------------------------------- + // Dispose + // ----------------------------------------------------------------------- + + [Fact] + public void Dispose_ShouldDisposeGpio() + { + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + var c = new SynchronizedDualAxisController(mockGpio, _config, _mockLogger); + c.Dispose(); + mockGpio.Received(1).Dispose(); + } + + [Fact] + public void Dispose_ShouldBeIdempotent() + { + var mockGpio = Substitute.For(); + mockGpio.Read(Arg.Any()).Returns(PinValue.High); + var c = new SynchronizedDualAxisController(mockGpio, _config, _mockLogger); + c.Dispose(); + c.Dispose(); + mockGpio.Received(1).Dispose(); + } +} diff --git a/UI/Program.cs b/UI/Program.cs index 52d71b6..46f0ba3 100644 --- a/UI/Program.cs +++ b/UI/Program.cs @@ -84,7 +84,7 @@ static void CreateMainWindow(Application app, IStepperMotorController motorContr { var window = ApplicationWindow.New(app); window.SetDecorated(true); - window.SetDefaultSize(800, 480); + window.SetDefaultSize(800, 440); window.SetResizable(false); var ui = new MotorControlUI(window, motorController, config); From 2d813c411dd343cfaa106e6ea96f242c24c14d43 Mon Sep 17 00:00:00 2001 From: Troye Stonich Date: Sun, 26 Apr 2026 18:40:29 -0400 Subject: [PATCH 2/5] doc --- .../Docs/SynchronizedDualAxisController.md | 242 ++++++++++++++++++ 1 file changed, 242 insertions(+) create mode 100644 MotorController/Docs/SynchronizedDualAxisController.md diff --git a/MotorController/Docs/SynchronizedDualAxisController.md b/MotorController/Docs/SynchronizedDualAxisController.md new file mode 100644 index 0000000..5dc5137 --- /dev/null +++ b/MotorController/Docs/SynchronizedDualAxisController.md @@ -0,0 +1,242 @@ +# SynchronizedDualAxisController + +## Overview + +`SynchronizedDualAxisController` drives two stepper motors — a **linear axis** and a **rotary axis** — from a single step-generation loop, analogous to LinuxCNC's stepgen thread. The rotary axis behaves as if it were geared to the linear axis lead screw, but the gear ratio is configured in software so it can be changed without swapping physical gears. + +Implements `ISynchronizedDualAxisController`, which extends `IStepperMotorController`. + +--- + +## Files + +| File | Description | +|---|---| +| `MotorController/ISynchronizedDualAxisController.cs` | Interface extending `IStepperMotorController` with `CurrentRotaryPositionDegrees` | +| `MotorController/SynchronizedDualAxisController.cs` | Full implementation | +| `MotorController/Config/SynchronizedDualAxisConfig.cs` | Combined configuration (already existed) | +| `MotorController/Config/RotaryAxisConfig.cs` | Rotary axis pin/SPR configuration (already existed) | +| `MotorController/Config/LinearAxisConfig.cs` | Linear axis configuration (already existed) | +| `Test/StepperMotorControllerTests.cs` | 30 tests in `SynchronizedDualAxisControllerTests` class appended | + +--- + +## Configuration + +`SynchronizedDualAxisConfig` combines the two axis configs with a gear ratio: + +```csharp +var config = new SynchronizedDualAxisConfig +{ + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 // steps/sec² + }, + RotaryAxisConfig = new RotaryAxisConfig + { + PulsePin = 16, + DirectionPin = 12, + EnablePin = null, // optional + StepsPerRevolution = StepsPerRevolution.SPR_400 + }, + GearRatio = 0.4 // rotary revolutions per linear revolution +}; +``` + +### GearRatio + +`GearRatio` is the number of rotary axis revolutions per one revolution of the linear axis lead screw. + +- `1.0` → rotary turns at exactly the same rate as the lead screw +- `0.4` → rotary turns 0.4× per lead screw revolution (default, matches original mechanical ratio) +- `2.0` → rotary turns twice per lead screw revolution + +The ratio can be changed at any time in configuration without hardware changes. + +--- + +## Design + +### Single Step-Generation Loop + +Both axes are driven from one `Task.Run` loop inside `ExecuteMotionInternalAsync`. The linear axis owns the timing via the **David Austin algorithm**. The rotary axis receives the exact same delay value at every iteration, giving it the identical acceleration and deceleration profile automatically — no separate ramp calculation is needed for the rotary axis. + +### DDA Accumulator — Zero Long-Term Drift + +The rotary steps per linear step is a rational number: + +``` +rotaryStepsPerLinearStep = GearRatio × RotarySPR / LinearSPR +``` + +A `double rotaryAccumulator` carries the sub-step fractional remainder forward across every step. Integer pulses are only fired when the accumulator crosses a whole number boundary. This is mathematically equivalent to LinuxCNC's stepgen DDA and guarantees **zero long-term positional drift** between the axes regardless of how non-integer the ratio is. + +**Example** — `GearRatio = 0.5`, both axes at `SPR_400`: + +``` +rotaryStepsPerLinearStep = 0.5 × 400 / 400 = 0.5 + +Step 0: acc = 0.0 + 0.5 = 0.5 → 0 rotary pulses, remainder 0.5 +Step 1: acc = 0.5 + 0.5 = 1.0 → 1 rotary pulse, remainder 0.0 +Step 2: acc = 0.0 + 0.5 = 0.5 → 0 rotary pulses, remainder 0.5 +... +``` + +Over 400 linear steps → exactly 200 rotary pulses. No drift. + +### `FireStepPeriod` — Stopwatch-Anchored Timing + +The entire step period is consumed inside `FireStepPeriod` using `Stopwatch.GetTimestamp()` spin-waits. This means: + +- The outer loop always advances at the correct linear rate +- Rotary pulses are inserted **within** the step period, not added on top of it +- When multiple rotary pulses fall in one step, they are spaced evenly across the period + +``` +|<-------- one linear step period -------->| +| linear HIGH | linear LOW | rot | rot | +``` + +### Acceleration Inheritance + +Because the rotary axis uses the same `delayMicroseconds` value as the linear axis at every step, it automatically follows the full David Austin acceleration and deceleration profile. No separate `Acceleration` parameter is needed or used for the rotary axis. + +### GPIO + +| Signal | Linear axis | Rotary axis | +|---|---|---| +| Pulse | `LinearAxisConfig.PulsePin` | `RotaryAxisConfig.PulsePin` | +| Direction | `LinearAxisConfig.DirectionPin` | `RotaryAxisConfig.DirectionPin` | +| Enable | `LinearAxisConfig.EnablePin` (optional) | `RotaryAxisConfig.EnablePin` (optional) | +| Min limit switch | `LinearAxisConfig.MinLimitSwitchPin` | **none** | +| Max limit switch | `LinearAxisConfig.MaxLimitSwitchPin` | **none** | + +The rotary axis has **no limit switches** because it rotates continuously. + +Active-low limit switches with internal pull-up resistors (`PinMode.InputPullUp`) — same convention as `StepperMotorController`. + +--- + +## Public API + +```csharp +// Inherited from IStepperMotorController: +Task MoveInchesAsync(double inches, double rpm, CancellationToken cancellationToken = default); +Task RunToLimitSwitchAsync(LimitSwitch direction, double rpm, CancellationToken cancellationToken = default); +Task StopAsync(); +Task ResetPositionAsync(); +void SetTargetSpeed(double rpm); +double CurrentPositionInches { get; } +bool IsMinLimitSwitchTriggered { get; } +bool IsMaxLimitSwitchTriggered { get; } +event EventHandler? MinLimitSwitchTriggered; +event EventHandler? MaxLimitSwitchTriggered; + +// Added by ISynchronizedDualAxisController: +double CurrentRotaryPositionDegrees { get; } +``` + +`CurrentRotaryPositionDegrees` accumulates the total angular displacement of the rotary axis since construction (or last reset). It is thread-safe via its own `SemaphoreSlim`. + +--- + +## Dependency Injection (UI) + +Register in `Program.cs` alongside or instead of `StepperMotorController`: + +```csharp +services.Configure( + context.Configuration.GetSection("SynchronizedDualAxisConfig")); + +services.AddSingleton(sp => + OperatingSystem.IsWindows() + ? new FakeGpioController(sp.GetRequiredService>()) + : new GpioControllerWrapper()); + +services.AddSingleton(sp => + new SynchronizedDualAxisController( + sp.GetRequiredService(), + sp.GetRequiredService>().Value, + sp.GetRequiredService>())); +``` + +Add the corresponding section to `UI/appsettings.json`: + +```json +"SynchronizedDualAxisConfig": { + "GearRatio": 0.4, + "LinearAxisConfig": { + "PulsePin": 21, + "DirectionPin": 20, + "MinLimitSwitchPin": 24, + "MaxLimitSwitchPin": 23, + "EnablePin": null, + "StepsPerRevolution": 400, + "LeadScrewThreadsPerInch": 5.0, + "Acceleration": 5000 + }, + "RotaryAxisConfig": { + "PulsePin": 16, + "DirectionPin": 12, + "EnablePin": null, + "StepsPerRevolution": 400 + } +} +``` + +--- + +## Stop Behaviour + +Consistent with `StepperMotorController`: + +| Trigger | Mechanism | Result | +|---|---|---| +| `StopAsync()` | Sets `_stopRequested = true` | Graceful deceleration, no exception | +| `CancellationToken` cancelled | Hard cancel via linked CTS | Immediate stop, throws `OperationCanceledException` | + +Both axes stop together because they share the same loop. + +--- + +## Thread Safety + +| State | Protection | +|---|---| +| `_currentLinearPositionSteps` | `SemaphoreSlim _positionLock` | +| `_currentRotaryPositionSteps` | `SemaphoreSlim _rotaryPositionLock` | +| `_targetRpm` | `Interlocked.Exchange` / `CompareExchange` | +| `_stopRequested` | `volatile bool` | +| Limit switch state | Written from GPIO callback thread, read from motion loop | + +--- + +## Testing + +30 unit tests in `Test/StepperMotorControllerTests.cs` inside `SynchronizedDualAxisControllerTests`. + +Key test cases: + +| Test | What it verifies | +|---|---| +| `Constructor_ShouldOpenLinearAxisPins` | All linear GPIO pins opened correctly | +| `Constructor_ShouldOpenRotaryAxisPins` | Rotary pulse and direction pins opened | +| `Constructor_ShouldOpenEnablePins_WhenConfigured` | Optional enable pins for both axes | +| `MoveInchesAsync_ShouldSetRotaryDirectionLow_WhenMovingPositive` | Rotary direction mirrors linear | +| `MoveInchesAsync_RotaryPulseCount_ShouldMatchGearRatioScaling_OneToOne` | 1:1 ratio, equal pulse counts | +| `MoveInchesAsync_RotaryPulseCount_ShouldScaleByGearRatio` | 0.5 ratio → half rotary pulses | +| `MoveInchesAsync_RotaryPositionDegrees_ShouldBeNonZeroAfterMove` | 400 rotary steps = 360° | +| `StopAsync_ShouldCompleteMotionGracefully` | Deceleration completes, no exception | +| `Dispose_ShouldBeIdempotent` | GPIO disposed exactly once | + +Run with: + +```powershell +dotnet test --filter "FullyQualifiedName~SynchronizedDualAxisControllerTests" +``` From 4e58dce41f65c59b1ccc6dda104a82fa3fae977b Mon Sep 17 00:00:00 2001 From: Troye Stonich Date: Thu, 30 Apr 2026 19:29:08 -0400 Subject: [PATCH 3/5] updates to UI project --- UI/MotorControlUI.cs | 8 ++++---- UI/Program.cs | 17 +++++++++++++---- UI/appsettings.json | 8 +++++++- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/UI/MotorControlUI.cs b/UI/MotorControlUI.cs index 96c7df6..b8c8628 100644 --- a/UI/MotorControlUI.cs +++ b/UI/MotorControlUI.cs @@ -8,7 +8,7 @@ namespace UI; public class MotorControlUI { private readonly ApplicationWindow _window; - private readonly IStepperMotorController _motorController; + private readonly ISynchronizedDualAxisController _motorController; private readonly LinearAxisConfig _config; private readonly Label _speedLabel; private readonly Label _positionLabel; @@ -31,12 +31,12 @@ public class MotorControlUI public MotorControlUI( ApplicationWindow window, - IStepperMotorController motorController, - LinearAxisConfig config) + ISynchronizedDualAxisController motorController, + SynchronizedDualAxisConfig config) { _window = window; _motorController = motorController; - _config = config; + _config = config.LinearAxisConfig; // Subscribe to limit switch events _motorController.MinLimitSwitchTriggered += OnMinLimitSwitchChanged; diff --git a/UI/Program.cs b/UI/Program.cs index 46f0ba3..69e8a4c 100644 --- a/UI/Program.cs +++ b/UI/Program.cs @@ -22,9 +22,18 @@ services.AddSingleton(); services.Configure(ctx.Configuration.GetSection("LinearAxisConfig")); + services.Configure(ctx.Configuration.GetSection("RotaryAxisConfig")); services.AddSingleton(sp => sp.GetRequiredService>().Value); + services.AddSingleton(sp => sp.GetRequiredService>().Value); - services.AddSingleton(); + services.AddSingleton(sp => new SynchronizedDualAxisConfig + { + LinearAxisConfig = sp.GetRequiredService(), + RotaryAxisConfig = sp.GetRequiredService(), + GearRatio = 0.4 + }); + + services.AddSingleton(); }).ConfigureLogging(logging => { @@ -42,8 +51,8 @@ // ============================ using var host = builder.Build(); -using var motorController = host.Services.GetRequiredService(); -var config = host.Services.GetRequiredService(); +using var motorController = host.Services.GetRequiredService(); +var config = host.Services.GetRequiredService(); // ============================ // Create and run application @@ -80,7 +89,7 @@ static bool IsRaspberryPi() } // Create main window -static void CreateMainWindow(Application app, IStepperMotorController motorController, LinearAxisConfig config) +static void CreateMainWindow(Application app, ISynchronizedDualAxisController motorController, SynchronizedDualAxisConfig config) { var window = ApplicationWindow.New(app); window.SetDecorated(true); diff --git a/UI/appsettings.json b/UI/appsettings.json index df894cf..4572a3e 100644 --- a/UI/appsettings.json +++ b/UI/appsettings.json @@ -13,5 +13,11 @@ "StepsPerRevolution": "SPR_400", "LeadScrewThreadsPerInch": 5.0, "Acceleration": 5000 + }, + "RotaryAxisConfig": { + "PulsePin": 19, + "DirectionPin": 26, + "EnablePin": null, + "StepsPerRevolution": "SPR_400" } -} \ No newline at end of file +} From 9d74ecafb02ee53724d535841c66697f719384a6 Mon Sep 17 00:00:00 2001 From: Troye Stonich Date: Fri, 1 May 2026 18:59:19 -0400 Subject: [PATCH 4/5] u --- MotorController/IStepperMotorController.cs | 75 --- .../ISynchronizedDualAxisController.cs | 74 ++- MotorController/StepperMotorController.cs | 492 -------------- .../SynchronizedDualAxisController.cs | 6 +- Scripts/update-and-publish.sh | 2 + Test/StepperMotorControllerTests.cs | 620 ++++++++++-------- 6 files changed, 426 insertions(+), 843 deletions(-) delete mode 100644 MotorController/IStepperMotorController.cs delete mode 100644 MotorController/StepperMotorController.cs diff --git a/MotorController/IStepperMotorController.cs b/MotorController/IStepperMotorController.cs deleted file mode 100644 index f8a0b8f..0000000 --- a/MotorController/IStepperMotorController.cs +++ /dev/null @@ -1,75 +0,0 @@ - -namespace MotorControllerApp; - -public interface IStepperMotorController : IDisposable -{ - /// - /// Asynchronously moves the device the specified distance in inches at the given speed in revolutions per minute - /// (RPM). - /// - /// The distance to move, in inches. Can be positive or negative to indicate direction. - /// The speed at which to move, in revolutions per minute. Must be greater than zero. - /// A cancellation token that can be used to cancel the move operation. - /// A task that represents the asynchronous move operation. - Task MoveInchesAsync(double inches, double rpm, CancellationToken cancellationToken = default); - - /// - /// Asynchronously resets the position to its initial state. - /// - /// A task that represents the asynchronous reset operation. - Task ResetPositionAsync(); - - /// - /// Moves the carriage toward the specified limit switch at the given speed until the switch is reached or the - /// operation is canceled. - /// - /// The direction to move: Min for minimum limit switch, Max for maximum limit switch. - /// The speed, in revolutions per minute, at which to move the carriage. Must be greater than zero. - /// A cancellation token that can be used to cancel the operation. - /// A task that represents the asynchronous operation. The task completes when the carriage reaches the specified - /// limit switch or the operation is canceled. - Task RunToLimitSwitchAsync(LimitSwitch direction, double rpm, CancellationToken cancellationToken = default); - - /// - /// Sets the target speed in revolutions per minute (RPM). - /// - /// - void SetTargetSpeed(double rpm); - - /// - /// Asynchronously stops the carriage. - /// - /// A task that represents the asynchronous stop operation. - Task StopAsync(); - - /// - /// Gets the current position of the carriage in inches. - /// - double CurrentPositionInches { get; } - - /// - /// Gets a value indicating whether the maximum limit switch is triggered. - /// - bool IsMaxLimitSwitchTriggered { get; } - - /// - /// Gets a value indicating whether the minimum limit switch is triggered. - /// - bool IsMinLimitSwitchTriggered { get; } - - /// - /// Occurs when the minimum limit switch is triggered. - /// - /// This event is typically raised when a device or mechanism reaches its minimum allowable - /// position or state, as detected by a limit switch. Subscribers can use this event to perform actions such as - /// stopping movement or initiating safety procedures. - event EventHandler? MinLimitSwitchTriggered; - - /// - /// Occurs when the maximum limit switch is triggered. - /// - /// This event is typically raised when a device or mechanism reaches its maximum allowable - /// position or state, as detected by a limit switch. Subscribers can use this event to perform actions such as - /// stopping movement or initiating safety procedures. - event EventHandler? MaxLimitSwitchTriggered; -} diff --git a/MotorController/ISynchronizedDualAxisController.cs b/MotorController/ISynchronizedDualAxisController.cs index d39734b..469005c 100644 --- a/MotorController/ISynchronizedDualAxisController.cs +++ b/MotorController/ISynchronizedDualAxisController.cs @@ -10,10 +10,82 @@ namespace MotorControllerApp; /// the configured gear ratio, with a DDA (Digital Differential Analyzer) accumulator ensuring /// zero long-term drift between the two axes. /// -public interface ISynchronizedDualAxisController : IStepperMotorController +public interface ISynchronizedDualAxisController : IDisposable { + /// + /// Asynchronously moves the device the specified distance in inches at the given speed in revolutions per minute + /// (RPM). + /// + /// The distance to move, in inches. Can be positive or negative to indicate direction. + /// The speed at which to move, in revolutions per minute. Must be greater than zero. + /// A cancellation token that can be used to cancel the move operation. + /// A task that represents the asynchronous move operation. + Task MoveInchesAsync(double inches, double rpm, CancellationToken cancellationToken = default); + + /// + /// Asynchronously resets the position to its initial state. + /// + /// A task that represents the asynchronous reset operation. + Task ResetPositionAsync(); + + /// + /// Moves the carriage toward the specified limit switch at the given speed until the switch is reached or the + /// operation is canceled. + /// + /// The direction to move: Min for minimum limit switch, Max for maximum limit switch. + /// The speed, in revolutions per minute, at which to move the carriage. Must be greater than zero. + /// A cancellation token that can be used to cancel the operation. + /// A task that represents the asynchronous operation. The task completes when the carriage reaches the specified + /// limit switch or the operation is canceled. + Task RunToLimitSwitchAsync(LimitSwitch direction, double rpm, CancellationToken cancellationToken = default); + + /// + /// Sets the target speed in revolutions per minute (RPM). + /// + /// + void SetTargetSpeed(double rpm); + + /// + /// Asynchronously stops the carriage. + /// + /// A task that represents the asynchronous stop operation. + Task StopAsync(); + + /// + /// Gets the current position of the carriage in inches. + /// + double CurrentPositionInches { get; } + /// /// Gets the current angular position of the rotary axis in degrees. /// double CurrentRotaryPositionDegrees { get; } + + /// + /// Gets a value indicating whether the maximum limit switch is triggered. + /// + bool IsMaxLimitSwitchTriggered { get; } + + /// + /// Gets a value indicating whether the minimum limit switch is triggered. + /// + bool IsMinLimitSwitchTriggered { get; } + + /// + /// Occurs when the minimum limit switch is triggered. + /// + /// This event is typically raised when a device or mechanism reaches its minimum allowable + /// position or state, as detected by a limit switch. Subscribers can use this event to perform actions such as + /// stopping movement or initiating safety procedures. + event EventHandler? MinLimitSwitchTriggered; + + /// + /// Occurs when the maximum limit switch is triggered. + /// + /// This event is typically raised when a device or mechanism reaches its maximum allowable + /// position or state, as detected by a limit switch. Subscribers can use this event to perform actions such as + /// stopping movement or initiating safety procedures. + event EventHandler? MaxLimitSwitchTriggered; + + } diff --git a/MotorController/StepperMotorController.cs b/MotorController/StepperMotorController.cs deleted file mode 100644 index 8ae08bd..0000000 --- a/MotorController/StepperMotorController.cs +++ /dev/null @@ -1,492 +0,0 @@ -using System.Device.Gpio; -using System.Diagnostics; -using Microsoft.Extensions.Logging; - -namespace MotorControllerApp; - -public class StepperMotorController : IStepperMotorController -{ - private readonly IGpioController _gpio; - private readonly LinearAxisConfig _config; - private readonly ILogger _logger; - private readonly SemaphoreSlim _positionLock = new(1, 1); - private readonly SemaphoreSlim _motionLock = new(1, 1); - private CancellationTokenSource _stopTokenSource = new(); - private double _currentPositionSteps; - private bool _disposed; - private volatile bool _stopRequested; - private double _targetRpm; - - public double CurrentPositionInches - { - get - { - _positionLock.Wait(); - try - { - return _currentPositionSteps / (int)_config.StepsPerRevolution / _config.LeadScrewThreadsPerInch; - } - finally - { - _positionLock.Release(); - } - } - } - - public bool IsMinLimitSwitchTriggered { get; private set; } - public bool IsMaxLimitSwitchTriggered { get; private set; } - - public event EventHandler? MinLimitSwitchTriggered; - public event EventHandler? MaxLimitSwitchTriggered; - - public StepperMotorController(IGpioController gpio, LinearAxisConfig config, ILogger logger) - { - _gpio = gpio ?? throw new ArgumentNullException(nameof(gpio)); - _config = config ?? throw new ArgumentNullException(nameof(config)); - _logger = logger ?? throw new ArgumentNullException(nameof(logger)); - - _logger.LogInformation("Initializing StepperMotorController with config: PulsePin={PulsePin}, DirectionPin={DirectionPin}, EnablePin={EnablePin}, MinLimitSwitchPin={MinLimitSwitchPin}, MaxLimitSwitchPin={MaxLimitSwitchPin}, StepsPerRevolution={StepsPerRevolution}, LeadScrewThreadsPerInch={LeadScrewThreadsPerInch}, Acceleration={Acceleration}", - _config.PulsePin, _config.DirectionPin, _config.EnablePin, _config.MinLimitSwitchPin, _config.MaxLimitSwitchPin, _config.StepsPerRevolution, _config.LeadScrewThreadsPerInch, _config.Acceleration); - - InitializePins(); - } - - private void InitializePins() - { - // Close pins if already open (cleanup from previous runs) - if (_gpio.IsPinOpen(_config.PulsePin)) _gpio.ClosePin(_config.PulsePin); - if (_gpio.IsPinOpen(_config.DirectionPin)) _gpio.ClosePin(_config.DirectionPin); - if (_config.EnablePin.HasValue && _gpio.IsPinOpen(_config.EnablePin.Value)) _gpio.ClosePin(_config.EnablePin.Value); - if (_gpio.IsPinOpen(_config.MinLimitSwitchPin)) _gpio.ClosePin(_config.MinLimitSwitchPin); - if (_gpio.IsPinOpen(_config.MaxLimitSwitchPin)) _gpio.ClosePin(_config.MaxLimitSwitchPin); - - // Initialize output pins - _gpio.OpenPin(_config.PulsePin, PinMode.Output); - _gpio.OpenPin(_config.DirectionPin, PinMode.Output); - - if (_config.EnablePin.HasValue) - { - _gpio.OpenPin(_config.EnablePin.Value, PinMode.Output); - _gpio.Write(_config.EnablePin.Value, PinValue.High); // Disabled by default - } - - // Initialize limit switch pins - _gpio.OpenPin(_config.MinLimitSwitchPin, PinMode.Input); - _gpio.OpenPin(_config.MaxLimitSwitchPin, PinMode.Input); - - // Register callbacks for limit switches - _gpio.RegisterCallbackForPinValueChangedEvent(_config.MinLimitSwitchPin, PinEventTypes.Falling | PinEventTypes.Rising, OnMinLimitSwitchChanged); - _gpio.RegisterCallbackForPinValueChangedEvent(_config.MaxLimitSwitchPin, PinEventTypes.Falling | PinEventTypes.Rising, OnMaxLimitSwitchChanged); - - // Read initial limit switch states - IsMinLimitSwitchTriggered = _gpio.Read(_config.MinLimitSwitchPin) == PinValue.Low; - IsMaxLimitSwitchTriggered = _gpio.Read(_config.MaxLimitSwitchPin) == PinValue.Low; - } - - private void OnMinLimitSwitchChanged(object sender, PinValueChangedEventArgs e) - { - IsMinLimitSwitchTriggered = e.ChangeType == PinEventTypes.Falling; - _logger.LogInformation("Min limit switch {Status} (Pin {Pin})", IsMinLimitSwitchTriggered ? "triggered" : "released", _config.MinLimitSwitchPin); - MinLimitSwitchTriggered?.Invoke(this, EventArgs.Empty); - } - - private void OnMaxLimitSwitchChanged(object sender, PinValueChangedEventArgs e) - { - IsMaxLimitSwitchTriggered = e.ChangeType == PinEventTypes.Falling; - _logger.LogInformation("Max limit switch {Status} (Pin {Pin})", IsMaxLimitSwitchTriggered ? "triggered" : "released", _config.MaxLimitSwitchPin); - MaxLimitSwitchTriggered?.Invoke(this, EventArgs.Empty); - } - - public async Task MoveInchesAsync(double inches, double rpm, CancellationToken cancellationToken = default) - { - if (rpm <= 0) - throw new ArgumentException("RPM must be greater than zero", nameof(rpm)); - - await _motionLock.WaitAsync(cancellationToken); - try - { - _stopRequested = false; - - var totalSteps = (int)(inches * _config.LeadScrewThreadsPerInch * (int)_config.StepsPerRevolution); - var direction = totalSteps >= 0; - - _gpio.Write(_config.DirectionPin, direction ? PinValue.Low : PinValue.High); - - if (_config.EnablePin.HasValue) - _gpio.Write(_config.EnablePin.Value, PinValue.Low); // Enable motor - - try - { - await ExecuteMotionAsync(Math.Abs(totalSteps), rpm, direction, cancellationToken); - } - catch (OperationCanceledException) - { - // Expected when motion is cancelled via StopAsync or cancellationToken - } - finally - { - if (_config.EnablePin.HasValue) - _gpio.Write(_config.EnablePin.Value, PinValue.High); // Disable motor - } - } - finally - { - _motionLock.Release(); - } - } - - public async Task RunToLimitSwitchAsync(LimitSwitch direction, double rpm, CancellationToken cancellationToken = default) - { - if (rpm <= 0) - throw new ArgumentException("RPM must be greater than zero", nameof(rpm)); - - await _motionLock.WaitAsync(cancellationToken); - try - { - _stopRequested = false; - - bool toMaxLimit = direction == LimitSwitch.Max; - _gpio.Write(_config.DirectionPin, toMaxLimit ? PinValue.Low : PinValue.High); - - if (_config.EnablePin.HasValue) - _gpio.Write(_config.EnablePin.Value, PinValue.Low); // Enable motor - - try - { - await ExecuteMotionInternalAsync( - direction: toMaxLimit, - initialRpm: rpm, - maxSteps: null, // Run indefinitely until limit switch - shouldStartDeceleration: (step, accelSteps) => - { - // Only check after acceleration phase - if (step < accelSteps) - return false; - - return toMaxLimit ? IsMaxLimitSwitchTriggered : IsMinLimitSwitchTriggered; - }, - maxDecelerationSteps: 300, // Limit deceleration for limit switch moves - cancellationToken: cancellationToken); - } - catch (OperationCanceledException) - { - // Expected when stopped - } - finally - { - if (_config.EnablePin.HasValue) - _gpio.Write(_config.EnablePin.Value, PinValue.High); // Disable motor - } - } - finally - { - _motionLock.Release(); - } - } - - public async Task StopAsync() - { - _stopRequested = true; - await Task.CompletedTask; - } - - - /// - /// Sets the target speed in revolutions per minute (RPM) while the motor is running. - /// - /// - /// This method allows dynamic speed adjustment during motion. The motor will smoothly transition - /// to the new speed by recalculating the pulse timing. The new RPM will be applied during the - /// constant speed phase of motion. Changes are ignored if RPM is zero or negative. - /// - /// The target speed in revolutions per minute. Must be greater than zero. - public void SetTargetSpeed(double rpm) - { - if (rpm <= 0) - { - _logger.LogWarning("SetTargetRpm called with invalid RPM value: {Rpm}. Ignoring.", rpm); - return; - } - - Interlocked.Exchange(ref _targetRpm, rpm); - _logger.LogInformation("Target RPM updated to {Rpm}", rpm); - } - - public async Task ResetPositionAsync() - { - await _positionLock.WaitAsync(); - try - { - _currentPositionSteps = 0; - } - finally - { - _positionLock.Release(); - } - } - - - private void MicrosecondSleep(int microseconds) - { - // Get the frequency of the stopwatch (ticks per second) - double stopwatchFrequency = Stopwatch.Frequency; - - // Calculate the number of ticks required for the desired delay - long ticks = (long)(microseconds * (stopwatchFrequency / 1_000_000)); - - // Get the start timestamp - long start = Stopwatch.GetTimestamp(); - - // Wait in a tight loop until the required ticks have elapsed - while (Stopwatch.GetTimestamp() - start < ticks) - { - // Optional: use Thread.SpinWait to prevent the OS from unnecessarily - // context switching for very short waits (e.g., < 40ns) - // Thread.SpinWait(1); - } - } - - - /// - /// Executes a motion sequence by generating step pulses with acceleration and deceleration profiles at the - /// specified speed. - /// - /// The motion sequence accelerates at the beginning, maintains a constant speed, and then - /// decelerates before stopping. The operation can be cancelled at any time via the provided cancellation token or - /// an internal stop request. If cancellation is requested, the motion will stop as soon as possible. - /// The total number of steps to move. Must be a non-negative integer. - /// The target speed in revolutions per minute. Must be greater than zero. - /// The direction of movement. True for forward/max, false for reverse/min. - /// A cancellation token that can be used to cancel the motion operation. - /// A task that represents the asynchronous operation of executing the motion sequence. - internal Task ExecuteMotionAsync(int steps, double rpm, bool direction, CancellationToken cancellationToken) - { - return ExecuteMotionInternalAsync( - direction: direction, - initialRpm: rpm, - maxSteps: steps, - shouldStartDeceleration: (step, accelSteps) => false, // Deceleration based on remaining steps - maxDecelerationSteps: null, - cancellationToken: cancellationToken); - } - - /// - /// Internal unified motion execution engine with acceleration/deceleration profiles. - /// - /// The direction of movement. True for forward/max, false for reverse/min. - /// Initial target speed in RPM. - /// Maximum steps to execute. Null means run indefinitely until shouldStartDeceleration triggers. - /// Function called each step to determine if deceleration should begin. Returns true to start decel. - /// Optional limit on deceleration steps (e.g., 300 for limit switch moves). - /// Cancellation token for the operation. - private Task ExecuteMotionInternalAsync( - bool direction, - double initialRpm, - int? maxSteps, - Func shouldStartDeceleration, - int? maxDecelerationSteps, - CancellationToken cancellationToken) - { - Interlocked.Exchange(ref _targetRpm, initialRpm); // Initialize target RPM - var currentRpm = Interlocked.CompareExchange(ref _targetRpm, 0, 0); // Thread-safe read - var maxStepsPerSecond = (currentRpm * (int)_config.StepsPerRevolution) / 60.0; - var targetDelayMicroseconds = 1000000.0 / maxStepsPerSecond; - - // Initial delay using David Austin algorithm: c0 = 0.676 * sqrt(2/α) * 10^6 - var initialDelayMicroseconds = 0.676 * Math.Sqrt(2.0 / _config.Acceleration) * 1000000.0; - - // Calculate acceleration steps needed to reach target speed - var accelerationSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2.0 * _config.Acceleration)); - var decelerationSteps = accelerationSteps; - - // Apply max deceleration limit if specified (for limit switch moves) - if (maxDecelerationSteps.HasValue) - decelerationSteps = Math.Min(decelerationSteps, maxDecelerationSteps.Value); - - // Adjust if motion is too short for full acceleration/deceleration (only for fixed-step moves) - if (maxSteps.HasValue && accelerationSteps + decelerationSteps > maxSteps.Value) - { - accelerationSteps = maxSteps.Value / 2; - decelerationSteps = maxSteps.Value - accelerationSteps; - } - - if (maxSteps.HasValue) - { - _logger.LogDebug($"Total Steps: {maxSteps.Value}"); - _logger.LogDebug($"Max steps/second: {maxStepsPerSecond:n2}"); - _logger.LogDebug($"Accel Steps: {accelerationSteps}"); - _logger.LogDebug($"Decel Steps: {decelerationSteps}"); - _logger.LogDebug($"Initial Delay us: {initialDelayMicroseconds}"); - _logger.LogDebug($"Target Delay us: {targetDelayMicroseconds}"); - } - - return Task.Run(async () => - { - using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _stopTokenSource.Token); - - double delayMicroseconds = initialDelayMicroseconds; - bool stopRequested = false; - bool externalDecelTrigger = false; - int decelStepCounter = 0; - int step = 0; - - while (true) - { - // Check for hard cancellation (emergency stop) - throw immediately without deceleration - if (linkedCts.Token.IsCancellationRequested) - linkedCts.Token.ThrowIfCancellationRequested(); - - // For fixed-step moves, check if we've completed all steps - if (maxSteps.HasValue && step >= maxSteps.Value) - break; - - // Check if RPM changed and update timing if in constant speed phase - if (!externalDecelTrigger && !stopRequested && step >= accelerationSteps) - { - var latestRpm = Interlocked.CompareExchange(ref _targetRpm, 0, 0); // Thread-safe read - if (Math.Abs(currentRpm - latestRpm) > 0.01) - { - currentRpm = latestRpm; - maxStepsPerSecond = (currentRpm * (int)_config.StepsPerRevolution) / 60.0; - targetDelayMicroseconds = 1000000.0 / maxStepsPerSecond; - - // Recalculate deceleration steps for new speed - var newAccelerationSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2.0 * _config.Acceleration)); - var newDecelerationSteps = newAccelerationSteps; - - // Apply max deceleration limit if specified - if (maxDecelerationSteps.HasValue) - newDecelerationSteps = Math.Min(newDecelerationSteps, maxDecelerationSteps.Value); - - // For fixed-step moves, adjust deceleration if not enough steps remaining - if (maxSteps.HasValue) - { - var stepsRemaining = maxSteps.Value - step; - if (newDecelerationSteps < stepsRemaining) - { - decelerationSteps = newDecelerationSteps; - } - else - { - // Not enough room for full deceleration, adjust to fit - decelerationSteps = Math.Max(1, stepsRemaining / 2); - } - - _logger.LogInformation("Speed changed to {Rpm} RPM during motion (step {Step}/{TotalSteps}, decel steps: {DecelSteps})", - currentRpm, step, maxSteps.Value, decelerationSteps); - } - else - { - // For infinite moves (limit switch), just update deceleration steps - decelerationSteps = newDecelerationSteps; - _logger.LogInformation("Speed changed to {Rpm} RPM during motion (decel steps: {DecelSteps})", currentRpm, decelerationSteps); - } - } - } - - // Check if stop was requested - if (!stopRequested && _stopRequested) - { - stopRequested = true; - - // Dynamically calculate deceleration steps based on current motion phase - if (step < accelerationSteps) - { - // Still accelerating - decelerate for the same number of steps we've accelerated - decelerationSteps = Math.Max(1, step); - _logger.LogInformation("Stop requested during acceleration at step {Step}. Will decelerate for {DecelSteps} steps.", step, decelerationSteps); - } - else - { - // In constant speed phase - use calculated deceleration steps (possibly limited by maxDecelerationSteps) - _logger.LogInformation("Stop requested during constant speed. Will decelerate for {DecelSteps} steps.", decelerationSteps); - } - } - - // Check for external deceleration trigger (e.g., limit switch) - if (!externalDecelTrigger && !stopRequested && shouldStartDeceleration(step, accelerationSteps)) - { - externalDecelTrigger = true; - } - - // If decelerating and reached the end, stop - if (externalDecelTrigger || stopRequested) - { - if (decelStepCounter >= decelerationSteps) - break; - - // Deceleration phase - mirror the acceleration ramp - int decelStep = decelerationSteps - decelStepCounter; - if (decelStep > 0) - { - delayMicroseconds = delayMicroseconds + ((2.0 * delayMicroseconds) / ((4.0 * decelStep) - 1.0)); - } - decelStepCounter++; - } - // Acceleration phase - David Austin algorithm: cn = cn-1 - (2 * cn-1) / (4n + 1) - else if (step < accelerationSteps) - { - if (step > 0) - { - delayMicroseconds = delayMicroseconds - ((2.0 * delayMicroseconds) / ((4.0 * step) + 1.0)); - } - } - // Constant speed phase - maintain target speed (or handle planned deceleration for fixed-step moves) - else - { - // For fixed-step moves, check if we should enter planned deceleration - if (maxSteps.HasValue && step >= maxSteps.Value - decelerationSteps) - { - // Start planned deceleration - int decelStep = maxSteps.Value - step; - if (decelStep > 0) - { - delayMicroseconds = delayMicroseconds + ((2.0 * delayMicroseconds) / ((4.0 * decelStep) - 1.0)); - } - } - else - { - delayMicroseconds = targetDelayMicroseconds; - } - } - - _logger.LogDebug($"Delay us: {delayMicroseconds}"); - - _gpio.Write(_config.PulsePin, PinValue.High); - MicrosecondSleep((int)delayMicroseconds / 2); - - _gpio.Write(_config.PulsePin, PinValue.Low); - MicrosecondSleep((int)delayMicroseconds / 2); - - await _positionLock.WaitAsync(linkedCts.Token); - try - { - _currentPositionSteps += direction ? 1 : -1; - } - finally - { - _positionLock.Release(); - } - - step++; - } - - _stopRequested = false; - }); - } - - public void Dispose() - { - if (_disposed) - return; - - _stopTokenSource?.Cancel(); - _stopTokenSource?.Dispose(); - _positionLock?.Dispose(); - _motionLock?.Dispose(); - - _gpio?.Dispose(); - - _disposed = true; - } -} \ No newline at end of file diff --git a/MotorController/SynchronizedDualAxisController.cs b/MotorController/SynchronizedDualAxisController.cs index eb93029..21a378f 100644 --- a/MotorController/SynchronizedDualAxisController.cs +++ b/MotorController/SynchronizedDualAxisController.cs @@ -304,7 +304,7 @@ public void SetTargetSpeed(double rpm) { if (rpm <= 0) { - _logger.LogWarning("SetTargetSpeed called with invalid RPM value: {Rpm}. Ignoring.", rpm); + _logger.LogWarning("SetTargetRpm called with invalid RPM value: {Rpm}", rpm); return; } @@ -478,13 +478,13 @@ private Task ExecuteMotionInternalAsync( { var remaining = maxSteps.Value - step; decelerationSteps = newDecelSteps < remaining ? newDecelSteps : Math.Max(1, remaining / 2); - _logger.LogInformation("Speed changed to {Rpm} RPM (step {Step}/{Total}, decel={Decel})", + _logger.LogInformation("Speed changed to {Rpm} RPM (step {Step}/{Total}, decel steps: {Decel})", currentRpm, step, maxSteps.Value, decelerationSteps); } else { decelerationSteps = newDecelSteps; - _logger.LogInformation("Speed changed to {Rpm} RPM (decel={Decel})", currentRpm, decelerationSteps); + _logger.LogInformation("Speed changed to {Rpm} RPM (decel steps: {Decel})", currentRpm, decelerationSteps); } } } diff --git a/Scripts/update-and-publish.sh b/Scripts/update-and-publish.sh index 76edee5..68c7b4f 100644 --- a/Scripts/update-and-publish.sh +++ b/Scripts/update-and-publish.sh @@ -21,6 +21,8 @@ cd "$REPO_DIR" echo "Pulling latest changes from git..." git pull +git checkout Troye/RotaryAxis + # Restore dependencies echo "Restoring dependencies..." dotnet restore diff --git a/Test/StepperMotorControllerTests.cs b/Test/StepperMotorControllerTests.cs index 57a7921..1ec18dc 100644 --- a/Test/StepperMotorControllerTests.cs +++ b/Test/StepperMotorControllerTests.cs @@ -9,30 +9,33 @@ namespace MotorController.Tests; public class StepperMotorControllerTests : IDisposable { private readonly IGpioController _mockGpio; - private readonly LinearAxisConfig _config; - private readonly ILogger _mockLogger; - private readonly StepperMotorController _controller; + private readonly SynchronizedDualAxisConfig _config; + private readonly ILogger _mockLogger; + private readonly SynchronizedDualAxisController _controller; public StepperMotorControllerTests() { _mockGpio = Substitute.For(); - _mockLogger = Substitute.For>(); - _config = new LinearAxisConfig + _mockLogger = Substitute.For>(); + _config = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; // Setup default behavior for Read - limit switches not triggered - _mockGpio.Read(_config.MinLimitSwitchPin).Returns(PinValue.High); - _mockGpio.Read(_config.MaxLimitSwitchPin).Returns(PinValue.High); + _mockGpio.Read(_config.LinearAxisConfig.MinLimitSwitchPin).Returns(PinValue.High); + _mockGpio.Read(_config.LinearAxisConfig.MaxLimitSwitchPin).Returns(PinValue.High); - _controller = new StepperMotorController(_mockGpio, _config, _mockLogger); + _controller = new SynchronizedDualAxisController(_mockGpio, _config, _mockLogger); } public void Dispose() @@ -40,9 +43,9 @@ public void Dispose() _controller?.Dispose(); } - private static ILogger CreateMockLogger() + private static ILogger CreateMockLogger() { - return Substitute.For>(); + return Substitute.For>(); } #region Constructor Tests @@ -51,30 +54,30 @@ private static ILogger CreateMockLogger() public void Constructor_ShouldThrowArgumentNullException_WhenGpioIsNull() { // Act & Assert - Assert.Throws(() => new StepperMotorController(null!, _config, _mockLogger)); + Assert.Throws(() => new SynchronizedDualAxisController(null!, _config, _mockLogger)); } [Fact] public void Constructor_ShouldThrowArgumentNullException_WhenConfigIsNull() { // Act & Assert - Assert.Throws(() => new StepperMotorController(_mockGpio, null!, _mockLogger)); + Assert.Throws(() => new SynchronizedDualAxisController(_mockGpio, null!, _mockLogger)); } [Fact] public void Constructor_ShouldInitializeOutputPins() { // Assert - _mockGpio.Received(1).OpenPin(_config.PulsePin, PinMode.Output); - _mockGpio.Received(1).OpenPin(_config.DirectionPin, PinMode.Output); + _mockGpio.Received(1).OpenPin(_config.LinearAxisConfig.PulsePin, PinMode.Output); + _mockGpio.Received(1).OpenPin(_config.LinearAxisConfig.DirectionPin, PinMode.Output); } [Fact] public void Constructor_ShouldInitializeLimitSwitchPins() { // Assert - _mockGpio.Received(1).OpenPin(_config.MinLimitSwitchPin, PinMode.Input); - _mockGpio.Received(1).OpenPin(_config.MaxLimitSwitchPin, PinMode.Input); + _mockGpio.Received(1).OpenPin(_config.LinearAxisConfig.MinLimitSwitchPin, PinMode.Input); + _mockGpio.Received(1).OpenPin(_config.LinearAxisConfig.MaxLimitSwitchPin, PinMode.Input); } [Fact] @@ -82,12 +85,12 @@ public void Constructor_ShouldRegisterLimitSwitchCallbacks() { // Assert _mockGpio.Received(1).RegisterCallbackForPinValueChangedEvent( - _config.MinLimitSwitchPin, + _config.LinearAxisConfig.MinLimitSwitchPin, PinEventTypes.Falling | PinEventTypes.Rising, Arg.Any()); _mockGpio.Received(1).RegisterCallbackForPinValueChangedEvent( - _config.MaxLimitSwitchPin, + _config.LinearAxisConfig.MaxLimitSwitchPin, PinEventTypes.Falling | PinEventTypes.Rising, Arg.Any()); } @@ -96,23 +99,26 @@ public void Constructor_ShouldRegisterLimitSwitchCallbacks() public void Constructor_ShouldInitializeEnablePin_WhenConfigured() { // Arrange - var configWithEnable = new LinearAxisConfig + var configWithEnable = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - EnablePin = 16, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + EnablePin = 15, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); // Act - using var controller = new StepperMotorController(mockGpio, configWithEnable, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, configWithEnable, CreateMockLogger()); // Assert - mockGpio.Received(1).OpenPin(16, PinMode.Output); - mockGpio.Received(1).Write(16, PinValue.High); // Disabled by default + mockGpio.Received(1).OpenPin(15, PinMode.Output); + mockGpio.Received(1).Write(15, PinValue.High); // Disabled by default } #endregion @@ -171,11 +177,11 @@ public void IsMinLimitSwitchTriggered_ShouldReturnTrue_WhenPinIsLowOnInit() { // Arrange var mockGpio = Substitute.For(); - mockGpio.Read(_config.MinLimitSwitchPin).Returns(PinValue.Low); - mockGpio.Read(_config.MaxLimitSwitchPin).Returns(PinValue.High); + mockGpio.Read(_config.LinearAxisConfig.MinLimitSwitchPin).Returns(PinValue.Low); + mockGpio.Read(_config.LinearAxisConfig.MaxLimitSwitchPin).Returns(PinValue.High); // Act - using var controller = new StepperMotorController(mockGpio, _config, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, _config, CreateMockLogger()); // Assert Assert.True(controller.IsMinLimitSwitchTriggered); @@ -208,7 +214,7 @@ public async Task MoveInchesAsync_ShouldSetDirectionLow_WhenMovingPositive() await _controller.MoveInchesAsync(0.1, 60); // Assert - _mockGpio.Received().Write(_config.DirectionPin, PinValue.Low); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.Low); } [Fact] @@ -218,28 +224,31 @@ public async Task MoveInchesAsync_ShouldSetDirectionHigh_WhenMovingNegative() await _controller.MoveInchesAsync(-0.1, 60); // Assert - _mockGpio.Received().Write(_config.DirectionPin, PinValue.High); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.High); } [Fact] public async Task MoveInchesAsync_ShouldEnableMotor_WhenEnablePinConfigured() { // Arrange - var configWithEnable = new LinearAxisConfig + var configWithEnable = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - EnablePin = 16, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + EnablePin = 16, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, configWithEnable, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, configWithEnable, CreateMockLogger()); // Act await controller.MoveInchesAsync(0.1, 60); @@ -256,27 +265,30 @@ public async Task MoveInchesAsync_ShouldGeneratePulses() await _controller.MoveInchesAsync(0.01, 60); // Assert - Should have generated some pulses - _mockGpio.Received().Write(_config.PulsePin, PinValue.High); - _mockGpio.Received().Write(_config.PulsePin, PinValue.Low); + _mockGpio.Received().Write(_config.LinearAxisConfig.PulsePin, PinValue.High); + _mockGpio.Received().Write(_config.LinearAxisConfig.PulsePin, PinValue.Low); } [Fact] public async Task MoveInchesAsync_ShouldUpdatePosition_AfterMove() { // Arrange - var shortConfig = new LinearAxisConfig + var shortConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 50000.0 // High acceleration for quick test + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 // High acceleration for quick test + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, shortConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, shortConfig, CreateMockLogger()); // Act await controller.MoveInchesAsync(0.1, 60); @@ -322,7 +334,7 @@ public async Task RunToLimitSwitchAsync_ShouldSetDirectionLow_WhenMovingToMaxLim } // Assert - _mockGpio.Received().Write(_config.DirectionPin, PinValue.Low); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.Low); } [Fact] @@ -342,7 +354,7 @@ public async Task RunToLimitSwitchAsync_ShouldSetDirectionHigh_WhenMovingToMinLi } // Assert - _mockGpio.Received().Write(_config.DirectionPin, PinValue.High); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.High); } #endregion @@ -382,43 +394,46 @@ public async Task StopAsync_ShouldTriggerGracefulDeceleration_InsteadOfAbruptSto { // Arrange var mockGpio = Substitute.For(); - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; - + mockGpio.Read(Arg.Any()).Returns(PinValue.High); - + // Track pulse timings to verify deceleration pattern var pulseHighTimestamps = new List(); var stopwatch = System.Diagnostics.Stopwatch.StartNew(); long stopCalledTimestamp = 0; - - mockGpio.When(x => x.Write(testConfig.PulsePin, PinValue.High)) + + mockGpio.When(x => x.Write(testConfig.LinearAxisConfig.PulsePin, PinValue.High)) .Do(_ => pulseHighTimestamps.Add(stopwatch.ElapsedTicks)); - - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); - + + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); + // Act var motionTask = controller.MoveInchesAsync(10.0, 100); // Long move at 100 RPM - + // Wait for motion to reach constant speed phase (past acceleration) await Task.Delay(200); - + // Record when stop was called stopCalledTimestamp = stopwatch.ElapsedTicks; await controller.StopAsync(); - + // Wait for motion to complete gracefully await motionTask; stopwatch.Stop(); - + // Assert - Verify deceleration pattern // Calculate delays between consecutive pulses (time between consecutive HIGH pulses) var delays = new List(); @@ -426,20 +441,20 @@ public async Task StopAsync_ShouldTriggerGracefulDeceleration_InsteadOfAbruptSto { delays.Add(pulseHighTimestamps[i] - pulseHighTimestamps[i - 1]); } - + // Find pulses that occurred after StopAsync was called var pulsesAfterStop = pulseHighTimestamps.Count(t => t >= stopCalledTimestamp); - + // Verify that motion continued with deceleration steps after stop - Assert.True(pulsesAfterStop > 5, + Assert.True(pulsesAfterStop > 5, $"Expected deceleration steps after StopAsync(), but only {pulsesAfterStop} pulses occurred after stop"); - + // Get the delays in the deceleration phase (last 30% of pulses after stop) var pulseIndexAtStop = pulseHighTimestamps.FindIndex(t => t >= stopCalledTimestamp); if (pulseIndexAtStop >= 0 && pulseIndexAtStop < delays.Count - 5) { var decelPhaseDelays = delays.Skip(pulseIndexAtStop).TakeLast(Math.Min(10, pulsesAfterStop - 1)).ToList(); - + // Verify deceleration: delays should generally increase (motor slowing down) // At least 60% of consecutive delay pairs should show increase int increasingDelayCount = 0; @@ -448,18 +463,18 @@ public async Task StopAsync_ShouldTriggerGracefulDeceleration_InsteadOfAbruptSto if (decelPhaseDelays[i] > decelPhaseDelays[i - 1]) increasingDelayCount++; } - - double decelerationRatio = decelPhaseDelays.Count > 1 - ? (double)increasingDelayCount / (decelPhaseDelays.Count - 1) + + double decelerationRatio = decelPhaseDelays.Count > 1 + ? (double)increasingDelayCount / (decelPhaseDelays.Count - 1) : 0; - - Assert.True(decelerationRatio > 0.5, + + Assert.True(decelerationRatio > 0.5, $"Expected increasing delays during deceleration, but only {decelerationRatio:P0} of delays increased. " + $"This indicates an abrupt stop rather than gradual deceleration."); } - + // Verify motion completed without throwing OperationCanceledException - Assert.True(motionTask.IsCompletedSuccessfully, + Assert.True(motionTask.IsCompletedSuccessfully, "Motion should complete gracefully without throwing OperationCanceledException when stopped via StopAsync()"); } @@ -484,7 +499,7 @@ public async Task HomeAsync_ShouldSetDirectionHigh() } // Assert - _mockGpio.Received().Write(_config.DirectionPin, PinValue.High); + _mockGpio.Received().Write(_config.LinearAxisConfig.DirectionPin, PinValue.High); } [Fact] @@ -495,23 +510,23 @@ public async Task HomeAsync_ShouldResetPositionToZero_WhenLimitReached() var callCount = 0; PinChangeEventHandler? minLimitCallback = null; - mockGpio.Read(_config.MinLimitSwitchPin).Returns(x => + mockGpio.Read(_config.LinearAxisConfig.MinLimitSwitchPin).Returns(x => { callCount++; return callCount > 5 ? PinValue.Low : PinValue.High; }); - mockGpio.Read(_config.MaxLimitSwitchPin).Returns(PinValue.High); + mockGpio.Read(_config.LinearAxisConfig.MaxLimitSwitchPin).Returns(PinValue.High); mockGpio.When(x => x.RegisterCallbackForPinValueChangedEvent( - _config.MinLimitSwitchPin, + _config.LinearAxisConfig.MinLimitSwitchPin, Arg.Any(), Arg.Any())) .Do(callInfo => minLimitCallback = callInfo.ArgAt(2)); - using var controller = new StepperMotorController(mockGpio, _config, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, _config, CreateMockLogger()); // Simulate limit switch trigger - var eventArgs = new PinValueChangedEventArgs(PinEventTypes.Falling, _config.MinLimitSwitchPin); + var eventArgs = new PinValueChangedEventArgs(PinEventTypes.Falling, _config.LinearAxisConfig.MinLimitSwitchPin); minLimitCallback?.Invoke(null, eventArgs); // Act @@ -530,19 +545,22 @@ public async Task HomeAsync_ShouldResetPositionToZero_WhenLimitReached() public async Task ResetPositionAsync_ShouldSetPositionToZero() { // Arrange - Move to a non-zero position first - var shortConfig = new LinearAxisConfig + var shortConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 50000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, shortConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, shortConfig, CreateMockLogger()); await controller.MoveInchesAsync(0.5, 60); Assert.NotEqual(0.0, controller.CurrentPositionInches); @@ -562,7 +580,7 @@ public async Task ResetPositionAsync_ShouldSetPositionToZero() public void Dispose_ShouldDisposeGpioController() { // Arrange - var controller = new StepperMotorController(_mockGpio, _config, CreateMockLogger()); + var controller = new SynchronizedDualAxisController(_mockGpio, _config, CreateMockLogger()); // Act controller.Dispose(); @@ -575,7 +593,7 @@ public void Dispose_ShouldDisposeGpioController() public void Dispose_ShouldBeIdempotent() { // Arrange - var controller = new StepperMotorController(_mockGpio, _config, CreateMockLogger()); + var controller = new SynchronizedDualAxisController(_mockGpio, _config, CreateMockLogger()); // Act controller.Dispose(); @@ -594,19 +612,22 @@ public void Dispose_ShouldBeIdempotent() public async Task CompleteWorkflow_ShouldWorkCorrectly() { // Arrange - var shortConfig = new LinearAxisConfig + var shortConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 50000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 50000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, shortConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, shortConfig, CreateMockLogger()); // Act - Move forward await controller.MoveInchesAsync(0.5, 60); @@ -634,33 +655,35 @@ public async Task CompleteWorkflow_ShouldWorkCorrectly() public async Task ExecuteMotionAsync_ShouldCalculateCorrectAccelerationSteps() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 // steps/sec^2 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 // steps/sec^2 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); double rpm = 60; // 60 RPM - double maxStepsPerSecond = (rpm * (int)testConfig.StepsPerRevolution) / 60.0; // 400 steps/sec - int expectedAccelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.Acceleration)); // (400*400)/(2*5000) = 16 steps + double maxStepsPerSecond = (rpm * (int)testConfig.LinearAxisConfig.StepsPerRevolution) / 60.0; // 400 steps/sec + int expectedAccelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.LinearAxisConfig.Acceleration)); // (400*400)/(2*5000) = 16 steps int totalSteps = 100; // Act - await controller.MoveInchesAsync(totalSteps / ((int)testConfig.StepsPerRevolution * testConfig.LeadScrewThreadsPerInch), rpm); + await controller.MoveInchesAsync(totalSteps / ((int)testConfig.LinearAxisConfig.StepsPerRevolution * testConfig.LinearAxisConfig.LeadScrewThreadsPerInch), rpm); // Assert - Verify we pulsed the correct total number of steps - mockGpio.Received(totalSteps).Write(testConfig.PulsePin, PinValue.High); - mockGpio.Received(totalSteps).Write(testConfig.PulsePin, PinValue.Low); - + mockGpio.Received(totalSteps).Write(testConfig.LinearAxisConfig.PulsePin, PinValue.High); + mockGpio.Received(totalSteps).Write(testConfig.LinearAxisConfig.PulsePin, PinValue.Low); // Expected accel steps should be 16 for this configuration Assert.Equal(16, expectedAccelSteps); } @@ -669,22 +692,26 @@ public async Task ExecuteMotionAsync_ShouldCalculateCorrectAccelerationSteps() public async Task ExecuteMotionAsync_ShouldCalculateCorrectInitialDelay() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; + var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); // David Austin formula: c0 = 0.676 * sqrt(2/a) * 10^6 - double expectedInitialDelay = 0.676 * Math.Sqrt(2.0 / testConfig.Acceleration) * 1_000_000.0; + double expectedInitialDelay = 0.676 * Math.Sqrt(2.0 / testConfig.LinearAxisConfig.Acceleration) * 1_000_000.0; // = 0.676 * sqrt(0.0004) * 1,000,000 = 0.676 * 0.02 * 1,000,000 = 13,520 microseconds // Act - move a small distance to trigger acceleration @@ -726,23 +753,26 @@ public async Task ExecuteMotionAsync_ShouldUseCorrectDelayFormula() public async Task ExecuteMotionAsync_ShouldHandleShortMoves() { // Arrange - Create config where accel+decel would exceed total steps - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); double rpm = 60; - double maxStepsPerSecond = (rpm * (int)testConfig.StepsPerRevolution) / 60.0; - int fullAccelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.Acceleration)); // 16 steps + double maxStepsPerSecond = (rpm * (int)testConfig.LinearAxisConfig.StepsPerRevolution) / 60.0; + int fullAccelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.LinearAxisConfig.Acceleration)); // 16 steps int totalSteps = 10; // Less than full accel+decel (16+16=32) @@ -751,10 +781,10 @@ public async Task ExecuteMotionAsync_ShouldHandleShortMoves() int expectedDecelSteps = totalSteps - expectedAccelSteps; // 5 // Act - await controller.MoveInchesAsync(totalSteps / ((int)testConfig.StepsPerRevolution * testConfig.LeadScrewThreadsPerInch), rpm); + await controller.MoveInchesAsync(totalSteps / ((int)testConfig.LinearAxisConfig.StepsPerRevolution * testConfig.LinearAxisConfig.LeadScrewThreadsPerInch), rpm); // Assert - Should still pulse correct number of times - mockGpio.Received(totalSteps).Write(testConfig.PulsePin, PinValue.High); + mockGpio.Received(totalSteps).Write(testConfig.LinearAxisConfig.PulsePin, PinValue.High); Assert.Equal(5, expectedAccelSteps); Assert.Equal(5, expectedDecelSteps); } @@ -763,28 +793,31 @@ public async Task ExecuteMotionAsync_ShouldHandleShortMoves() public async Task ExecuteMotionAsync_ShouldReachTargetSpeed() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); double rpm = 60; - double maxStepsPerSecond = (rpm * (int)testConfig.StepsPerRevolution) / 60.0; // 400 steps/sec + double maxStepsPerSecond = (rpm * (int)testConfig.LinearAxisConfig.StepsPerRevolution) / 60.0; // 400 steps/sec double targetDelayMicroseconds = 1_000_000.0 / maxStepsPerSecond; // 2500 microseconds int totalSteps = 100; // Enough steps for full accel, constant speed, and decel // Act - await controller.MoveInchesAsync(totalSteps / ((int)testConfig.StepsPerRevolution * testConfig.LeadScrewThreadsPerInch), rpm); + await controller.MoveInchesAsync(totalSteps / ((int)testConfig.LinearAxisConfig.StepsPerRevolution * testConfig.LinearAxisConfig.LeadScrewThreadsPerInch), rpm); // Assert - Verify target delay calculation Assert.Equal(2500.0, targetDelayMicroseconds, 0.1); @@ -794,29 +827,32 @@ public async Task ExecuteMotionAsync_ShouldReachTargetSpeed() public async Task ExecuteMotionAsync_ShouldSymmetricallyAccelerateAndDecelerate() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); double rpm = 60; - double maxStepsPerSecond = (rpm * (int)testConfig.StepsPerRevolution) / 60.0; - int accelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.Acceleration)); + double maxStepsPerSecond = (rpm * (int)testConfig.LinearAxisConfig.StepsPerRevolution) / 60.0; + int accelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.LinearAxisConfig.Acceleration)); int decelSteps = accelSteps; // Should be symmetric int totalSteps = 100; // Act - await controller.MoveInchesAsync(totalSteps / ((int)testConfig.StepsPerRevolution * testConfig.LeadScrewThreadsPerInch), rpm); + await controller.MoveInchesAsync(totalSteps / ((int)testConfig.LinearAxisConfig.StepsPerRevolution * testConfig.LinearAxisConfig.LeadScrewThreadsPerInch), rpm); // Assert - Verify symmetry Assert.Equal(accelSteps, decelSteps); @@ -859,27 +895,30 @@ public async Task ExecuteMotionAsync_ShouldCalculateDecelerationCorrectly() public async Task ExecuteMotionAsync_ShouldCalculateCorrectlyForDifferentConfigurations(double rpm, StepsPerRevolution stepsPerRev) { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = stepsPerRev, - LeadScrewThreadsPerInch = 5.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = stepsPerRev, + LeadScrewThreadsPerInch = 5.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); double maxStepsPerSecond = (rpm * (int)stepsPerRev) / 60.0; - int expectedAccelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.Acceleration)); + int expectedAccelSteps = (int)((maxStepsPerSecond * maxStepsPerSecond) / (2 * testConfig.LinearAxisConfig.Acceleration)); double expectedTargetDelay = 1_000_000.0 / maxStepsPerSecond; // Act - move enough steps for full profile int totalSteps = Math.Max(expectedAccelSteps * 3, 50); - await controller.MoveInchesAsync(totalSteps / ((int)testConfig.StepsPerRevolution * testConfig.LeadScrewThreadsPerInch), rpm); + await controller.MoveInchesAsync(totalSteps / ((int)testConfig.LinearAxisConfig.StepsPerRevolution * testConfig.LinearAxisConfig.LeadScrewThreadsPerInch), rpm); // Assert - calculations should be consistent Assert.True(expectedAccelSteps > 0); @@ -960,29 +999,32 @@ public void SetTargetRpm_ShouldAcceptVariousValidRpmValues(double rpm) public async Task SetTargetRpm_ShouldChangeSpeedDuringMotion() { // Arrange - Create a config with high acceleration for faster test - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 10000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 10000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); // Act - Start motion in a background task var motionTask = Task.Run(async () => await controller.MoveInchesAsync(2.0, 60)); - + // Wait a bit for motion to start and reach constant speed await Task.Delay(50); - + // Change speed during motion controller.SetTargetSpeed(120); - + // Wait for motion to complete await motionTask; @@ -999,31 +1041,33 @@ public async Task SetTargetRpm_ShouldChangeSpeedDuringMotion() public async Task SetTargetRpm_ShouldRecalculateDecelerationSteps() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); var mockLogger = CreateMockLogger(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, mockLogger); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, mockLogger); // Calculate expected deceleration steps for different speeds double initialRpm = 60; double newRpm = 120; - - double initialMaxStepsPerSec = (initialRpm * (int)testConfig.StepsPerRevolution) / 60.0; - double newMaxStepsPerSec = (newRpm * (int)testConfig.StepsPerRevolution) / 60.0; - - int expectedInitialDecelSteps = (int)((initialMaxStepsPerSec * initialMaxStepsPerSec) / (2.0 * testConfig.Acceleration)); - int expectedNewDecelSteps = (int)((newMaxStepsPerSec * newMaxStepsPerSec) / (2.0 * testConfig.Acceleration)); + double initialMaxStepsPerSec = (initialRpm * (int)testConfig.LinearAxisConfig.StepsPerRevolution) / 60.0; + double newMaxStepsPerSec = (newRpm * (int)testConfig.LinearAxisConfig.StepsPerRevolution) / 60.0; + + int expectedInitialDecelSteps = (int)((initialMaxStepsPerSec * initialMaxStepsPerSec) / (2.0 * testConfig.LinearAxisConfig.Acceleration)); + int expectedNewDecelSteps = (int)((newMaxStepsPerSec * newMaxStepsPerSec) / (2.0 * testConfig.LinearAxisConfig.Acceleration)); // Act - Start motion and change speed var motionTask = Task.Run(async () => await controller.MoveInchesAsync(2.0, initialRpm)); await Task.Delay(50); @@ -1044,20 +1088,23 @@ public async Task SetTargetRpm_ShouldRecalculateDecelerationSteps() public async Task SetTargetRpm_ShouldNotAffectAccelerationPhase() { // Arrange - Very short motion so most of it is acceleration/deceleration - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 10.0, - Acceleration = 5000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 10.0, + Acceleration = 5000.0 + } }; var mockGpio = Substitute.For(); var mockLogger = CreateMockLogger(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, mockLogger); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, mockLogger); // Act - Start very short motion and immediately try to change speed var motionTask = Task.Run(async () => await controller.MoveInchesAsync(0.05, 60)); @@ -1084,35 +1131,35 @@ public async Task SetTargetRpm_ShouldChangeSpeedDuringRunToLimitSwitch() PinChangeEventHandler? minLimitCallback = null; // Simulate limit switch triggering after some steps - mockGpio.Read(_config.MinLimitSwitchPin).Returns(x => + mockGpio.Read(_config.LinearAxisConfig.MinLimitSwitchPin).Returns(x => { callCount++; return callCount > 100 ? PinValue.Low : PinValue.High; }); - mockGpio.Read(_config.MaxLimitSwitchPin).Returns(PinValue.High); + mockGpio.Read(_config.LinearAxisConfig.MaxLimitSwitchPin).Returns(PinValue.High); mockGpio.When(x => x.RegisterCallbackForPinValueChangedEvent( - _config.MinLimitSwitchPin, + _config.LinearAxisConfig.MinLimitSwitchPin, Arg.Any(), Arg.Any())) .Do(callInfo => minLimitCallback = callInfo.ArgAt(2)); - using var controller = new StepperMotorController(mockGpio, _config, mockLogger); + using var controller = new SynchronizedDualAxisController(mockGpio, _config, mockLogger); // Act - Start motion to limit switch var motionTask = Task.Run(async () => await controller.RunToLimitSwitchAsync(LimitSwitch.Min, 60)); - + // Wait for motion to start await Task.Delay(50); - + // Change speed during motion controller.SetTargetSpeed(100); - + // Trigger limit switch after speed change await Task.Delay(50); - var eventArgs = new PinValueChangedEventArgs(PinEventTypes.Falling, _config.MinLimitSwitchPin); + var eventArgs = new PinValueChangedEventArgs(PinEventTypes.Falling, _config.LinearAxisConfig.MinLimitSwitchPin); minLimitCallback?.Invoke(null, eventArgs); - + await motionTask; // Assert - Speed change should be logged @@ -1128,23 +1175,26 @@ public async Task SetTargetRpm_ShouldChangeSpeedDuringRunToLimitSwitch() public async Task SetTargetRpm_ShouldBeThreadSafe() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 10000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 10000.0 + } }; var mockGpio = Substitute.For(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, CreateMockLogger()); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, CreateMockLogger()); // Act - Start motion and rapidly change speed from multiple threads var motionTask = Task.Run(async () => await controller.MoveInchesAsync(3.0, 60)); - + var speedChangeTasks = new List(); for (int i = 0; i < 10; i++) { @@ -1163,33 +1213,36 @@ public async Task SetTargetRpm_ShouldBeThreadSafe() public async Task SetTargetRpm_ShouldHandleMultipleSpeedChanges() { // Arrange - var testConfig = new LinearAxisConfig + var testConfig = new SynchronizedDualAxisConfig { - PulsePin = 21, - DirectionPin = 20, - MinLimitSwitchPin = 24, - MaxLimitSwitchPin = 23, - StepsPerRevolution = StepsPerRevolution.SPR_400, - LeadScrewThreadsPerInch = 1.0, - Acceleration = 10000.0 + LinearAxisConfig = new LinearAxisConfig + { + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, + StepsPerRevolution = StepsPerRevolution.SPR_400, + LeadScrewThreadsPerInch = 1.0, + Acceleration = 10000.0 + } }; var mockGpio = Substitute.For(); var mockLogger = CreateMockLogger(); mockGpio.Read(Arg.Any()).Returns(PinValue.High); - using var controller = new StepperMotorController(mockGpio, testConfig, mockLogger); + using var controller = new SynchronizedDualAxisController(mockGpio, testConfig, mockLogger); // Act - Start motion and change speed multiple times var motionTask = Task.Run(async () => await controller.MoveInchesAsync(3.0, 60)); - + await Task.Delay(20); controller.SetTargetSpeed(80); - + await Task.Delay(30); controller.SetTargetSpeed(120); - + await Task.Delay(30); controller.SetTargetSpeed(100); - + await motionTask; // Assert - All speed changes should be logged @@ -1199,14 +1252,14 @@ public async Task SetTargetRpm_ShouldHandleMultipleSpeedChanges() Arg.Is(o => o.ToString()!.Contains("Target RPM updated to 80")), Arg.Any(), Arg.Any>()); - + mockLogger.Received(1).Log( LogLevel.Information, Arg.Any(), Arg.Is(o => o.ToString()!.Contains("Target RPM updated to 120")), Arg.Any(), Arg.Any>()); - + mockLogger.Received(1).Log( LogLevel.Information, Arg.Any(), @@ -1319,12 +1372,17 @@ public void Constructor_ShouldOpenEnablePins_WhenConfigured() { LinearAxisConfig = new LinearAxisConfig { - PulsePin = 21, DirectionPin = 20, EnablePin = 19, - MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23 + PulsePin = 21, + DirectionPin = 20, + EnablePin = 19, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23 }, RotaryAxisConfig = new RotaryAxisConfig { - PulsePin = 16, DirectionPin = 12, EnablePin = 13 + PulsePin = 16, + DirectionPin = 12, + EnablePin = 13 }, GearRatio = 1.0 }; @@ -1424,7 +1482,10 @@ public async Task MoveInchesAsync_ShouldUpdateLinearPosition() { LinearAxisConfig = new LinearAxisConfig { - PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, StepsPerRevolution = StepsPerRevolution.SPR_400, LeadScrewThreadsPerInch = 1.0, Acceleration = 50000.0 @@ -1453,14 +1514,18 @@ public async Task MoveInchesAsync_RotaryPulseCount_ShouldMatchGearRatioScaling_O { LinearAxisConfig = new LinearAxisConfig { - PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, StepsPerRevolution = StepsPerRevolution.SPR_400, LeadScrewThreadsPerInch = 1.0, Acceleration = 50000.0 }, RotaryAxisConfig = new RotaryAxisConfig { - PulsePin = 16, DirectionPin = 12, + PulsePin = 16, + DirectionPin = 12, StepsPerRevolution = StepsPerRevolution.SPR_400 }, GearRatio = 1.0 @@ -1486,14 +1551,18 @@ public async Task MoveInchesAsync_RotaryPulseCount_ShouldScaleByGearRatio() { LinearAxisConfig = new LinearAxisConfig { - PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, StepsPerRevolution = StepsPerRevolution.SPR_400, LeadScrewThreadsPerInch = 1.0, Acceleration = 50000.0 }, RotaryAxisConfig = new RotaryAxisConfig { - PulsePin = 16, DirectionPin = 12, + PulsePin = 16, + DirectionPin = 12, StepsPerRevolution = StepsPerRevolution.SPR_400 }, GearRatio = 0.5 @@ -1517,14 +1586,18 @@ public async Task MoveInchesAsync_RotaryPositionDegrees_ShouldBeNonZeroAfterMove { LinearAxisConfig = new LinearAxisConfig { - PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, StepsPerRevolution = StepsPerRevolution.SPR_400, LeadScrewThreadsPerInch = 1.0, Acceleration = 50000.0 }, RotaryAxisConfig = new RotaryAxisConfig { - PulsePin = 16, DirectionPin = 12, + PulsePin = 16, + DirectionPin = 12, StepsPerRevolution = StepsPerRevolution.SPR_400 }, GearRatio = 1.0 @@ -1608,7 +1681,10 @@ public async Task ResetPositionAsync_ShouldSetLinearPositionToZero() { LinearAxisConfig = new LinearAxisConfig { - PulsePin = 21, DirectionPin = 20, MinLimitSwitchPin = 24, MaxLimitSwitchPin = 23, + PulsePin = 21, + DirectionPin = 20, + MinLimitSwitchPin = 24, + MaxLimitSwitchPin = 23, StepsPerRevolution = StepsPerRevolution.SPR_400, LeadScrewThreadsPerInch = 1.0, Acceleration = 50000.0 From 1cfe9c1929df0138308b550d1fd45bfa966649bc Mon Sep 17 00:00:00 2001 From: Troye Stonich Date: Fri, 1 May 2026 19:07:31 -0400 Subject: [PATCH 5/5] u --- MotorController/SynchronizedDualAxisController.cs | 7 ++++++- Test/StepperMotorControllerTests.cs | 4 ++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/MotorController/SynchronizedDualAxisController.cs b/MotorController/SynchronizedDualAxisController.cs index 21a378f..fb8f7e3 100644 --- a/MotorController/SynchronizedDualAxisController.cs +++ b/MotorController/SynchronizedDualAxisController.cs @@ -335,9 +335,14 @@ private void SetLinearDirection(bool forward) _gpio.Write(_config.LinearAxisConfig.DirectionPin, forward ? PinValue.Low : PinValue.High); } + /// + /// Sets the rotary direction pin based on the desired linear direction and gear ratio. + /// Note: the rotary direction is inverted relative to the linear direction. + /// + /// private void SetRotaryDirection(bool forward) { - _gpio.Write(_config.RotaryAxisConfig.DirectionPin, forward ? PinValue.Low : PinValue.High); + _gpio.Write(_config.RotaryAxisConfig.DirectionPin, forward ? PinValue.High : PinValue.Low); } private void EnableMotors() diff --git a/Test/StepperMotorControllerTests.cs b/Test/StepperMotorControllerTests.cs index 1ec18dc..b6f27b3 100644 --- a/Test/StepperMotorControllerTests.cs +++ b/Test/StepperMotorControllerTests.cs @@ -1457,14 +1457,14 @@ public async Task MoveInchesAsync_ShouldSetLinearDirectionHigh_WhenMovingNegativ public async Task MoveInchesAsync_ShouldSetRotaryDirectionLow_WhenMovingPositive() { await _controller.MoveInchesAsync(0.1, 60); - _mockGpio.Received().Write(_config.RotaryAxisConfig.DirectionPin, PinValue.Low); + _mockGpio.Received().Write(_config.RotaryAxisConfig.DirectionPin, PinValue.High); } [Fact] public async Task MoveInchesAsync_ShouldSetRotaryDirectionHigh_WhenMovingNegative() { await _controller.MoveInchesAsync(-0.1, 60); - _mockGpio.Received().Write(_config.RotaryAxisConfig.DirectionPin, PinValue.High); + _mockGpio.Received().Write(_config.RotaryAxisConfig.DirectionPin, PinValue.Low); } [Fact]