/// <summary>
/// Update unit speed based on wheels speed
/// </summary>
/// <param name="currentTime"></param>
public void UpdateSpeed(double currentTime)
{
if (movement == null) return;
var requestedAngular = rules.WheelRadius * (movement.RightRotatingVelocity.Radian - movement.LeftRotatingVelocity.Radian) / rules.DistanceBetweenWheels;
var requestedLinear = rules.WheelRadius * (movement.LeftRotatingVelocity.Radian + movement.RightRotatingVelocity.Radian) / 2;
var linear = requestedLinear; // Math.Sign(requestedLinear) * Math.Min(Math.Abs(requestedLinear), rules.LinearVelocityLimit);
var angular = requestedAngular;// Math.Sign(requestedAngular) * Math.Min(Math.Abs(requestedAngular), rules.AngularVelocityLimit.Radian);
var angle = actor.World.Engine.GetAbsoluteLocation(actor.ObjectId).Yaw.Radian;
//convert into cvarc world velocity
var unitSpeed = new AIRLab.Mathematics.Frame3D(
linear * Math.Cos(angle),
linear * Math.Sin(angle),
0,
Angle.Zero,
Angle.FromRad(angular),
Angle.Zero);
actor.World.Engine.SetSpeed(actor.ObjectId, unitSpeed);
EncodersData encoderRecord = new EncodersData
{
Timestamp = actor.World.Clocks.CurrentTime,
TotalLeftRotation = Angle.FromRad(movement.LeftRotatingVelocity.Radian * 0.005),
TotalRightRotation = Angle.FromRad(movement.RightRotatingVelocity.Radian * 0.005)
};
actor.DWMData.EncodersHistory.Add(encoderRecord);
}
/// <summary>
/// Update unit speed based on wheels speed
/// </summary>
/// <param name="currentTime"></param>
public void UpdateSpeed(double currentTime)
{
if (movement == null)
{
return;
}
var engine = actor.World.GetEngine <ICommonEngine>();
var requestedAngular = rules.WheelRadius * (movement.RightRotatingVelocity.Radian - movement.LeftRotatingVelocity.Radian) / rules.DistanceBetweenWheels;
var requestedLinear = rules.WheelRadius * (movement.LeftRotatingVelocity.Radian + movement.RightRotatingVelocity.Radian) / 2;
var linear = requestedLinear; // Math.Sign(requestedLinear) * Math.Min(Math.Abs(requestedLinear), rules.LinearVelocityLimit);
var angular = requestedAngular; // Math.Sign(requestedAngular) * Math.Min(Math.Abs(requestedAngular), rules.AngularVelocityLimit.Radian);
var angle = engine.GetAbsoluteLocation(actor.ObjectId).Yaw.Radian;
//convert into cvarc world velocity
var unitSpeed = new AIRLab.Mathematics.Frame3D(
linear * Math.Cos(angle),
linear * Math.Sin(angle),
0,
Angle.Zero,
Angle.FromRad(angular),
Angle.Zero);
engine.SetAbsoluteSpeed(actor.ObjectId, unitSpeed);
EncodersData encoderRecord = new EncodersData
{
Timestamp = actor.World.Clocks.CurrentTime,
TotalLeftRotation = Angle.FromRad(movement.LeftRotatingVelocity.Radian * 0.005),
TotalRightRotation = Angle.FromRad(movement.RightRotatingVelocity.Radian * 0.005)
};
actor.DWMData.EncodersHistory.Add(encoderRecord);
}
DWMTestEntry EncodersTest(EncodersData encoders, params DWMCommand[] command)
{
return (client, world, asserter) =>
{
DWMSensorsData data = new DWMSensorsData();
foreach (var c in command)
data = client.Act(c);
asserter.IsEqual(encoders.TotalLeftRotation.Radian, data.Encoders.Sum(x => x.TotalLeftRotation.Radian), 0);
asserter.IsEqual(encoders.TotalRightRotation.Radian, data.Encoders.Sum(x => x.TotalRightRotation.Radian),0);
};
}
