Merge pull request #848 from WildernessLabs/feature/motors

Feature/motors

Source/Meadow.Foundation.Peripherals/Motors.GpioStepper/Driver/CwCcwStepper.cs

@@ -0,0 +1,263 @@
+using Meadow.Hardware;
+using Meadow.Peripherals;
+using Meadow.Units;
+using System;
+using System.Runtime.CompilerServices;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace Meadow.Foundation.Motors.Stepper;
+
+/// <summary>
+/// A stepper motor that uses separate GPIO pulses for clockwise and counter-clockwise movement
+/// </summary>
+public class CwCcwStepper : GpioStepperBase
+{
+    private const int MinimumMicrosecondDelayRequiredByDrive = 5; // per the data sheet
+
+    private readonly IDigitalOutputPort _cwPort;
+    private readonly IDigitalOutputPort _ccwPort;
+    private readonly IDigitalOutputPort? _enablePort;
+    private float _usPerCall;
+    private readonly object _syncRoot = new();
+    private int _positionStep;
+    private bool _stopRequested = false;
+    private bool _isMoving = false;
+
+    /// <summary>
+    /// Gets or sets the minimum step dwell time when motor changes from stationary to moving. Motors with more mass or larger steps require more dwell.
+    /// </summary>
+    public int MinimumStartupDwellMicroseconds { get; set; } = 50;
+
+    /// <summary>
+    /// Gets or sets a constant that affects the rate of linear acceleration for the motor. A lower value yields faster acceleration.
+    /// Motors with more mass or larger steps require slower acceleration
+    /// </summary>
+    public int LinearAccelerationConstant { get; set; } = 40;
+
+    /// <inheritdoc/>
+    public override int StepsPerRevolution { get; }
+
+    /// <summary>
+    /// Gets a value indicating whether or not the logic for the stepper motor driver is inverted.
+    /// </summary>
+    /// <remarks>
+    /// "Normal" is step-on-rising-edge, "Inverse" is step-on-falling-edge of the step port
+    /// </remarks>
+    public bool InverseLogic { get; }
+
+    /// <inheritdoc/>
+    public override bool IsMoving => _isMoving;
+
+    /// <inheritdoc/>
+    public override Angle Position => new Angle(_positionStep * (360d / StepsPerRevolution), Angle.UnitType.Degrees);
+
+    /// <inheritdoc/>
+    public override AngularVelocity MaxVelocity { get; }
+
+    /// <inheritdoc/>
+    public override Task ResetPosition(CancellationToken cancellationToken = default)
+    {
+        if (IsMoving) throw new Exception("Cannot reset position while the motor is moving.");
+
+        _positionStep = 0;
+
+        return Task.CompletedTask;
+    }
+
+    /// <summary>
+    /// Initializes a new instance of the <see cref="StepDirStepper"/> class with the specified parameters.
+    /// </summary>
+    /// <param name="clockwisePort">The digital output port for controlling the clockwise signal.</param>
+    /// <param name="counterclockwisePort">The digital output port for controlling the clockwise signal.</param>
+    /// <param name="enable">The optional digital output port for enabling or disabling the motor (if available).</param>
+    /// <param name="stepsPerRevolution">The number of steps per revolution for the stepper motor (default is 200).</param>
+    /// <param name="inverseLogic">A value indicating whether the logic for the stepper motor driver is inverted (default is false).</param>
+    public CwCcwStepper(
+        IDigitalOutputPort clockwisePort,
+        IDigitalOutputPort counterclockwisePort,
+        IDigitalOutputPort? enable = null,
+        int stepsPerRevolution = 200,
+        bool inverseLogic = false
+        )
+    {
+        StepsPerRevolution = stepsPerRevolution;
+        MaxVelocity = new AngularVelocity(1 / (StepsPerRevolution * 0.0000010), AngularVelocity.UnitType.RevolutionsPerSecond); // 10us per step minimum speed
+
+        InverseLogic = inverseLogic;
+
+        _cwPort = clockwisePort;
+        _ccwPort = counterclockwisePort;
+        _enablePort = enable;
+
+        _cwPort.State = InverseLogic;
+        _ccwPort.State = InverseLogic;
+
+        if (_enablePort != null)
+        {
+            _enablePort.State = false;
+        }
+
+        CalculateCallDuration();
+    }
+
+    [MethodImpl(MethodImplOptions.NoOptimization | MethodImplOptions.NoInlining)]
+    private void CalculateCallDuration()
+    {
+        // this estimates how long a method call takes on the current platform.
+        // this is used below to provide a sub-millisecond "delay" used for step dwell
+        var temp = 0;
+        var calls = 100000;
+
+        var start = Environment.TickCount;
+
+        for (var i = 0; i < calls; i++)
+        {
+            temp = i;
+        }
+
+        var et = Environment.TickCount - start;
+
+        _usPerCall = et * 1000 / (float)calls;
+    }
+
+    [MethodImpl(MethodImplOptions.NoOptimization | MethodImplOptions.NoInlining)]
+    private void MicrosecondSleep(int microseconds)
+    {
+        var temp = 0;
+
+        for (var i = 0; i < microseconds / _usPerCall; i++)
+        {
+            temp = i;
+        }
+    }
+
+    /// <inheritdoc/>
+    public override Task Rotate(int steps, RotationDirection direction, Frequency rate, CancellationToken cancellationToken = default)
+    {
+        if (steps < -1) throw new ArgumentOutOfRangeException(nameof(steps));
+
+        Resolver.Log.Info($"Moving {steps} steps at {rate.Hertz}Hz");
+
+        // usPerCall is calculated async at startup.  This loop is to make sure it's calculated before the first Rotate is run
+        while (_usPerCall == 0)
+        {
+            Thread.Sleep(10);
+        }
+
+        if (steps == 0)
+        {
+            Resolver.Log.Info($"no move required");
+            return Task.CompletedTask;
+        }
+
+        return Task.Run(() =>
+        {
+            lock (_syncRoot)
+            {
+                _isMoving = true;
+
+                try
+                {
+                    var directionState = direction == RotationDirection.Clockwise;
+                    if (InverseLogic) directionState = !directionState;
+                    _ccwPort.State = directionState;
+
+                    if (_enablePort != null)
+                    {
+                        _enablePort.State = !InverseLogic;
+                    }
+
+                    // pull both low
+                    _cwPort.State = _ccwPort.State = InverseLogic;
+
+                    var stepPort = direction == RotationDirection.Clockwise ? _cwPort : _ccwPort;
+
+                    var targetus = (int)(1000000d / rate.Hertz);
+
+                    if (targetus < MinimumMicrosecondDelayRequiredByDrive) throw new ArgumentOutOfRangeException(
+                        "Rate cannot have pulses shorter than 5us. Use 200KHz or less.");
+
+                    var us = targetus < MinimumStartupDwellMicroseconds ? MinimumStartupDwellMicroseconds : targetus;
+
+                    int step;
+
+                    var infiniteRun = steps == -1;
+                    if (infiniteRun)
+                    {
+                        steps = 10;
+                    }
+
+                    for (step = 0; step < steps; step++)
+                    {
+                        if (cancellationToken.IsCancellationRequested || _stopRequested)
+                        {
+                            break;
+                        }
+
+                        stepPort.State = InverseLogic; // low means "step"
+
+                        MicrosecondSleep(us);
+
+                        stepPort.State = !InverseLogic;
+
+                        MicrosecondSleep(us);
+
+                        // DEV NOTE:
+                        // naive linear acceleration tested only with STP-MTR-34066 motor
+                        if (us > targetus && step % LinearAccelerationConstant == 0)
+                        {
+                            us--;
+                        }
+
+                        if (direction == RotationDirection.Clockwise)
+                        {
+                            _positionStep++;
+
+                            if (_positionStep > StepsPerRevolution)
+                            {
+                                _positionStep -= StepsPerRevolution;
+                            }
+                        }
+                        else
+                        {
+                            _positionStep--;
+
+                            if (_positionStep < 0)
+                            {
+                                _positionStep += StepsPerRevolution;
+                            }
+                        }
+
+                        if (infiniteRun) step = 0;
+                    }
+
+                    _cwPort.State = _ccwPort.State = InverseLogic;
+
+                    if (_enablePort != null)
+                    {
+                        _enablePort.State = !InverseLogic;
+                    }
+
+                    return Task.CompletedTask;
+                }
+                finally
+                {
+                    _stopRequested = false;
+                    _isMoving = false;
+                }
+            }
+        });
+    }
+
+    /// <inheritdoc/>
+    public override Task Stop(CancellationToken cancellationToken = default)
+    {
+        if (IsMoving)
+        {
+            _stopRequested = true;
+        }
+
+        return Task.CompletedTask;
+    }
+}