Prettified output,
Added average for Calibration Values, Removed /input/run, Added thresholds
This commit is contained in:
parent
c7f4a96a0d
commit
c846e50fd9
@ -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
|
||||
@ -169,14 +185,26 @@ 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;
|
||||
|
||||
double gpsPosX = (gpsFactorB / GPSConstantD);
|
||||
double gpsPosY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD);
|
||||
|
||||
AddCalibrationValues(gpsPosX, gpsPosY);
|
||||
|
||||
_posX = (gpsFactorB / GPSConstantD) + CalibrationX;
|
||||
_posY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD) + CalibrationY;
|
||||
double posX = gpsPosX + CalibrationX;
|
||||
double posY = gpsPosY + CalibrationY;
|
||||
|
||||
double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));
|
||||
|
||||
double inverseLength = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2));
|
||||
|
||||
_verticalMovement = _posY * inverseLength * _leashStretch;
|
||||
_horizontalMovement = _posX * inverseLength * -1 * _leashStretch;
|
||||
_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
29
OSCCollar/Vector.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user