public async Task CalculateJointPostionWhenPointPostionKnown(double x, double y, double z, int expectingPositions)
{
var robotArm = new RobotArm1(3, 4, 0, 1.5, 0, 3.1);
var agleStep = 0.1;
var result = await robotArm.CalculateWholeDataSet(agleStep);
result.ShouldBe(true);
var zeroPoint = new Point {
X = 0, Y = 0, Z = 0
};
var endPoint = new Point {
X = x, Y = y, Z = 0
};
var res = await robotArm.CalculateArmJoint(endPoint);
res.ShouldNotBeNull();
res.Count().ShouldBe(expectingPositions);
var points = res.Select(te => te.JointPosition).ToArray();
points[0].X.ShouldBeGreaterThan(0);
points[0].Y.ShouldBeGreaterThan(0);
points[0].Z.ShouldBe(0);
(Math.Abs(points[0].DistanceFromOtherPoint(zeroPoint) - robotArm.L1) < 0.0000001).ShouldBe(true);
(Math.Abs(points[0].DistanceFromOtherPoint(endPoint) - robotArm.L2) < 0.0000001).ShouldBe(true);
if (expectingPositions == 2)
{
(Math.Abs(points[1].DistanceFromOtherPoint(zeroPoint) - robotArm.L1) < 0.0000001).ShouldBe(true);
(Math.Abs(points[1].DistanceFromOtherPoint(endPoint) - robotArm.L2) < 0.0000001).ShouldBe(true);
}
}
/// <summary>
/// Calculate Arm joint position, when given end point of the robot arm. (Pointing point given)
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <returns></returns>
public async Task <JsonResult> CalculateJointPostions(double x, double y)
{
try
{
if (_robotArm == null)
{
throw new NullReferenceException("Robot Arm is not Initialized");
}
var result = await _robotArm.CalculateArmJoint(new Point { X = x, Y = y, Z = 0 });
if (result.Any(point => double.IsNaN(point.JointPosition.X) || double.IsNaN(point.JointPosition.Y)))
{
throw new Exception("Some of the coordinates is NaN.");
}
return(Json(new { Success = result.Any(), Outcomes = result.Select(p => new KinematicOutcome(p.Theta1.ConvertRadiansToDegrees(), p.Theta2.ConvertRadiansToDegrees(), p.JointPosition)) }));
}
catch (Exception e)
{
Console.WriteLine(e);
return(Json(new { Success = false, e.Message }));
}
}
