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); }
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); } }
/// <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); } }
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); }
/// <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); } }
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); }
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); }
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); }
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); } }
/// <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); } }
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.暂停; } }