/// <summary> /// Compares two joints based on a comparision function /// </summary> /// <param name="a">lhs</param> /// <param name="b">rhs</param> /// <param name="c">Comparision Constrains</param> /// <returns></returns> public bool JointCompare(UJoint[] joints, JointId a, JointId b, PositionCompare c) { if (joints == null) { Debug.LogError("joints was null"); return(false); } Vector3 posA = joints[(int)a].Position; Vector3 posB = joints[(int)b].Position; Vector3 posDiff = c.posDiff; bool3 xyz = new bool3(true); for (int i = 0; i < c.comp.Length; i++) { switch (c.comp[i]) { case Compare.NONE: break; case Compare.POS: xyz[i] = posB[i] - posDiff[i] > posA[i]; break; case Compare.NEG: xyz[i] = posB[i] - posDiff[i] < posA[i]; break; } } return(xyz.x && xyz.y && xyz.z); }
private async void buttonCheck_Click(object sender, EventArgs e) { double.TryParse(this.textBoxDegreePerSecond.Text, out var degreePerSecond); int.TryParse(this.textBoxDriveMilliseconds.Text, out var driveMilliseconds); int.TryParse(this.textBoxStartPosition.Text, out var startPosition); var items = new List <PositionCompare>(); var bindingSource = new BindingSource(); bindingSource.DataSource = items; this.dataGridView2.DataSource = bindingSource; var axisType = (AxisType)this.comboBoxAxisType.SelectedItem; await Task.Run(() => { using (var logic = new SpeedTestLogic(this._communication, axisType)) { logic.LoadSpeedTableEneo(); var breakpoints = new List <double>(); for (var j = 0; j < 10; j++) { var destinationPosition = startPosition + (degreePerSecond *driveMilliseconds / 1000); for (var i = 0; i < 3; i++) { logic.GoToStartPosition(startPosition); logic.Move(degreePerSecond, driveMilliseconds); Thread.Sleep(300); var lastPosition = logic.LastPosition; breakpoints.Add(lastPosition); } var item = new PositionCompare(); item.DegreePerSecond = degreePerSecond; item.MoveTime = driveMilliseconds; item.ActualPosition = breakpoints.Average(); item.TargetPosition = destinationPosition; items.Add(item); this.dataGridView2.Invoke(o => ((BindingSource)o.DataSource).ResetBindings(true)); degreePerSecond += 0.5; breakpoints.Clear(); } } }); }
private async void buttonCheck_Click(object sender, EventArgs e) { this.buttonCheck.Enabled = false; this.buttonAbortQuickCheck.Enabled = true; this._cancellationTokenSource?.Dispose(); this._cancellationTokenSource = new CancellationTokenSource(); double.TryParse(this.textBoxDegreePerSecond.Text, out var degreePerSecond); int.TryParse(this.textBoxDriveMilliseconds.Text, out var driveMilliseconds); int.TryParse(this.textBoxStartPosition.Text, out var startPosition); var items = new List <PositionCompare>(); var bindingSource = new BindingSource { DataSource = items }; this.dataGridViewQuickCheck.DataSource = bindingSource; var axisType = (AxisType)this.comboBoxAxisType.SelectedItem; await Task.Run(async() => { using (var logic = new SpeedTestLogic(this._communication, this._panTiltControlType, axisType)) { logic.GoToStartPosition(startPosition); var breakpoints = new List <double>(); for (var j = 0; j < 80; j++) { if (this._cancellationTokenSource.IsCancellationRequested) { return; } var destinationPosition = startPosition + (degreePerSecond *driveMilliseconds / 1000); for (var i = 0; i < 3; i++) { logic.GoToStartPosition(startPosition); logic.Move(degreePerSecond, driveMilliseconds); await Task.Delay(300, this._cancellationTokenSource.Token); var lastPosition = logic.LastPosition; breakpoints.Add(lastPosition); if (this._cancellationTokenSource.IsCancellationRequested) { return; } } var item = new PositionCompare { DegreePerSecond = degreePerSecond, MoveTime = driveMilliseconds, ActualPosition = Math.Round(breakpoints.Average(), 2), TargetPosition = destinationPosition }; items.Add(item); this.dataGridViewQuickCheck.Invoke(o => ((BindingSource)o.DataSource).ResetBindings(true)); degreePerSecond += 0.5; breakpoints.Clear(); } } }, this._cancellationTokenSource.Token).ContinueWith(t => { }); this.buttonCheck.Enabled = true; this.buttonAbortQuickCheck.Enabled = false; }
public bool JointCompare(JointId a, JointId b, PositionCompare c) => JointCompare(trackedJoints, a, b, c);