Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added EM542s stepper motor #825

Merged
merged 4 commits into from
Oct 31, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
using Meadow;
using Meadow.Devices;
using Meadow.Foundation.Sensors.Rotary;
using Meadow.Peripherals;
using Meadow.Peripherals.Sensors.Rotary;
using System.Threading.Tasks;

Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
using Meadow;
using Meadow.Devices;
using Meadow.Foundation.Sensors.Rotary;
using Meadow.Peripherals;
using Meadow.Peripherals.Sensors.Rotary;
using System.Threading.Tasks;

Expand Down Expand Up @@ -37,7 +38,7 @@ public override Task Initialize()
return Task.CompletedTask;
}

void RotaryEncoder_Rotated(object sender, RotaryChangeResult e)
private void RotaryEncoder_Rotated(object sender, RotaryChangeResult e)
{
switch (e.New)
{
Expand Down
238 changes: 119 additions & 119 deletions Source/Meadow.Foundation.Core/Sensors/Rotary/RotaryEncoder.cs
Original file line number Diff line number Diff line change
@@ -1,142 +1,142 @@
using Meadow.Hardware;
using Meadow.Peripherals;
using Meadow.Peripherals.Sensors.Rotary;
using System;

namespace Meadow.Foundation.Sensors.Rotary
namespace Meadow.Foundation.Sensors.Rotary;

/// <summary>
/// Digital rotary encoder that uses two-bit Gray Code to encode rotation.
/// </summary>
public class RotaryEncoder : IRotaryEncoder
{
/// <summary>
/// Digital rotary encoder that uses two-bit Gray Code to encode rotation.
/// Raised when the rotary encoder is rotated and returns a RotaryTurnedEventArgs object which describes the direction of rotation.
/// </summary>
public class RotaryEncoder : IRotaryEncoder
{
/// <summary>
/// Raised when the rotary encoder is rotated and returns a RotaryTurnedEventArgs object which describes the direction of rotation.
/// </summary>
public event EventHandler<RotaryChangeResult> Rotated = delegate { };

/// <summary>
/// Returns the pin connected to the A-phase output on the rotary encoder.
/// </summary>
protected IDigitalInterruptPort APhasePort { get; }

/// <summary>
/// Returns the pin connected to the B-phase output on the rotary encoder.
/// </summary>
protected IDigitalInterruptPort BPhasePort { get; }

/// <summary>
/// Gets the last direction of rotation
/// </summary>
public RotationDirection? LastDirectionOfRotation { get; protected set; }

/// <summary>
/// Contains the previous offset used to find direction information
/// </summary>
private int dynamicOffset = 0;

/// <summary>
/// The rotary encoder has 2 inputs, called A and B. Because of its design
/// either A or B changes but not both on each notification when the encoder
/// is rotated. Because of this the direction to be determined. If A goes
/// High before B then we are rotating one direction if B goes high before A
/// we are rotating the other direction. For each change we must consider the
/// previous state of A and B and the current state of A and B. This can be
/// represented as 4-bit number.
/// 3 2 1 0
/// |old|new|
/// |A|B|A|B|
///
/// Bits 0 and 1 represent the current state of A and B while bits 2 and 3
/// represent previous states of A and B. This 4-bit number yields 16 possible
/// combination, however, there are combination that for which no change is
/// represent. For example, the if bits 0-3 are all 0, this would mean that A
/// was Low and is Low, and that B was Low and is Low (nothing changed.)
/// </summary>
private readonly int[] RotEncLookup = { 0, -1, 1, 0,
1, 0, 0, -1,
-1, 0, 0, 1,
0, 1, -1, 0 };

/// <summary>
/// Instantiate a new RotaryEncoder on the specified pins
/// </summary>
/// <param name="aPhasePin">Pin A</param>
/// <param name="bPhasePin">Pin B</param>
/// <param name="isCommonGround">Do the encode pins use a common ground (true) or common positive (false)</param>
public RotaryEncoder(IPin aPhasePin, IPin bPhasePin, bool isCommonGround = false) :
this(aPhasePin.CreateDigitalInterruptPort(InterruptMode.EdgeBoth, isCommonGround ? ResistorMode.InternalPullUp : ResistorMode.InternalPullDown, TimeSpan.Zero, TimeSpan.FromMilliseconds(0.1)),
bPhasePin.CreateDigitalInterruptPort(InterruptMode.EdgeBoth, isCommonGround ? ResistorMode.InternalPullUp : ResistorMode.InternalPullDown, TimeSpan.Zero, TimeSpan.FromMilliseconds(0.1)))
{ }

/// <summary>
/// Instantiate a new RotaryEncoder on the specified ports
/// </summary>
/// <param name="aPhasePort"></param>
/// <param name="bPhasePort"></param>
public RotaryEncoder(IDigitalInterruptPort aPhasePort, IDigitalInterruptPort bPhasePort)
{
APhasePort = aPhasePort;
BPhasePort = bPhasePort;
public event EventHandler<RotaryChangeResult> Rotated = delegate { };

APhasePort.Changed += PhaseAPinChanged;
BPhasePort.Changed += PhaseBPinChanged;
}
/// <summary>
/// Returns the pin connected to the A-phase output on the rotary encoder.
/// </summary>
protected IDigitalInterruptPort APhasePort { get; }

private void PhaseAPinChanged(object s, DigitalPortResult result)
{
// Clear bit A bit
int new2LsBits = dynamicOffset & 0x02; // Save bit 2 (B)
/// <summary>
/// Returns the pin connected to the B-phase output on the rotary encoder.
/// </summary>
protected IDigitalInterruptPort BPhasePort { get; }

if (result.New.State)
{
new2LsBits |= 0x01; // Set bit 1 (A)
}
/// <summary>
/// Gets the last direction of rotation
/// </summary>
public RotationDirection? LastDirectionOfRotation { get; protected set; }

FindDirection(new2LsBits);
}
/// <summary>
/// Contains the previous offset used to find direction information
/// </summary>
private int dynamicOffset = 0;

private void PhaseBPinChanged(object s, DigitalPortResult result)
{
// Clear bit B bit
int new2LsBits = dynamicOffset & 0x01; // Save bit 1 (A)
/// <summary>
/// The rotary encoder has 2 inputs, called A and B. Because of its design
/// either A or B changes but not both on each notification when the encoder
/// is rotated. Because of this the direction to be determined. If A goes
/// High before B then we are rotating one direction if B goes high before A
/// we are rotating the other direction. For each change we must consider the
/// previous state of A and B and the current state of A and B. This can be
/// represented as 4-bit number.
/// 3 2 1 0
/// |old|new|
/// |A|B|A|B|
///
/// Bits 0 and 1 represent the current state of A and B while bits 2 and 3
/// represent previous states of A and B. This 4-bit number yields 16 possible
/// combination, however, there are combination that for which no change is
/// represent. For example, the if bits 0-3 are all 0, this would mean that A
/// was Low and is Low, and that B was Low and is Low (nothing changed.)
/// </summary>
private readonly int[] RotEncLookup = { 0, -1, 1, 0,
1, 0, 0, -1,
-1, 0, 0, 1,
0, 1, -1, 0 };

if (result.New.State)
{
new2LsBits |= 0x02; // Set bit 2 (B)
}
/// <summary>
/// Instantiate a new RotaryEncoder on the specified pins
/// </summary>
/// <param name="aPhasePin">Pin A</param>
/// <param name="bPhasePin">Pin B</param>
/// <param name="isCommonGround">Do the encode pins use a common ground (true) or common positive (false)</param>
public RotaryEncoder(IPin aPhasePin, IPin bPhasePin, bool isCommonGround = false) :
this(aPhasePin.CreateDigitalInterruptPort(InterruptMode.EdgeBoth, isCommonGround ? ResistorMode.InternalPullUp : ResistorMode.InternalPullDown, TimeSpan.Zero, TimeSpan.FromMilliseconds(0.1)),
bPhasePin.CreateDigitalInterruptPort(InterruptMode.EdgeBoth, isCommonGround ? ResistorMode.InternalPullUp : ResistorMode.InternalPullDown, TimeSpan.Zero, TimeSpan.FromMilliseconds(0.1)))
{ }

FindDirection(new2LsBits);
/// <summary>
/// Instantiate a new RotaryEncoder on the specified ports
/// </summary>
/// <param name="aPhasePort"></param>
/// <param name="bPhasePort"></param>
public RotaryEncoder(IDigitalInterruptPort aPhasePort, IDigitalInterruptPort bPhasePort)
{
APhasePort = aPhasePort;
BPhasePort = bPhasePort;

APhasePort.Changed += PhaseAPinChanged;
BPhasePort.Changed += PhaseBPinChanged;
}

private void PhaseAPinChanged(object s, DigitalPortResult result)
{
// Clear bit A bit
int new2LsBits = dynamicOffset & 0x02; // Save bit 2 (B)

if (result.New.State)
{
new2LsBits |= 0x01; // Set bit 1 (A)
}

private void FindDirection(int new2LsBits)
FindDirection(new2LsBits);
}

private void PhaseBPinChanged(object s, DigitalPortResult result)
{
// Clear bit B bit
int new2LsBits = dynamicOffset & 0x01; // Save bit 1 (A)

if (result.New.State)
{
dynamicOffset <<= 2; // Move previous A & B to bits 2 & 3
dynamicOffset |= new2LsBits; // Set the current A & B states in bits 0 & 1
dynamicOffset &= 0x0000000f; // Save only lowest 4 bits

int direction = RotEncLookup[dynamicOffset];

// save state
var oldRotation = LastDirectionOfRotation;

switch (direction)
{
case 0: // no valid change
return;
case 1: // clockwise
LastDirectionOfRotation = RotationDirection.Clockwise;
RaiseRotatedAndNotify(new RotaryChangeResult(RotationDirection.Clockwise, oldRotation));
break;
default: // counter clockwise
LastDirectionOfRotation = RotationDirection.CounterClockwise;
RaiseRotatedAndNotify(new RotaryChangeResult(RotationDirection.CounterClockwise, oldRotation));
break;
}
new2LsBits |= 0x02; // Set bit 2 (B)
}

private void RaiseRotatedAndNotify(RotaryChangeResult result)
FindDirection(new2LsBits);
}

private void FindDirection(int new2LsBits)
{
dynamicOffset <<= 2; // Move previous A & B to bits 2 & 3
dynamicOffset |= new2LsBits; // Set the current A & B states in bits 0 & 1
dynamicOffset &= 0x0000000f; // Save only lowest 4 bits

int direction = RotEncLookup[dynamicOffset];

// save state
var oldRotation = LastDirectionOfRotation;

switch (direction)
{
Rotated?.Invoke(this, result);
case 0: // no valid change
return;
case 1: // clockwise
LastDirectionOfRotation = RotationDirection.Clockwise;
RaiseRotatedAndNotify(new RotaryChangeResult(RotationDirection.Clockwise, oldRotation));
break;
default: // counter clockwise
LastDirectionOfRotation = RotationDirection.CounterClockwise;
RaiseRotatedAndNotify(new RotaryChangeResult(RotationDirection.CounterClockwise, oldRotation));
break;
}
}

private void RaiseRotatedAndNotify(RotaryChangeResult result)
{
Rotated?.Invoke(this, result);
}
}
Loading