diff --git a/.bonsai/Bonsai.config b/.bonsai/Bonsai.config index ab4ecd0..7e2921c 100644 --- a/.bonsai/Bonsai.config +++ b/.bonsai/Bonsai.config @@ -2,45 +2,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -48,9 +93,12 @@ + + + @@ -60,6 +108,7 @@ + @@ -70,6 +119,8 @@ + + diff --git a/OpenEphys.Commutator/AutoCommutator.bonsai b/OpenEphys.Commutator/AutoCommutator.bonsai new file mode 100644 index 0000000..ffe3127 --- /dev/null +++ b/OpenEphys.Commutator/AutoCommutator.bonsai @@ -0,0 +1,113 @@ + + + Control an Open Ephys commutator by using rotation angle measurements. + + + + Source1 + + + + PT0.1S + + + + + + + + + 0 + 0 + 1 + + + + + {{turn : {0}}} + it + + + + + + + false + + + + + + + Source1 + + + + + + + + + + + {led: true} + + + + + + + Source1 + + + + + + + + + + + + + {led: false} + + + + + + + + + + + + \r\n + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/OpenEphys.Commutator/OpenEphys.Commutator.csproj b/OpenEphys.Commutator/OpenEphys.Commutator.csproj index 26d1d82..5e7a039 100644 --- a/OpenEphys.Commutator/OpenEphys.Commutator.csproj +++ b/OpenEphys.Commutator/OpenEphys.Commutator.csproj @@ -7,6 +7,10 @@ net472 + + + + diff --git a/OpenEphys.Commutator/QuaternionToTwist.cs b/OpenEphys.Commutator/QuaternionToTwist.cs new file mode 100644 index 0000000..f16d504 --- /dev/null +++ b/OpenEphys.Commutator/QuaternionToTwist.cs @@ -0,0 +1,65 @@ +using System; +using System.ComponentModel; +using System.Linq; +using System.Numerics; +using System.Reactive.Linq; +using Bonsai; + +namespace OpenEphys.Commutator +{ + /// + /// Calculates a the rotation about a specified axis (the "twist") that has occurred between successive 3D + /// rotation measurements. + /// + [Description("Calculates a feedback control signal to compensate for twisting about the specified axis, in units of turns.")] + public class QuaternionToTwist : Combinator + { + /// + /// Gets or sets the direction vector specifying the axis around which to calculate the twist. + /// + /// + /// This vector should point, using the reference frame of the device producing rotation measurements, + /// in the direction that the tether exits the headstage. Note that negating this vector will result in + /// negating the direction of twisting. + /// + [TypeConverter(typeof(NumericRecordConverter))] + [Description("The direction vector specifying the axis around which to calculate the twist.")] + public Vector3 RotationAxis { get; set; } = Vector3.UnitZ; + + /// + /// Calculates a twist about that has occurred between successive rotation + /// measurements provided by the input sequence. + /// + /// The sequence of rotation measurements. + /// The sequence of twist values, in units of turns. + public override IObservable Process(IObservable source) + { + return Observable.Defer(() => + { + double? previousTwist = default; + return source.Select(rotation => + { + // project rotation axis onto the direction axis + var direction = RotationAxis; + var rotationAxis = new Vector3(rotation.X, rotation.Y, rotation.Z); + var dotProduct = Vector3.Dot(rotationAxis, direction); + var projection = dotProduct / Vector3.Dot(direction, direction) * direction; + var twist = new Quaternion(projection, rotation.W); + twist = Quaternion.Normalize(twist); + if (dotProduct < 0) // account for angle-axis flipping + { + twist = -twist; + } + + // normalize twist feedback in units of turns + var twistAngle = 2 * Math.Acos(twist.W); + var feedback = previousTwist.HasValue + ? (twistAngle - previousTwist.GetValueOrDefault() + 3 * Math.PI) % (2 * Math.PI) - Math.PI + : 0; + previousTwist = twistAngle; + return -feedback / (2 * Math.PI); + }); + }); + } + } +}