コード例 #1
0
        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);
            }
        }
コード例 #2
0
        /// <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 }));
            }
        }