public void DrawProjectilePath()
        {
            var p = new Projectile
            {
                Position = new Point(0, 1, 0),
                Velocity = new Vector(1, 1.8, 0).Normalize() * 11.25
            };
            var gravity = new Vector(0, -0.1, 0);
            var wind    = new Vector(-0.01, 0, 0);
            var w       = new World {
                Gravity = gravity, Wind = wind
            };
            var c = new Canvas(900, 550);

            var tick = MotionHelper.Tick(w, p);

            while (tick.Position.X < c.Width && tick.Position.Y < c.Height && tick.Position.Y > 0)
            {
                c.WritePixel((int)tick.Position.X, c.Height - (int)tick.Position.Y, new Color(1, 0, 0));
                tick = MotionHelper.Tick(w, tick);
            }

            var ppm = c.ToPpm();

            File.WriteAllText(@"d:\zetes\projectile.ppm", ppm);
        }
示例#2
0
 public void Transform(PathTransform p)
 {
     for (int i = 0; i < points.Count; i++)
     {
         points[i] *= p.Scale;
         points[i] += p.Translation;
         points[i]  = MotionHelper.Rotate(points[i], p.Rotation, p.RotationOrigin);
     }
 }
示例#3
0
        /// <summary>
        /// Do work event handler.
        /// Turns until the robot sees a mark.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void _findMarkWorker_DoWork(object sender, DoWorkEventArgs e)
        {
            // reset the actual rotation
            var actualRotation = 0f;

            //init robot
            InitRobotPosition();

            // create a LandMarkDetectionProxy to start looking for marks
            using (var landMarkDetectionProxy = _connection.GetProxy <LandMarkDetectionProxy>())
            {
                landMarkDetectionProxy.subscribe(SUBSCRIBER_NAME);

                // create a MemoryProxy to read landmark information
                using (var memProxy = _connection.GetProxy <MemoryProxy>())
                {
                    // use one motion proxy
                    using (var motionProxy = _connection.GetProxy <MotionProxy>())
                    {
                        do
                        {// turn the robot 360 degrees
                            // reset the yaw to 0
                            MotionHelper.MoveHead(motionProxy, null, 0f, true);
                            // initialize the look direction
                            var startLookDirection   = this.StartLookDirection;
                            var currentLookDirection = this.StartLookDirection;
                            do
                            {// make the robot look left and right
                                // reset head pitch position to 0
                                MotionHelper.MoveHead(motionProxy, 0f, null, true);
                                do
                                { // make the robot look down
                                    var landMarks = this.TryGetLandMark(memProxy);
                                    if (landMarks != null && landMarks.ContainsKey(MarkID))
                                    {
                                        // align the robot to the landmark
                                        AlignWithMark(motionProxy, landMarks[MarkID]);

                                        // reload the mark information from memory because the robot moved
                                        landMarks = this.TryGetLandMark(memProxy);
                                        // should not be empty but just make sure
                                        if (landMarks != null && landMarks.ContainsKey(MarkID))
                                        {
                                            // save the result
                                            e.Result = new KeyValuePair <int, ArrayList>(MarkID, landMarks[MarkID]);
                                        }
                                    }
                                }while (e.Result == null && !_findMarkWorker.CancellationPending && LookDown(motionProxy));
                            }while (e.Result == null && !_findMarkWorker.CancellationPending &&
                                    LookAround(motionProxy, startLookDirection, ref currentLookDirection));
                        }while (e.Result == null && !_findMarkWorker.CancellationPending && Rotate(motionProxy, ref actualRotation));
                    }
                }

                landMarkDetectionProxy.unsubscribe(SUBSCRIBER_NAME);
            }
        }
示例#4
0
        private int LookForMarks(MotionProxy motionProxy)
        {
            List <string>  names = new List <string>();
            List <float[]> times = new List <float[]>();
            List <float[]> keys  = new List <float[]>();

            var time = 2f;

            MotionHelper.MoveHead(motionProxy, 0f, 0f, true);

            /*
             * names.Add("HeadYaw");
             * times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
             * keys.Add(new[] { 0.00149f, -0.27925f, -0.34826f, -0.47124f, -0.52360f, -0.28690f, -0.00940f, -0.00940f });
             */


            /* // absolute
             * names.Add("HeadPitch");
             * times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f});
             * keys.Add(new[] { 0f, 0.5f, 0f, 0f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f});
             *
             * names.Add("HeadYaw");
             * times.Add(new[] {   2f,     4f,     6f,     9f,     15f,    18f,    21f,    22f,    28f,    29f,    35f,    38f});
             * keys.Add(new[] {    0f,     0f,     0f,     -1f,    1f,     0f,     -1f,    -1f,    1f,     1f,     -1f,    0f });
             *
             *
             * var methodID = motionProxy.post.angleInterpolation(names, keys, times, true);
             */

            // relative
            names.Add("HeadPitch");
            times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f });
            keys.Add(new[]  { 0.5f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f });

            names.Add("HeadYaw");
            times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f });
            keys.Add(new[] { 0f, 0f, -1f, 1f, 1f, -1f, -1f, 1f, 0f });


            var methodID = motionProxy.post.angleInterpolation(names, keys, times, false);

            return(methodID);
        }
