101 lines
2.8 KiB
C#
101 lines
2.8 KiB
C#
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
using System.Text;
|
|
using CommonUsage.Mathematics;
|
|
|
|
namespace CommonUsage.Chassis
|
|
{
|
|
public class SteerWheel : Wheel
|
|
{
|
|
public SteerWheel(Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
|
|
Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, float angleLimitMarginDeg = 15f) : base(position, speedWriter,
|
|
speedReader)
|
|
{
|
|
_angleLowerLimit = angleLowerLimit;
|
|
_angleUpperLimit = angleUpperLimit;
|
|
_angleWriter = angleWriter;
|
|
_angleReader = angleReader;
|
|
_centerDistance = position.Length();
|
|
AngleLimitMarginDeg = angleLimitMarginDeg;
|
|
}
|
|
|
|
public float AngleLimitMarginDeg = 15f;
|
|
|
|
public bool TrySetDirection(bool allowReverse, ref float desireDirection, ref int dir)
|
|
{
|
|
if (TryNormalizeAngleInLimit(desireDirection, out var normalized))
|
|
{
|
|
dir = 1;
|
|
desireDirection = normalized;
|
|
return true;
|
|
}
|
|
|
|
if (!allowReverse) return false;
|
|
|
|
var oppositeTh = (float)CommonMath.RoundTh(desireDirection + 180);
|
|
if (TryNormalizeAngleInLimit(oppositeTh, out normalized))
|
|
{
|
|
dir = -1;
|
|
desireDirection = normalized;
|
|
return true;
|
|
}
|
|
|
|
dir = 0;
|
|
return false;
|
|
}
|
|
|
|
private bool TryNormalizeAngleInLimit(float angle, out float normalized)
|
|
{
|
|
var lower = CommonMath.RoundTh(_angleLowerLimit);
|
|
var upper = CommonMath.RoundTh(_angleUpperLimit);
|
|
while (upper < lower) upper += 360;
|
|
|
|
normalized = (float)CommonMath.RoundTh(angle);
|
|
while (normalized < lower) normalized += 360;
|
|
while (normalized > upper && normalized - 360 >= lower) normalized -= 360;
|
|
|
|
var margin = Math.Min(normalized - lower, upper - normalized);
|
|
return normalized >= lower && normalized <= upper && margin >= Math.Max(0, AngleLimitMarginDeg);
|
|
}
|
|
|
|
public float ReadAngle()
|
|
{
|
|
return _angleReader();
|
|
}
|
|
|
|
public void WriteAngle(float angle)
|
|
{
|
|
_angleWriter.Invoke(_sendAngle = Math.Max(_angleLowerLimit, Math.Min(angle, _angleUpperLimit)));
|
|
}
|
|
|
|
public float GetSendAngle()
|
|
{
|
|
return _sendAngle;
|
|
}
|
|
|
|
public float GetAngleRelativeToChassis()
|
|
{
|
|
return ZeroDirection + _sendAngle;
|
|
}
|
|
|
|
public float CenterDistance()
|
|
{
|
|
return _centerDistance;
|
|
}
|
|
|
|
public float AngleLowerLimit => _angleLowerLimit;
|
|
|
|
public float AngleUpperLimit => _angleUpperLimit;
|
|
|
|
[JsonIgnore] private readonly Action<float> _angleWriter;
|
|
[JsonIgnore] private readonly Func<float> _angleReader;
|
|
|
|
private float _angleLowerLimit = -90, _angleUpperLimit = 90;
|
|
private float _centerDistance;
|
|
|
|
public float _sendAngle = 0f;
|
|
}
|
|
}
|