diff --git a/Quadruped.WebInterface/Pages/SettingsPage.cshtml b/Quadruped.WebInterface/Pages/SettingsPage.cshtml index 4350b23..cb54a2e 100644 --- a/Quadruped.WebInterface/Pages/SettingsPage.cshtml +++ b/Quadruped.WebInterface/Pages/SettingsPage.cshtml @@ -11,7 +11,13 @@
Framerate
HorizontalResolution
VerticalResolution
-
ImageQuality
+
ImageQuality
+ +

Relaxed stance adjustment

+
X:
+
Y:
+
Z:
+ diff --git a/Quadruped.WebInterface/Pages/SettingsPage.cshtml.cs b/Quadruped.WebInterface/Pages/SettingsPage.cshtml.cs index 1bed27c..d494e40 100644 --- a/Quadruped.WebInterface/Pages/SettingsPage.cshtml.cs +++ b/Quadruped.WebInterface/Pages/SettingsPage.cshtml.cs @@ -1,6 +1,9 @@ -using System.Threading.Tasks; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Threading.Tasks; using Microsoft.AspNetCore.Mvc; using Microsoft.AspNetCore.Mvc.RazorPages; +using Quadruped.WebInterface.RobotController; using Quadruped.WebInterface.VideoStreaming; namespace Quadruped.WebInterface.Pages @@ -8,19 +11,25 @@ namespace Quadruped.WebInterface.Pages public class SettingsPageModel : PageModel { private readonly IVideoService _streamService; + private readonly IRobot _robotController; - public SettingsPageModel(IVideoService streamService) + public SettingsPageModel(IVideoService streamService, IRobot robotController) { _streamService = streamService; + _robotController = robotController; } public void OnGet() { StreamConfiguration = _streamService.StreamerConfiguration; + var relaxed = _robotController.RelaxedStance; + RelaxedStance = new Vector3Class{X = relaxed.X, Y = relaxed.Y, Z = relaxed.Z}; } [BindProperty] public StreamerConfig StreamConfiguration { get; set; } + [BindProperty] + public Vector3Class RelaxedStance { get; set; } public async Task OnPostAsync() { @@ -28,9 +37,25 @@ public async Task OnPostAsync() { return Page(); } - _streamService.StreamerConfiguration = StreamConfiguration; - await _streamService.RestartAsync(); + if (_streamService.StreamerConfiguration != StreamConfiguration) + { + _streamService.StreamerConfiguration = StreamConfiguration; + await _streamService.RestartAsync(); + } + _robotController.UpdateAboluteRelaxedStance((Vector3)RelaxedStance); return RedirectToPage("/VideoRobot"); } } + + public class Vector3Class + { + public float X { get; set; } + public float Y { get; set; } + public float Z { get; set; } + + public static explicit operator Vector3(Vector3Class original) + { + return new Vector3(original.X, original.Y, original.Z); + } + } } diff --git a/Quadruped.WebInterface/RobotController/IRobot.cs b/Quadruped.WebInterface/RobotController/IRobot.cs index c968f28..9f11445 100644 --- a/Quadruped.WebInterface/RobotController/IRobot.cs +++ b/Quadruped.WebInterface/RobotController/IRobot.cs @@ -8,6 +8,8 @@ public interface IRobot Vector2 Direction { get; set; } float Rotation { get; set; } void StartRobot(); + Vector3 RelaxedStance { get; } + void UpdateAboluteRelaxedStance(Vector3 transform); Task DisableMotors(); } } diff --git a/Quadruped.WebInterface/RobotController/MockRobot.cs b/Quadruped.WebInterface/RobotController/MockRobot.cs index 32e46e6..39e174b 100644 --- a/Quadruped.WebInterface/RobotController/MockRobot.cs +++ b/Quadruped.WebInterface/RobotController/MockRobot.cs @@ -14,6 +14,13 @@ public void StartRobot() } + public Vector3 RelaxedStance { get; private set; } + + public void UpdateAboluteRelaxedStance(Vector3 transform) + { + RelaxedStance = transform; + } + public Task DisableMotors() { return Task.CompletedTask; diff --git a/Quadruped.WebInterface/RobotController/Robot.cs b/Quadruped.WebInterface/RobotController/Robot.cs index 4def7da..aa16954 100644 --- a/Quadruped.WebInterface/RobotController/Robot.cs +++ b/Quadruped.WebInterface/RobotController/Robot.cs @@ -52,6 +52,16 @@ public void StartRobot() _robotRunnerTask = Task.Run((Action)RobotRunnerLoop); } + public Vector3 RelaxedStance { get; private set; } = Vector3.Zero; + + public void UpdateAboluteRelaxedStance(Vector3 transform) + { + RelaxedStance = transform; + var newRelaxed = _basicQuadrupedGaitEngine.RelaxedStance; + newRelaxed.Transform(transform); + _basicQuadrupedGaitEngine.RelaxedStance = newRelaxed; + } + private void RobotRunnerLoop() { while (_keepRunning) diff --git a/Quadruped.WebInterface/VideoStreaming/StreamerConfig.cs b/Quadruped.WebInterface/VideoStreaming/StreamerConfig.cs index 00c9343..f4e9871 100644 --- a/Quadruped.WebInterface/VideoStreaming/StreamerConfig.cs +++ b/Quadruped.WebInterface/VideoStreaming/StreamerConfig.cs @@ -1,10 +1,57 @@ -namespace Quadruped.WebInterface.VideoStreaming +using System; + +namespace Quadruped.WebInterface.VideoStreaming { - public class StreamerConfig + public class StreamerConfig : IEquatable { public int ImageQuality { get; set; } = 85; public int HorizontalResolution { get; set; } = 800; public int VerticalResolution { get; set; } = 600; public int Framerate { get; set; } = 10; + + public bool Equals(StreamerConfig other) + { + if (ReferenceEquals(null, other)) return false; + if (ReferenceEquals(this, other)) return true; + return ImageQuality == other.ImageQuality && HorizontalResolution == other.HorizontalResolution && VerticalResolution == other.VerticalResolution && Framerate == other.Framerate; + } + + public override bool Equals(object obj) + { + if (ReferenceEquals(null, obj)) return false; + if (ReferenceEquals(this, obj)) return true; + if (obj.GetType() != this.GetType()) return false; + return Equals((StreamerConfig) obj); + } + + public override int GetHashCode() + { + unchecked + { + var hashCode = ImageQuality; + hashCode = (hashCode * 397) ^ HorizontalResolution; + hashCode = (hashCode * 397) ^ VerticalResolution; + hashCode = (hashCode * 397) ^ Framerate; + return hashCode; + } + } + + public static bool operator ==(StreamerConfig a, StreamerConfig b) + { + if (ReferenceEquals(a, null)) + { + if (ReferenceEquals(b, null)) + { + return true; + } + return false; + } + return a.Equals(b); + } + + public static bool operator !=(StreamerConfig a, StreamerConfig b) + { + return !(a == b); + } } } diff --git a/Quadruped/BasicQuadrupedGaitEngine.cs b/Quadruped/BasicQuadrupedGaitEngine.cs index cea5a1a..c89ac05 100644 --- a/Quadruped/BasicQuadrupedGaitEngine.cs +++ b/Quadruped/BasicQuadrupedGaitEngine.cs @@ -22,7 +22,7 @@ public class BasicQuadrupedGaitEngine : QuadrupedGaitEngine private const int LegDistanceLongitudinal = 15; private const int LegDistanceLateral = 15; - private LegPositions RelaxedStance => new LegPositions + private LegPositions OriginalRelaxedStance => new LegPositions { LeftFront = new Vector3(-LegDistanceLateral, LegDistanceLongitudinal, LegHeight), RightFront = new Vector3(LegDistanceLateral, LegDistanceLongitudinal, LegHeight), @@ -30,12 +30,22 @@ public class BasicQuadrupedGaitEngine : QuadrupedGaitEngine RightRear = new Vector3(LegDistanceLateral, -LegDistanceLongitudinal, LegHeight) }; + private LegPositions _relaxedStance; + + public LegPositions RelaxedStance + { + get => _relaxedStance.Copy(); + set => _relaxedStance = value; + } + + private LegPositions _lastWrittenPosition; public bool IsComamndQueueEmpty => _moveQueueSingal.IsSet && _moves.IsEmpty; public BasicQuadrupedGaitEngine(QuadrupedIkDriver driver) : base(driver) { + _relaxedStance = OriginalRelaxedStance; Driver.Setup(); EnqueueInitialStandup(); if (_moves.TryDequeue(out var deqeueuedLegPosition))