示例#5
0
        /// <summary>
        /// Aligns the robot with the landmark
        /// </summary>
        /// <param name="motionProxy"></param>
        /// <param name="markInfo"></param>
        private void AlignWithMark(MotionProxy motionProxy, ArrayList markInfo)
        {
            // get mark shape infor
            var shapeInfo = LandMarkHelper.Instance.GetShapeInfo(markInfo);

            // get current yaw position
            var currentYawPosition = motionProxy.getAngles("HeadYaw", true);

            if (!currentYawPosition.IsNullOrEmpty() && shapeInfo[1] != null)
            {
                // move pitch to landmark
                MotionHelper.MoveHead(motionProxy, shapeInfo[2] as float?, null, false);
                // calculate robot rotation
                var rotation = currentYawPosition[0] + (float)shapeInfo[1];
                motionProxy.moveTo(0f, 0f, rotation);
                // reset yaw
                MotionHelper.MoveHead(motionProxy, null, 0f, true);
            }
        }
示例#6
0
        public void ProjectileInWorld()
        {
            var p = new Projectile {
                Position = new Point(0, 1, 0), Velocity = new Vector(1, 1, 0).Normalize()
            };
            var w = new World {
                Gravity = new Vector(0, -0.1, 0), Wind = new Vector(-0.01, 0, 0)
            };

            var expected = new Projectile
            {
                Position = new Point(0.70710678118654746, 1.7071067811865475, 0),
                Velocity = new Vector(0.69710678118654745, 0.60710678118654748, 0)
            };

            var actual = MotionHelper.Tick(w, p);

            Assert.Equal(expected.Position, actual.Position);
            Assert.Equal(expected.Velocity, actual.Velocity);
        }
示例#7
0
        private void Lift()
        {
            List <string>  names = new List <string>();
            List <float[]> times = new List <float[]>();
            List <float[]> keys  = new List <float[]>();

            var time          = 1.5f;
            var shoulderPitch = MotionHelper.DegreeToRadian(40f);

            #region Positions
            names.Add("LShoulderPitch");
            times.Add(new[] { time });
            keys.Add(new[] { shoulderPitch });

            names.Add("RShoulderPitch");
            times.Add(new[] { time });
            keys.Add(new[] { shoulderPitch });
            #endregion

            AngleInterpolation(names, keys, times, true);
        }
示例#8
0
        private void InitHands()
        {
            List <string>  names = new List <string>();
            List <float[]> times = new List <float[]>();
            List <float[]> keys  = new List <float[]>();

            var time      = 1.5f;
            var elbowRoll = MotionHelper.DegreeToRadian(20f);

            #region 1
            names.Add("LElbowRoll");
            times.Add(new[] { time });
            keys.Add(new[] { elbowRoll });

            names.Add("RElbowRoll");
            times.Add(new[] { time });
            keys.Add(new[] { -elbowRoll });
            #endregion

            AngleInterpolation(names, keys, times, true);
        }
示例#9
0
        private bool LookDown(MotionProxy motionProxy)
        {
            // get initial head pitch position
            var initialHeadPitchPosition = motionProxy.getAngles("HeadPitch", true);

            MotionHelper.MoveHead(motionProxy, this.HeadPitchStepAmount, null, false);

            // get current pitch position
            var currentHeadPitchPosition = motionProxy.getAngles("HeadPitch", true);

            // return true if the current position changed
            if (!initialHeadPitchPosition.IsNullOrEmpty() && !currentHeadPitchPosition.IsNullOrEmpty() &&
                initialHeadPitchPosition[0] != currentHeadPitchPosition[0])
            {
                return(true);
            }
            else
            {
                // reset pitch position
                MotionHelper.MoveHead(motionProxy, 0f, null, true);
                return(false);
            }
        }
