/// <summary> /// Crates a new <see cref="BaseStringGlarduinoClient"/>. /// </summary> /// <param name="connectionInfo">The connection information for the port.</param> /// <param name="messageDeserializer">The message deserialization strategy.</param> /// <param name="messageDispatcher">The message dispatching strategy.</param> /// <param name="comPort">The specified communication port.</param> public BaseStringGlarduinoClient(ArduinoPortConnectionInfo connectionInfo, IMessageDeserializerStrategy <string> messageDeserializer, IMessageDispatchingStrategy <string> messageDispatcher, ICommunicationPort comPort) : base(connectionInfo, messageDeserializer, messageDispatcher, comPort) { }
/// <inheritdoc /> protected BaseUnityGlarduinoClient(ArduinoPortConnectionInfo connectionInfo, IMessageDeserializerStrategy <TMessageType> messageDeserializer, IMessageDispatchingStrategy <TMessageType> messageDispatcher, ICommunicationPort comPort) : base(connectionInfo, messageDeserializer, messageDispatcher, comPort) { }
/// <summary> /// Reads a chunk into the provided <paramref name="buffer"/> from the provided <see cref="SerialPort"/>. /// </summary> /// <param name="serialPort">The port to read from.</param> /// <param name="buffer">The buffer to read into.</param> /// <param name="offset">The offset into the buffer.</param> /// <param name="count">The count of bytes to read.</param> /// <param name="cancellationToken">Optional cancellation token for the read operation.</param> /// <returns>Awaitable for when the operation is completed.</returns> public static async Task ReadAsync(this ICommunicationPort serialPort, byte[] buffer, int offset, int count, CancellationToken cancellationToken = default(CancellationToken)) { if (serialPort == null) { throw new ArgumentNullException(nameof(serialPort)); } if (buffer == null) { throw new ArgumentNullException(nameof(buffer)); } if (offset < 0) { throw new ArgumentOutOfRangeException(nameof(offset)); } if (count < 0) { throw new ArgumentOutOfRangeException(nameof(count)); } if (!serialPort.IsOpen) { throw new InvalidOperationException($"Provided {nameof(serialPort)} is not in an open state. Cannot read."); } for (int i = 0; i < count && !cancellationToken.IsCancellationRequested;) { int readBytes = await serialPort.BaseStream.ReadAsync(buffer, i + offset, count - i, cancellationToken) .ConfigureAwait(false); i += readBytes; } }
/// <inheritdoc /> public UnityQuaternionSegmentGlarduinoClient(ArduinoPortConnectionInfo connectionInfo, IMessageDeserializerStrategy <RecyclableArraySegment <Quaternion> > messageDeserializer, IMessageDispatchingStrategy <RecyclableArraySegment <Quaternion> > messageDispatcher, ICommunicationPort comPort) : base(connectionInfo, messageDeserializer, messageDispatcher, comPort) { }
/// <summary> /// Gets the size of the incoming <see cref="Quaternion"/> array. /// </summary> /// <param name="serialPort">The communication port.</param> /// <param name="cancellationToken">Optional cancel token.</param> /// <returns>Awaitable byte future.</returns> protected virtual async Task <byte> GetQuaternionCountAsync(ICommunicationPort serialPort, CancellationToken cancellationToken) { await serialPort.ReadAsync(SingleByteBuffer, 0, 1, cancellationToken); byte quaternionCount = SingleByteBuffer[0]; return(quaternionCount); }
public Controller(ICommunicationPort communicationPort, IPersistedStorage persistedStorage) { CommunicationPort = communicationPort; CommunicationPortLock = new object(); RefreshInterval = 150; GlobalConfiguration = new GlobalConfiguration(this); IgnitionMap = new IgnitionMap(this, persistedStorage); CurrentState = new CurrentState(this); }
/// <summary> /// Crates a new <see cref="BaseGlarduinoClient{TMessageType}"/>. /// Specifically defining the communication port. /// </summary> /// <param name="connectionInfo">The connection information for the port.</param> /// <param name="messageDeserializer">The message deserialization strategy.</param> /// <param name="messageDispatcher">The message dispatching strategy.</param> /// <param name="comPort">The specified communication port.</param> protected BaseGlarduinoClient(ArduinoPortConnectionInfo connectionInfo, IMessageDeserializerStrategy <TMessageType> messageDeserializer, IMessageDispatchingStrategy <TMessageType> messageDispatcher, ICommunicationPort comPort) { ConnectionInfo = connectionInfo ?? throw new ArgumentNullException(nameof(connectionInfo)); MessageDeserializer = messageDeserializer ?? throw new ArgumentNullException(nameof(messageDeserializer)); MessageDispatcher = messageDispatcher ?? throw new ArgumentNullException(nameof(messageDispatcher)); InternallyManagedPort = comPort ?? throw new ArgumentNullException(nameof(comPort)); _ConnectionEvents = new ConnectionEvents(); }
/// <inheritdoc /> public async Task <RecyclableArraySegment <Quaternion> > ReadMessageAsync(ICommunicationPort serialPort, CancellationToken cancellationToken = default(CancellationToken)) { byte quaternionCount = await GetQuaternionCountAsync(serialPort, cancellationToken); if (quaternionCount == 0) { return(RecyclableArraySegment <Quaternion> .Empty); } //Now we should read the quats based on the provided count var quatArray = ArrayPool <Quaternion> .Shared.Rent(quaternionCount); try { for (int i = 0; i < quaternionCount && !cancellationToken.IsCancellationRequested; i++) { await serialPort.ReadAsync(SingleQuatBuffer, 0, SingleQuatBuffer.Length, cancellationToken); //Just return whatever we have if cancelled. if (cancellationToken.IsCancellationRequested) { return(new RecyclableArraySegment <Quaternion>(quatArray, 0, quaternionCount)); } //quatArray[i] = new Quaternion(GetFloatFromQuatBuffer(1), GetFloatFromQuatBuffer(2), GetFloatFromQuatBuffer(3), GetFloatFromQuatBuffer(0)); quatArray[i] = new Quaternion(GetFloatFromQuatBuffer(0), GetFloatFromQuatBuffer(1), GetFloatFromQuatBuffer(2), GetFloatFromQuatBuffer(3)); if (GlardunioStaticConfig.DebugLogging) { Debug.Log($"Quat log: {quatArray[i].x} {quatArray[i].y} {quatArray[i].z} {quatArray[i].w}"); } } return(new RecyclableArraySegment <Quaternion>(quatArray, 0, quaternionCount)); } catch (Exception e) { throw; } finally { //If exceptions happen let's dipose of the array ArrayPool <Quaternion> .Shared.Return(quatArray); } }
/// <summary> /// Read a line from the SerialPort asynchronously /// </summary> /// <param name="serialPort">The port to read data from</param> /// <returns>A line read from the input</returns> public static async Task <string> ReadLineAsync(this ICommunicationPort serialPort) { if (!serialPort.Encoding.IsSingleByte) { throw new InvalidOperationException($"TODO: Cannot support {serialPort.Encoding.EncodingName}. Only supports 1 byte ASCII right now."); } int charSizeCount = serialPort.Encoding.IsSingleByte ? 1 : serialPort.Encoding.GetMaxByteCount(1); byte[] _singleCharBuffer = ArrayPool <byte> .Shared.Rent(charSizeCount); StringBuilder builder = new StringBuilder(); try { // Read the input one byte at a time, convert the // byte into a char, add that char to the overall // response string, once the response string ends // with the line ending then stop reading while (true) { await serialPort.ReadAsync(_singleCharBuffer, 0, 1) .ConfigureAwait(false); builder.Append((char)_singleCharBuffer[0]); if (builder[builder.Length - 1] == DefaultNewLine) { // Truncate the line ending return(builder.ToString(0, builder.Length - 1)); } } } catch (Exception e) { throw; } finally { ArrayPool <byte> .Shared.Return(_singleCharBuffer, true); } }
private void Form1_Load(object sender, EventArgs e) { _estimator = new CompositePoseEstimator[numberOfLink]; // フィルタ:速い動きに追従する設定です var fs = 1 / 0.01; var cutoff = 25.0; var tc = 0.4; for (int i = 0; i < numberOfLink; i++) { _estimator[i] = new CompositePoseEstimator(); _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.AccLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0)); _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.CompLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0)); _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.GyroLowPassFilter, FilterDisignerBiquad.LowPass(fs, 45.0, 1.0)); _estimator[i].TimeConstantComposition = tc; } // 接続処理 var dialog = new PortOpenDialog(); if (dialog.ShowDialog() != System.Windows.Forms.DialogResult.OK) { return; } _port = CommunicationManager.CreateCommunicationPort(dialog.ConnectionNameType); // データ更新イベントの登録 _port.MsgMeasurementReceived += new ReceivedMsgMeasurementHandler(_port_MsgMeasurementReceived); dialog.Dispose(); _port.Open(); #region 形状、位置設定 // 骨盤 _linkPelvis = new Link() { LinkId = 0, ParentLinkId = -1, Position = new Vec4(), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkPelvisShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(30.7 / scale, 3 / scale, 10 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkPelvis); _allShapes.Add(_linkPelvisShape); // 左大腿 _linkLeftThigh = new Link() { LinkId = 1, ParentLinkId = 0, Position = new Vec4(15.4 / scale, 0 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkLeftThighShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkLeftThigh); _allShapes.Add(_linkLeftThighShape); // 左脛 _linkLeftShin = new Link() { LinkId = 2, ParentLinkId = 1, Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkLeftShinShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkLeftShin); _allShapes.Add(_linkLeftShinShape); // 左足首 _linkLeftFoot = new Link() { LinkId = 3, ParentLinkId = 2, Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkLeftFootShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkLeftFoot); _allShapes.Add(_linkLeftFootShape); // 右大腿 _linkRightThigh = new Link() { LinkId = 4, ParentLinkId = 0, Position = new Vec4(-15.4 / scale, 0 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkRightThighShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkRightThigh); _allShapes.Add(_linkRightThighShape); // 右脛 _linkRightShin = new Link() { LinkId = 5, ParentLinkId = 4, Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkRightShinShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkRightShin); _allShapes.Add(_linkRightShinShape); // 右足首 _linkRightFoot = new Link() { LinkId = 6, ParentLinkId = 5, Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkRightFootShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkRightFoot); _allShapes.Add(_linkRightFootShape); #endregion // キネマティクスの初期化 _kvm = new KinematicsViewModel(new ForwardKinematicsSolver()); _kvm.Init(_allLinkes, _allShapes); // 描画領域に追加 foreach (var s in _allShapes) { imuzDraw1.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z); } return; }
private void Form1_Load(object sender, EventArgs e) { _estimator = new CompositePoseEstimator[numberOfLink]; // フィルタ:速い動きに追従する設定です var fs = 1 / 0.01; var cutoff = 25.0; var tc = 0.4; for (int i = 0; i < numberOfLink; i++) { _estimator[i] = new CompositePoseEstimator(); _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.AccLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0)); _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.CompLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0)); _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.GyroLowPassFilter, FilterDisignerBiquad.LowPass(fs, 45.0, 1.0)); _estimator[i].TimeConstantComposition = tc; } // 接続処理 var dialog = new PortOpenDialog(); if (dialog.ShowDialog() != System.Windows.Forms.DialogResult.OK) { return; } _port = CommunicationManager.CreateCommunicationPort(dialog.ConnectionNameType); // データ更新イベントの登録 _port.MsgMeasurementReceived += new ReceivedMsgMeasurementHandler(_port_MsgMeasurementReceived); dialog.Dispose(); _port.Open(); #region 形状、位置設定 // 骨盤 _linkPelvis = new Link() { LinkId = 0, ParentLinkId = -1, Position = new Vec4(), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkPelvisShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(30.7 / scale, 3 / scale, 10 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkPelvis); _allShapes.Add(_linkPelvisShape); // 左大腿 _linkLeftThigh = new Link() { LinkId = 1, ParentLinkId = 0, Position = new Vec4(15.4 / scale, 0 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkLeftThighShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkLeftThigh); _allShapes.Add(_linkLeftThighShape); // 左脛 _linkLeftShin = new Link() { LinkId = 2, ParentLinkId = 1, Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkLeftShinShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkLeftShin); _allShapes.Add(_linkLeftShinShape); // 左足首 _linkLeftFoot = new Link() { LinkId = 3, ParentLinkId = 2, Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkLeftFootShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkLeftFoot); _allShapes.Add(_linkLeftFootShape); // 右大腿 _linkRightThigh = new Link() { LinkId = 4, ParentLinkId = 0, Position = new Vec4(-15.4 / scale, 0 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkRightThighShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkRightThigh); _allShapes.Add(_linkRightThighShape); // 右脛 _linkRightShin = new Link() { LinkId = 5, ParentLinkId = 4, Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkRightShinShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkRightShin); _allShapes.Add(_linkRightShinShape); // 右足首 _linkRightFoot = new Link() { LinkId = 6, ParentLinkId = 5, Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale), Orientation = Quaternion.CreateFromAngle(0, 0, 0), }; var _linkRightFootShape = new Shape() { Type = ShapeType.Pyramid, Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale), Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2), Position = new Vec4(0, 0, 0, 0), }; _allLinkes.Add(_linkRightFoot); _allShapes.Add(_linkRightFootShape); #endregion // キネマティクスの初期化 _kvm = new KinematicsViewModel(new ForwardKinematicsSolver()); _kvm.Init(_allLinkes, _allShapes); // 描画領域に追加 foreach (var s in _allShapes) { imuzDraw1.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z); } return; }
/// <inheritdoc /> public Task <string> ReadMessageAsync(ICommunicationPort serialPort, CancellationToken cancellationToken = default(CancellationToken)) { return(serialPort.ReadLineAsync()); }
protected override Task <byte> GetQuaternionCountAsync(ICommunicationPort serialPort, CancellationToken cancellationToken) { return(Task.FromResult((byte)HarcodedSize)); }
/// <inheritdoc /> public async Task <int> ReadMessageAsync(ICommunicationPort serialPort, CancellationToken cancellationToken = default(CancellationToken)) { await serialPort.ReadAsync(IntBuffer, 0, sizeof(int), cancellationToken); return(Unsafe.ReadUnaligned <int>(ref IntBuffer[0])); }