Initial commit from MyParking project
This commit is contained in:
@@ -0,0 +1,126 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography.X509Certificates;
|
||||
using System.Text;
|
||||
using FundamentalLib;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public abstract class AbstractChassis
|
||||
{
|
||||
protected AbstractChassis()
|
||||
{
|
||||
Valid = false;
|
||||
}
|
||||
|
||||
public abstract void Initialize();
|
||||
|
||||
public abstract void Visualize();
|
||||
|
||||
public abstract void AfterDirectionChanged();
|
||||
|
||||
/// <summary>
|
||||
/// 当前行进方向。
|
||||
/// </summary>
|
||||
[Obsolete]
|
||||
public float DirectionAngle
|
||||
{
|
||||
get => _originBiasTh;
|
||||
set
|
||||
{
|
||||
_originBiasTh = value;
|
||||
if (_originBiasTh != _lastDirectionAngle) AfterDirectionChanged();
|
||||
_lastDirectionAngle = _originBiasTh;
|
||||
}
|
||||
}
|
||||
|
||||
protected float _originBiasX = 0f, _originBiasY = 0f, _originBiasTh;
|
||||
|
||||
public Vector3 GetOriginBias()
|
||||
{
|
||||
return new Vector3(_originBiasX, _originBiasY, _originBiasTh);
|
||||
}
|
||||
|
||||
public class CarSpeed
|
||||
{
|
||||
public float Vx,Vy,Vw;
|
||||
}
|
||||
|
||||
public abstract CarSpeed GetCarSpeed(bool isActual = false);
|
||||
public List<GeometricControlPoint> GetGeometricControlPoints()
|
||||
{
|
||||
return GeometricControlPoints;
|
||||
}
|
||||
|
||||
public void ComputeWheelsGeometrically(float speed)
|
||||
{
|
||||
// 打印调用位置信息
|
||||
var stackTrace = new StackTrace(true);
|
||||
var callerFrame = stackTrace.GetFrame(1); // 获取调用者的帧
|
||||
if (callerFrame != null)
|
||||
{
|
||||
var fileName = callerFrame.GetFileName();
|
||||
var lineNumber = callerFrame.GetFileLineNumber();
|
||||
DLog.Log($"s:{speed:0.000} from {fileName} ln.{lineNumber}", $"WheelComputeCaller");
|
||||
}
|
||||
|
||||
DefineGeometricWheelComputation(speed);
|
||||
}
|
||||
|
||||
protected abstract void DefineGeometricWheelComputation(float speed);
|
||||
|
||||
public void DriveStop()
|
||||
{
|
||||
PredefinedDriveStop();
|
||||
CustomDriveStop?.Invoke();
|
||||
}
|
||||
|
||||
public abstract void PredefinedDriveStop();
|
||||
|
||||
public Action CustomDriveStop;
|
||||
|
||||
public abstract bool ComputeRotateWheels(float rotSpeed);
|
||||
|
||||
public abstract float CalculateTurningSpeedDecayFac(float turn);
|
||||
|
||||
public enum ChassisState
|
||||
{
|
||||
Standby,
|
||||
Running,
|
||||
AbnormalFeedback,
|
||||
ExceedMotionAbility,
|
||||
}
|
||||
|
||||
protected ChassisState State;
|
||||
|
||||
protected string StateDescription;
|
||||
|
||||
public (ChassisState State, string Description) GetChassisState()
|
||||
{
|
||||
return (State, StateDescription);
|
||||
}
|
||||
|
||||
public bool Debug = false;
|
||||
public float AccPerSecond = 0.2f;
|
||||
public float DeAccPerSecond = 0.2f;
|
||||
public float MaxSpeed = 1; // m/s
|
||||
public float MinTurnSpeedFac = 0.5f;
|
||||
public float MaxTurnThreshold = 90f;
|
||||
|
||||
public float GcpThetaPerSecond = 10f;
|
||||
|
||||
public DateTime LastMoveTime = DateTime.MinValue;
|
||||
|
||||
protected bool Valid = false;
|
||||
protected List<GeometricControlPoint> GeometricControlPoints = new();
|
||||
protected bool RotatingActive = false;
|
||||
protected bool GoingActive = false;
|
||||
protected bool GoingWheelAligned = false;
|
||||
|
||||
private float _lastDirectionAngle = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class DiffSteerWheel:SteerWheel
|
||||
{
|
||||
public DiffSteerWheel(float wheelDistance,Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
|
||||
Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, Action<float> leftSpeedWriter, Action<float> rightSpeedWriter,
|
||||
float angleLimitMarginDeg = 15f) : base(position,
|
||||
angleLowerLimit, angleUpperLimit, speedWriter, speedReader, angleWriter, angleReader, angleLimitMarginDeg)
|
||||
{
|
||||
_leftSpeedWriter = leftSpeedWriter;
|
||||
_rightSpeedWriter = rightSpeedWriter;
|
||||
WheelDistance = wheelDistance;
|
||||
}
|
||||
|
||||
public float GetLeftSendSpeed()
|
||||
{
|
||||
return _leftSendSpeed;
|
||||
}
|
||||
|
||||
public float GetRightSendSpeed()
|
||||
{
|
||||
return _rightSendSpeed;
|
||||
}
|
||||
|
||||
public void WriteLeftSpeed(float speed)
|
||||
{
|
||||
_leftSpeedWriter(_leftSendSpeed = speed);
|
||||
}
|
||||
|
||||
public void WriteRightSpeed(float speed)
|
||||
{
|
||||
_rightSpeedWriter(_rightSendSpeed = speed);
|
||||
}
|
||||
|
||||
public float WheelDistance;
|
||||
private readonly Action<float> _leftSpeedWriter;
|
||||
private readonly Action<float> _rightSpeedWriter;
|
||||
|
||||
private float _leftSendSpeed;
|
||||
private float _rightSendSpeed;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,170 @@
|
||||
using FundamentalLib;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Diagnostics;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class DifferentialChassis : AbstractChassis
|
||||
{
|
||||
public void SetLeftRightWheels(Wheel wheelL, Wheel wheelR)
|
||||
{
|
||||
_leftWheel = wheelL;
|
||||
_rightWheel = wheelR;
|
||||
_halfWheelTrack = Math.Abs(_leftWheel.Position.Y);
|
||||
}
|
||||
|
||||
public override void Visualize()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public override CarSpeed GetCarSpeed(bool isActual = false)
|
||||
{
|
||||
if (!isActual)
|
||||
{
|
||||
return new CarSpeed()
|
||||
{
|
||||
Vx = (_speedL + _speedR) / 2f,
|
||||
Vw = (_speedR - _speedL) / Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
|
||||
(float)Math.PI * 180f * 1000f,
|
||||
Vy = 0
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
return new CarSpeed()
|
||||
{
|
||||
Vx = GetLinearSpeed(),
|
||||
Vw = (_rightWheel.ReadSpeed() - _leftWheel.ReadSpeed()) /
|
||||
Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
|
||||
(float)Math.PI * 180f * 1000f,
|
||||
Vy = 0
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public (Wheel,Wheel) GetWheels()
|
||||
{
|
||||
return (_leftWheel, _rightWheel);
|
||||
}
|
||||
|
||||
public float GetLinearSpeed()
|
||||
{
|
||||
return (_leftWheel.ReadSpeed() + _rightWheel.ReadSpeed()) / 2f;
|
||||
}
|
||||
|
||||
public override void Initialize()
|
||||
{
|
||||
GeometricControlPoints.Add(new GeometricControlPoint(new Vector2(0, 0)));
|
||||
Valid = true;
|
||||
}
|
||||
|
||||
public override void AfterDirectionChanged()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public override void PredefinedDriveStop()
|
||||
{
|
||||
if (!Valid) return;
|
||||
_sendSpeedL = _sendSpeedR = 0;
|
||||
_speedL = _speedR = 0;
|
||||
_leftWheel.WriteSpeed(_sendSpeedL);
|
||||
_rightWheel.WriteSpeed(_sendSpeedR);
|
||||
GoingActive = false;
|
||||
RotatingActive = false;
|
||||
}
|
||||
|
||||
protected override void DefineGeometricWheelComputation(float speed)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
if (!GoingActive) LastMoveTime = now;
|
||||
|
||||
SendSpeed(speed, GeometricControlPoints[0].Theta, now - LastMoveTime);
|
||||
|
||||
GoingActive = true;
|
||||
RotatingActive = false;
|
||||
}
|
||||
|
||||
public override bool ComputeRotateWheels(float rotSpeed)
|
||||
{
|
||||
if (!RotatingActive) LastMoveTime = DateTime.Now;
|
||||
|
||||
SendSpeed(0, rotSpeed);
|
||||
|
||||
GoingActive = false;
|
||||
RotatingActive = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
public override float CalculateTurningSpeedDecayFac(float turn)
|
||||
{
|
||||
return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
|
||||
}
|
||||
|
||||
public void SendSpeed(float linearSpeed, float angularSpeed, TimeSpan? deltaTime = null)
|
||||
{
|
||||
var edgeLinearSpeed = (float)(angularSpeed / 180f * Math.PI * _halfWheelTrack / 1000);
|
||||
var vl = linearSpeed - edgeLinearSpeed;
|
||||
var vr = linearSpeed + edgeLinearSpeed;
|
||||
_speedL = vl;
|
||||
_speedR = vr;
|
||||
AccumulateSpeed(vl, vr, deltaTime);
|
||||
LastMoveTime = DateTime.Now;
|
||||
}
|
||||
|
||||
private void AccumulateSpeed(float vl, float vr, TimeSpan? deltaTime = null)
|
||||
{
|
||||
// var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
|
||||
//
|
||||
// var speedSignL = Math.Sign(vl - _sendSpeedL);
|
||||
// var accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
|
||||
// _sendSpeedL += speedSignL * Math.Min(Math.Abs(vl - _sendSpeedL), accL * dTime);
|
||||
// _leftWheel.WriteSpeed(_sendSpeedL);
|
||||
//
|
||||
// var speedSignR = Math.Sign(vr - _sendSpeedR);
|
||||
// var accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
|
||||
// _sendSpeedR += speedSignR * Math.Min(Math.Abs(vr - _sendSpeedR), accR * dTime);
|
||||
// _rightWheel.WriteSpeed(_sendSpeedR);
|
||||
|
||||
// if (Debug)
|
||||
// Console.WriteLine($"DiffChassis, target:{v:0.00},send:{_sendSpeed:0.0}");
|
||||
// var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
|
||||
var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
|
||||
float diffL = vl - _sendSpeedL;
|
||||
float diffR = vr - _sendSpeedR;
|
||||
|
||||
float accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
|
||||
float accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
|
||||
|
||||
float maxDeltaL = accL * dTime;
|
||||
float maxDeltaR = accR * dTime;
|
||||
|
||||
float factorL = Math.Abs(diffL) > maxDeltaL ? maxDeltaL / Math.Abs(diffL) : 1.0f;
|
||||
float factorR = Math.Abs(diffR) > maxDeltaR ? maxDeltaR / Math.Abs(diffR) : 1.0f;
|
||||
|
||||
float factor = Math.Min(factorL, factorR);
|
||||
|
||||
_sendSpeedL += diffL * factor;
|
||||
_sendSpeedR += diffR * factor;
|
||||
|
||||
_leftWheel.WriteSpeed(_sendSpeedL);
|
||||
_rightWheel.WriteSpeed(_sendSpeedR);
|
||||
// Console.WriteLine($"DiffChassis, target:{vl:0.00},send:{_sendSpeedL:0.00} dTime：{dTime} diffL:{diffL} factor:{factor}" );
|
||||
}
|
||||
|
||||
private Wheel _leftWheel;
|
||||
private Wheel _rightWheel;
|
||||
private float _halfWheelTrack; // millimeter
|
||||
|
||||
private float _sendSpeedL;
|
||||
private float _sendSpeedR;
|
||||
private int _direction = 1; // 1 forward, -1 backward
|
||||
private float _speedL;
|
||||
private float _speedR;
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,172 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Diagnostics;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class SingleSteerChassis : AbstractChassis
|
||||
{
|
||||
public void SetSteerWheel(SteerWheel wheel)
|
||||
{
|
||||
_steerWheel = wheel;
|
||||
}
|
||||
|
||||
public SteerWheel GetSteerWheel()
|
||||
{
|
||||
return _steerWheel;
|
||||
}
|
||||
|
||||
public override void Visualize()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public override CarSpeed GetCarSpeed(bool isActual = false)
|
||||
{
|
||||
if (!isActual)
|
||||
{
|
||||
var sendAngle = _steerWheel.GetSendAngle();
|
||||
var sendAngleRad = _steerWheel.GetSendAngle() / 180f * Math.PI;
|
||||
// VSteer* Cos = v;
|
||||
var vsteer = _sendSpeed / ((Math.Cos(Math.Abs(sendAngleRad)) + 0.000001));
|
||||
var vsteerY = vsteer * Math.Sin(sendAngleRad);
|
||||
// Console.WriteLine($"{vsteer} {vsteerY} {sendAngleRad} {_sendSpeed}");
|
||||
return new CarSpeed()
|
||||
{
|
||||
Vx = (float)(_sendSpeed * Math.Cos(Math.Abs(sendAngle) / 180f * Math.PI)),
|
||||
Vy = 0,
|
||||
Vw = (float)(_sendSpeed * Math.Sin(Math.Abs(sendAngle) / 180f * Math.PI) /
|
||||
Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
|
||||
//阿克曼
|
||||
// Vx = (float)(_sendSpeed),
|
||||
// Vy = 0,
|
||||
// Vw = (float)(vsteerY / Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
return new CarSpeed()
|
||||
{
|
||||
Vx = (float)(_steerWheel.ReadSpeed() *
|
||||
Math.Cos(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI)),
|
||||
Vy = 0,
|
||||
Vw = (float)(_steerWheel.ReadSpeed() *
|
||||
Math.Sin(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI) /
|
||||
Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public override void Initialize()
|
||||
{
|
||||
GeometricControlPoints = new List<GeometricControlPoint>()
|
||||
{
|
||||
new (_steerWheel.Position),
|
||||
new (Vector2.Zero)
|
||||
};
|
||||
Valid = true;
|
||||
}
|
||||
|
||||
public override void AfterDirectionChanged()
|
||||
{
|
||||
if (Math.Abs(CommonMath.ThDiff(0, _originBiasTh)) > 90)
|
||||
{
|
||||
GeometricControlPoints = new List<GeometricControlPoint>()
|
||||
{
|
||||
new (-_steerWheel.Position),
|
||||
};
|
||||
_direction = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
GeometricControlPoints = new List<GeometricControlPoint>()
|
||||
{
|
||||
new (_steerWheel.Position),
|
||||
};
|
||||
_direction = 1;
|
||||
}
|
||||
}
|
||||
|
||||
public override void PredefinedDriveStop()
|
||||
{
|
||||
if (!Valid) return;
|
||||
_sendSpeed = 0;
|
||||
_steerWheel.WriteSpeed(_sendSpeed);
|
||||
GoingActive = false;
|
||||
RotatingActive = false;
|
||||
}
|
||||
|
||||
protected override void DefineGeometricWheelComputation(float speed)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
|
||||
if (!GoingActive)
|
||||
{
|
||||
LastMoveTime = now;
|
||||
GoingWheelAligned = false;
|
||||
}
|
||||
|
||||
SendSteerMotion(speed * _direction, GeometricControlPoints[0].Theta, now - LastMoveTime);
|
||||
|
||||
GoingActive = true;
|
||||
RotatingActive = false;
|
||||
}
|
||||
|
||||
public override bool ComputeRotateWheels(float rotSpeed)
|
||||
{
|
||||
if (!RotatingActive)
|
||||
{
|
||||
LastMoveTime = DateTime.Now;
|
||||
GoingWheelAligned = false;
|
||||
}
|
||||
|
||||
SendSteerMotion(rotSpeed, 90);
|
||||
|
||||
GoingActive = false;
|
||||
RotatingActive = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SendSteerMotion(float speed, float theta, TimeSpan? deltaTime = null)
|
||||
{
|
||||
_steerWheel.WriteAngle(theta);
|
||||
|
||||
if (!GoingWheelAligned && Math.Abs(CommonMath.ThDiff(_steerWheel.ReadAngle(), theta)) < 1)
|
||||
GoingWheelAligned = true;
|
||||
if (!GoingWheelAligned) speed = 0;
|
||||
|
||||
var turnThresholdSpeed = CalculateTurningSpeedDecayFac(Math.Abs(theta)) * MaxSpeed;
|
||||
AccumulateSpeed(Math.Min(turnThresholdSpeed, Math.Abs(speed)) * Math.Sign(speed), deltaTime);
|
||||
|
||||
LastMoveTime = DateTime.Now;
|
||||
}
|
||||
|
||||
public override float CalculateTurningSpeedDecayFac(float turn)
|
||||
{
|
||||
return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
|
||||
}
|
||||
|
||||
private void AccumulateSpeed(float v, TimeSpan? deltaTime = null)
|
||||
{
|
||||
// _targetSpeed = v;
|
||||
var speedSign = Math.Sign(v - _sendSpeed);
|
||||
var acc = Math.Abs(v) > Math.Abs(_sendSpeed) ? AccPerSecond : DeAccPerSecond;
|
||||
_sendSpeed += speedSign * Math.Min(Math.Abs(v - _sendSpeed),
|
||||
acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
|
||||
_steerWheel.WriteSpeed(_sendSpeed);
|
||||
|
||||
if (Debug)
|
||||
Console.WriteLine($"SingleSteer, target:{v:0.00},send:{_sendSpeed:0.0}");
|
||||
}
|
||||
|
||||
private SteerWheel _steerWheel;
|
||||
private float _sendSpeed;
|
||||
private int _direction = 1; // 1 forward, -1 backward
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class Wheel
|
||||
{
|
||||
public Wheel(Vector2 position, Action<float> speedWriter, Func<float> speedReader)
|
||||
{
|
||||
PhysicalPosition = Position = position;
|
||||
SpeedWriter = speedWriter;
|
||||
SpeedReader = speedReader;
|
||||
}
|
||||
|
||||
public void WriteSpeed(float speed)
|
||||
{
|
||||
SpeedWriter.Invoke(_sendSpeed = speed);
|
||||
}
|
||||
|
||||
public float GetSendSpeed()
|
||||
{
|
||||
return _sendSpeed;
|
||||
}
|
||||
|
||||
public float ReadSpeed()
|
||||
{
|
||||
return SpeedReader();
|
||||
}
|
||||
|
||||
// PhysicalPosition ===(chassis transform)===> Position
|
||||
// useful in dual agv coordination
|
||||
public readonly Vector2 PhysicalPosition;
|
||||
public Vector2 Position;
|
||||
|
||||
public float ZeroDirection = 0;
|
||||
|
||||
[JsonIgnore] public readonly Action<float> SpeedWriter;
|
||||
[JsonIgnore] public readonly Func<float> SpeedReader;
|
||||
|
||||
public float _sendSpeed;
|
||||
}
|
||||
|
||||
public class GeometricControlPoint
|
||||
{
|
||||
public GeometricControlPoint(Vector2 position)
|
||||
{
|
||||
Position = position;
|
||||
}
|
||||
|
||||
public Vector2 Position;
|
||||
public float Theta;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user