Prettified output,

Added average for Calibration Values,
Removed /input/run,
Added thresholds
This commit is contained in:
glax 2023-12-03 02:56:35 +01:00
parent c7f4a96a0d
commit c846e50fd9
2 changed files with 104 additions and 42 deletions

View File

@ -21,7 +21,7 @@ public class OSCCollar
double walkStretchDeadzone = jObject.GetValue("walkStretchDeadzone")?.ToObject<double>() ?? 0.1;
double runStretch = jObject.GetValue("runStretch")?.ToObject<double>() ?? 0.4;
bool skipSetup = jObject.GetValue("skipSetup")?.ToObject<bool>() ?? true;
var _ = new OSCCollar(ip, portReceive, portSend, radius, calibrationX, calibrationY, skipSetup);
var _ = new OSCCollar(ip, portReceive, portSend, radius, calibrationX, calibrationY, walkStretchDeadzone, runStretch, skipSetup);
}else
{
var _ = new OSCCollar();
@ -31,12 +31,13 @@ public class OSCCollar
private OscServer Server { get; init; }
private OscClient Client { get; init; }
private float _leashStretch;
private double _posX, _posY, _verticalMovement, _horizontalMovement;
private readonly Vector _unitVectorLeash = new(1, 0);
private Vector _movementVector = new(0, 0);
private bool _allowMoving = true;
private uint _nilSentCount;
private DateTime _lastNilMessageSent = DateTime.UnixEpoch;
private DateTime _lastConsoleOutput = DateTime.UnixEpoch;
private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(100);
private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(500);
private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(10);
private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400);
private readonly double _radius, _walkStretch, _runStretch;
@ -45,7 +46,8 @@ public class OSCCollar
private GPS GPS3 { get; init; }
private double CalibrationX { get; init; }
private double CalibrationY { get; init; }
private string debugValue = "";
private readonly Queue<ValueTuple<double, double>> _calibrationAverage = new();
private string _debugValue = "";
private double GPSConstantA { get; init; }
private double GPSConstantB { get; init; }
private double GPSConstantC { get; init; }
@ -91,7 +93,7 @@ public class OSCCollar
runningThread.Start();
}
private void PrintOutput(double verticalMovement, double horizontalMovement)
private void PrintOutput()
{
Console.Clear();
Console.ForegroundColor = ConsoleColor.White;
@ -99,43 +101,57 @@ public class OSCCollar
Console.WriteLine("==============================");
Console.WriteLine($"GPS 1:................{GPS1.Distance:00.00000}");
Console.WriteLine($"GPS 2:................{GPS2.Distance:00.00000}");
Console.WriteLine($"GPS 3:................{GPS3.Distance:0.00000}");
Console.WriteLine($"Position X:..........{_posX:'.'0.00000;'-'0.00000}");
Console.WriteLine($"Position Y:..........{_posY:'.'0.00000;'-'0.00000}");
Console.WriteLine($"Stretch:...............{_leashStretch:0.00000}");
Console.WriteLine($"Vertical Movement:...{verticalMovement:' '0.00000;'-'0.00000}");
Console.WriteLine($"Horizontal Movement:..{horizontalMovement:' '0.00000;'-'0.00000}");
string nilString = $"/input nil {_nilSent}";
Console.WriteLine($"GPS 3:................{GPS3.Distance:00.00000}");
Console.ForegroundColor = ConsoleColor.Cyan;
Console.WriteLine($"Position Vector:......{_unitVectorLeash}");
Console.ForegroundColor = ConsoleColor.White;
Console.WriteLine($"Stretch:..............{_leashStretch:0.00000}");
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine($"Movement Vector:......{_movementVector}");
Console.ForegroundColor = ConsoleColor.Gray;
ValueTuple<double, double> calibrationValues = GetCalibrationValues();
Console.WriteLine($"Calibration (x y): ...({calibrationValues.Item1:' '00.0000;'-'00.0000} {calibrationValues.Item2:' '00.0000;'-'00.0000})");
Console.ForegroundColor = ConsoleColor.White;
string nilString = $"/input nil {_nilSentCount}";
Console.SetCursorPosition(Console.WindowWidth - nilString.Length - 1, 0);
Console.Write(nilString);
Console.ForegroundColor = ConsoleColor.DarkGray;
if (Console.WindowHeight < 13)
return;
Console.SetCursorPosition(40, 5);
Console.SetCursorPosition(43, 5);
Console.WriteLine("----------+----------");
for (int i = 1; i < 10; i++)
{
if(i == 5)
continue;
Console.SetCursorPosition(50,i);
Console.SetCursorPosition(53,i);
Console.Write("|");
}
const int centerX = 50;
const int centerX = 53;
const int centerY = 5;
int consoleX = Convert.ToInt32(Math.Floor(_horizontalMovement * 10));
int consoleY = Convert.ToInt32(-Math.Floor(_verticalMovement * 4));
double position = (_verticalMovement * 10) % 10;
char c = position < 3 ? '.' : position > 7 ? '\'' : 'x';
Console.SetCursorPosition(centerX + consoleX, centerY + consoleY);
int positionX = Convert.ToInt32(Math.Floor(_unitVectorLeash.X * 10));
int positionY = Convert.ToInt32(-Math.Floor(_unitVectorLeash.Y * 4));
double positionPrecision = (_unitVectorLeash.Y * 10) % 10;
char positionPrecisionChar = positionPrecision < 3 ? '.' : positionPrecision > 7 ? '\'' : 'x';
Console.SetCursorPosition(centerX + positionX, centerY + positionY);
Console.ForegroundColor = ConsoleColor.Cyan;
Console.Write(c);
Console.SetCursorPosition(Console.WindowWidth - 1, Console.WindowHeight - 1);
Console.Write(positionPrecisionChar);
int movementX = Convert.ToInt32(Math.Floor(_movementVector.X * 10));
int movementY = Convert.ToInt32(-Math.Floor(_movementVector.Y * 4));
double movementPrecision = (_movementVector.Y * 10) % 10;
char movementPrecisionChar = movementPrecision < 3 ? '.' : movementPrecision > 7 ? '\'' : 'x';
Console.SetCursorPosition(centerX + movementX, centerY + movementY);
Console.ForegroundColor = ConsoleColor.Red;
Console.Write(movementPrecisionChar);
Console.SetCursorPosition(Console.WindowWidth - debugValue.Length - 1, Console.WindowHeight - 2);
Console.SetCursorPosition(Console.WindowWidth - _debugValue.Length - 1, Console.WindowHeight - 2);
Console.ForegroundColor = ConsoleColor.Gray;
Console.WriteLine(debugValue);
Console.WriteLine(_debugValue);
}
#region Handle OSC-Messages
@ -170,13 +186,25 @@ public class OSCCollar
double gpsFactorA = Math.Pow(GPS1.Distance, 2) - Math.Pow(GPS2.Distance, 2) - GPSConstantC;
double gpsFactorB = Math.Pow(GPS2.Distance, 2) - Math.Pow(GPS3.Distance, 2) - GPSConstantE;
_posX = (gpsFactorB / GPSConstantD) + CalibrationX;
_posY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD) + CalibrationY;
double gpsPosX = (gpsFactorB / GPSConstantD);
double gpsPosY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD);
double inverseLength = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2));
AddCalibrationValues(gpsPosX, gpsPosY);
_verticalMovement = _posY * inverseLength * _leashStretch;
_horizontalMovement = _posX * inverseLength * -1 * _leashStretch;
double posX = gpsPosX + CalibrationX;
double posY = gpsPosY + CalibrationY;
double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));
_unitVectorLeash.X = posX * inverseLength * -1;
_unitVectorLeash.Y = posY * inverseLength;
if (_leashStretch < _walkStretch) //Below Deadzone
_movementVector = new();
else if (_walkStretch < _runStretch) //Walk
_movementVector = _unitVectorLeash.MultipliedWith(0.5);
else //Run
_movementVector = _unitVectorLeash.MultipliedWith(1);
}
private void RunningThread()
@ -189,30 +217,35 @@ public class OSCCollar
{
this.Client.Send("/input/Vertical", 0f);
this.Client.Send("/input/Horizontal", 0f);
this.Client.Send("/input/Run", 0);
this._lastNilMessageSent = DateTime.Now;
_nilSentCount++;
}else if (_allowMoving)
{
if (_leashStretch > 0.1)
{
this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement));
this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement));
}
if (_leashStretch > 0.4)
this.Client.Send("/input/Run", 1);
else
this.Client.Send("/input/Run", 0);
this.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
this.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
}
if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now)
{
PrintOutput(_verticalMovement, _horizontalMovement);
PrintOutput();
_lastConsoleOutput = DateTime.Now;
}
Thread.Sleep(UpdateInterval);
}
}
private void AddCalibrationValues(double x, double y)
{
this._calibrationAverage.Enqueue(new ValueTuple<double, double>(x, y));
if (this._calibrationAverage.Count > 50)
this._calibrationAverage.Dequeue();
}
private ValueTuple<double, double> GetCalibrationValues()
{
double averageX = -this._calibrationAverage.Average(value => value.Item1);
double averageY = -this._calibrationAverage.Average(value => value.Item2);
return new ValueTuple<double, double>(averageX, averageY);
}
}

29
OSCCollar/Vector.cs Normal file
View File

@ -0,0 +1,29 @@
namespace VRC_Console;
public class Vector
{
public double X { get; set; }
public double Y { get; set; }
public Vector()
{
this.X = 0;
this.Y = 0;
}
public Vector(double x, double y)
{
this.X = x;
this.Y = y;
}
public override string ToString()
{
return $"({this.X:' '0.00000;'-'0.00000} {this.Y:' '0.00000;'-'0.00000})";
}
public Vector MultipliedWith(double factor)
{
return new Vector(this.X * factor, this.Y * factor);
}
}