示例#10
0
        /// <summary>
        /// Makes the robot look left and right
        /// </summary>
        /// <param name="motionProxy"></param>
        /// <returns></returns>
        private bool LookAround(MotionProxy motionProxy, LookDirection startLookDirection, ref LookDirection currentLookDirection)
        {
            // get initial head yaw position
            var initialHeadYawPositionArray = motionProxy.getAngles("HeadYaw", true);

            if (initialHeadYawPositionArray.IsNullOrEmpty())
            {
                // should not happen
                return(false);
            }

            var   initialHeadYawPosition = initialHeadYawPositionArray[0];
            float maxHeadYaw, step;
            float marginOfError    = 0.01f;
            var   maxReached       = false;
            var   directionChanged = currentLookDirection != startLookDirection;

            if (currentLookDirection == LookDirection.Left)
            {
                maxHeadYaw = this.MaxHeadYaw;
                step       = this.HeadYawStepAmount;
                // check if it's the last movement and ajust the step if it is
                if (step + initialHeadYawPosition >= maxHeadYaw)
                {
                    step = maxHeadYaw - initialHeadYawPosition;
                }
                // if it reached the maximum value we need to change the direction and update the step
                if (step - marginOfError <= 0)
                {
                    maxReached = true;
                    step       = -this.HeadYawStepAmount;
                }
            }
            else
            {
                // if it's the right direction we need to have negative values for max and step angles
                maxHeadYaw = -this.MaxHeadYaw;
                step       = -this.HeadYawStepAmount;
                // check if it's the last movement and ajust the step if it is
                if (step + initialHeadYawPosition <= maxHeadYaw)
                {
                    step = maxHeadYaw - initialHeadYawPosition;
                }
                // if it reached the maximum value we need to change the direction and update the step
                if (step + marginOfError >= 0)
                {
                    maxReached = true;
                    step       = this.HeadYawStepAmount;
                }
            }

            if (maxReached && directionChanged)
            {
                // no more movement allowed
                // reset head yaw position to 0
                MotionHelper.MoveHead(motionProxy, null, 0f, true);
                return(false);
            }
            else
            {
                MotionHelper.MoveHead(motionProxy, null, step, maxReached);
                if (maxReached)
                {
                    currentLookDirection = currentLookDirection == LookDirection.Left ? LookDirection.Right : LookDirection.Left;
                }
                return(true);
            }
        }
示例#11
0
        public void auto()
        {
            NowModel        = ModelsHelper.NowModels;
            string[,] array = new string[100, 20];
            string file = Application.StartupPath + "\\data\\" + NowModel;

            if (!File.Exists(file))
            {
                PushMesss("文件不存在File not Find");
                return;
            }
            int Rows = 0;

            // DataGridView DGV = new DataGridView();
            ProfileHelper.ReadExcel1(file, ref array, ref Rows);
            // array =ProfileHelper. GetTable(DGV);
            //       int a = DGV.Rows.Count;

            bool first = true;

            //自动时
            if (ucSwitch7.Checked)
            {
                while (!ucSwitch2.Checked || first)    //根据条件进行循环
                {
                    for (int i = 0; i < Rows - 1; i++)
                    {
                        if (array[i, 1] == "Arc_Target")
                        {
                            continue;
                        }
                        var mess = Getmess(array, i);
                        PushMesss(mess);
                        Thread.Sleep(100);

                        Global.Cdefault data1 = new Global.Cdefault();
                        data1.setup  = i;
                        data1.table  = array;
                        data1.tryRun = !ucSwitch1.Checked;
                        MotionHelper.MotionFun(data1);
                        first = false;
                        // ActionThread.Abort();
                    }
                    button1.Image = global::LuoGuoFeng.Properties.Resources.暂停;
                }
            }
            else                 //单步
            {
                //
                Setup = Convert.ToInt32(numericUpDown1.Value);
                PushMesss((Setup) + " line " + array[Setup, 1] + " " + array[Setup, 2] + " " + array[Setup, 3] + " " + array[Setup, 4]);
                Global.Cdefault data1 = new Global.Cdefault();
                data1.setup  = Setup;
                data1.table  = array;
                data1.tryRun = !ucSwitch1.Checked;
                MotionHelper.MotionFun(data1);
                if (Setup < Rows - 2)
                {
                    Setup++;
                }
                else
                {
                    Setup = 0;
                }
                ucTextBoxexMess(Setup);
                button1.Image = global::LuoGuoFeng.Properties.Resources.暂停;
            }
        }