public void When_converting_millimeters_Then_works(decimal distanceValue, Type type, decimal expected) { var distance = new Millimeters(distanceValue); var actual = distance.ConvertTo(type); Assert.That(actual.Value, Is.EqualTo(expected).Within(0.000001M)); }
public override void MoveRelAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion) { Millimeters mmSpeed = new Millimeters(speed); try { lock (this) { int axisNumber = axis.AxisNo + 1; double percent = mmSpeed; double dPos = axis.TargetPosition; string cmd = string.Format("DRIVEI({0},{1:F2}),S={2:F2}", axisNumber, dPos, percent); SendCommand(cmd); string ret = WaitForCommand(); CheckOKReplied("DRIVEI", ret, "RUN"); ret = WaitForCommand(30000); CheckOKReplied("WaitMoveDone", ret, "END"); } lock (this) { OnUpdateCurrentPositions(); } } catch (Exception ex) { //U.LogAlarmPopup(ex, "Move Absolute Command fail {0}", this.Nickname); throw new MCoreExceptionPopup(ex, "Move Absolute Command fail {0}", this.Nickname); } }
public override void MoveLinearXY(AxesBase axes, MLengthSpeed speed) { Millimeters mmSpeed = new Millimeters(speed); try { double xPos = (axes.ChildArray[0] as RealAxis).TargetPosition; double yPos = (axes.ChildArray[1] as RealAxis).TargetPosition; double zPos = (axes.ChildArray[2] as RealAxis).TargetPosition; double rPos = (axes.ChildArray[3] as RealAxis).TargetPosition; double percent = mmSpeed; lock (this) { string cmd = string.Format("MOVE P,{0:F2} {1:F2} {2:F2} {3:F2} 0.0 0.0 {4},S={5:F2}", xPos, yPos, zPos, rPos, this.AxesMoveMode, percent); SendCommand(cmd); string ret = WaitForCommand(); CheckOKReplied("MOVE P", ret, "RUN"); ret = WaitForCommand(30000); CheckOKReplied("WaitMoveDone", ret, "END"); } lock (this) { OnUpdateCurrentPositions(); } } catch (Exception ex) { //U.LogAlarmPopup(ex, "MoveLinearXY Command fail {0}", this.Nickname); throw new MCoreExceptionPopup(ex, "MoveLinearXY Command fail {0}", this.Nickname); } }
/// <summary> /// Snap all triggers to the grid /// </summary> /// <param name="gridSize"></param> public void SnapTriggers(Millimeters gridSize) { foreach (GTriggerPoint triggerPt in _listTriggerPts) { triggerPt.Distance = U.Snap(triggerPt.Distance, gridSize); } }
/// <summary> /// Return the Relative point from the line origin /// With no speed arg, we can only be looking at Endpoint /// </summary> /// <param name="distance"></param> /// <param name="applyToolOffset"></param> /// <param name="applyTrajLead"></param> /// <returns></returns> public PointF RelVectorPoint(Millimeters distance, bool applyToolOffset, bool applyTrajLead) { float X = (float)(distance * Math.Cos(Loc.Yaw)); float Y = (float)(distance * Math.Sin(Loc.Yaw)); return(new PointF(X, Y)); }
/// <summary> /// Reset all values to default /// </summary> public void Reset() { _mmX = 0.0; _mmY = 0.0; _mmZ = 0.0; _yaw = 0.0; _xyyawMode = eMode.Relative; _zMode = eMode.Relative; }
/// <summary> /// Set the XY Offset values /// Assumes parent contains absolute location /// </summary> /// <param name="xCurrentPos"></param> /// <param name="yCurrentPos"></param> public void ApplyXYOffset(Millimeters xCurrentPos, Millimeters yCurrentPos) { // Get parent if (Container != null) { // And set it Loc.X.Val = xCurrentPos - Container.Loc.X; Loc.Y.Val = yCurrentPos - Container.Loc.Y; } }
///// <summary> ///// Get the absolute robot coordinates (Used for End point) ///// </summary> ///// <param name="distance">Distance along the Loc vector (angle of Yaw) </param> ///// <param name="applyToolOffset"> Add the tool offset to adjust for tool moounting position</param> ///// <param name="applyTrajLead">Subtract to the distance according to the delay and speed.</param> ///// <returns></returns> //public G3DPoint GetRobotLoc(Millimeters distance, bool applyToolOffset, bool applyTrajLead) //{ // return GetRobotLoc(distance, applyToolOffset, applyTrajLead, Speed); //} /// <summary> /// Get the absolute robot coordinates (Used for Trigger Point) /// </summary> /// <param name="distance">Distance along the Loc vector (angle of Yaw) </param> /// <param name="applyToolOffset"> Ad the too offset to adjust for tool moounting position</param> /// <param name="applyTrajLead">Subtract to the distance according to the delay and speed.</param> /// <returns></returns> public G3DPoint GetRobotLoc(Millimeters distance, bool applyToolOffset, bool applyTrajLead) { MillimetersPerSecond speed = Speed; if (applyTrajLead) { distance -= speed * GetToolDelay().ToSeconds; } return(GetRobotLoc(distance, applyToolOffset)); }
private void OnChangedValue(Millimeters val) { if (TriggerQue != null) { MillimeterEventHandler delCallback = TriggerQue.GetNextCallback(); if (delCallback != null) { delCallback(val); } } }
public override void MoveRel(Millimeters position) { if (position.Val > 0) { MoveScale(0.75); } else { MoveScale(0.25); } }
/// <summary> /// Transform a point Z to the /// </summary> /// <param name="Z"></param> /// <returns></returns> public Millimeters TransformZ(Millimeters Z) { G3DCompBase reference = Container; Z += Loc.Z; if (reference == null || Loc.ZMode == G3DPoint.eMode.Absolute) { return(Z); } return(reference.TransformZ(Z)); }
/// <summary> /// Move up safely in Z, then move X and Y, then move to destination in Z /// </summary> /// <param name="mmX"></param> /// <param name="mmY"></param> /// <param name="mmZ"></param> public void MoveXYSafeZ(Millimeters mmX, Millimeters mmY, Millimeters mmZ) { // Temp //AxisX.MoveAbs(dest.X); //AxisY.MoveAbs(dest.Y); AxisZ.MoveAbs(SafeZHeight); AxisX.SetTarget(mmX); AxisY.SetTarget(mmY); _motionSystem.MoveLinearXY(this, AxisX.DefaultSpeed); AxisZ.MoveAbs(mmZ); }
/// <summary> /// Handle an incoming raw value /// </summary> /// <param name="dVal"></param> public override void ApplyRawValue(double dVal) { base.ApplyRawValue(dVal); if (XferFunc != null) { Value = XferFunc.Evaluate(dVal); } else { Value = dVal; } }
/// <summary> /// Locate the position where the boolean toggles and stop at that point /// </summary> /// <param name="boolInput"></param> /// <param name="tolerance"></param> /// <param name="speed"></param> /// <returns>Returns the position of the flag</returns> public Millimeters FindSignalPos(BoolInput boolInput, Millimeters tolerance, MillimetersPerSecond speed) { if (boolInput == null) { U.LogPopup("Expected to find BoolInput in FindSignalPos of {0}}'", Nickname); } else { FindSignalChange(boolInput, tolerance, speed); FindSignalChange(boolInput, -tolerance, speed / 2); } return(CurrentPosition); }
/// <summary> /// Move to the absolute popsition at the specified speed /// </summary> /// <param name="position"></param> /// <param name="speed"></param> /// <param name="waitForCompletion"></param> public override void MoveAbs(Millimeters position, MillimetersPerSecond speed, bool waitForCompletion) { if (!Homed && !_motionSystem.Simulated) { U.LogPopup("Please Home the axis first"); return; } MoveDone = false; // Set tgt motor counts base.MoveAbs(position); // Now do the move _motionSystem.MoveAbsAxis(this, speed, waitForCompletion); }
/// <summary> /// Ensure minimum spacing /// </summary> /// <param name="spacing"></param> public void MinSpacingTriggers(Millimeters spacing) { Millimeters prevPt = -spacing; SortTriggers(); foreach (GTriggerPoint triggerPt in _listTriggerPts) { if ((triggerPt.Distance - prevPt) < spacing) { triggerPt.Distance = prevPt + spacing; } prevPt = triggerPt.Distance; } }
/// <summary> /// Snap the line begin and endpoints to the grid /// </summary> /// <param name="gridSize"></param> public void Snap(Millimeters gridSize) { G3DPoint ptEnd = ExtendPoint(EndPoint.Distance); Loc.X = U.Snap(Loc.X, gridSize); Loc.Y = U.Snap(Loc.Y, gridSize); ptEnd.X = U.Snap(ptEnd.X, gridSize); ptEnd.Y = U.Snap(ptEnd.Y, gridSize); PointF ptLoc = Loc.ToPointF; PointF ptfEnd = ptEnd.ToPointF; PointF ptDiff = U.Subtract(ptfEnd, ptLoc); EndPoint.Distance = U.GetDistance(ptDiff); Loc.Yaw = U.GetAngle(ptDiff); }
/// <summary> /// Get extended point in XY /// </summary> /// <param name="distance"></param> /// <returns></returns> public G3DPoint ExtendPoint(Millimeters distance) { G3DPoint pt = new G3DPoint(Loc); //double cos = Math.Cos(Loc.Yaw); //double sin = Math.Sin(Loc.Yaw); //double cosx = distance * cos; //double siny = distance * sin; //double x = pt.X + cosx; //double y = pt.Y + siny; //pt.X = x; //pt.Y = y; pt.X = pt.X + distance * Math.Cos(Loc.Yaw); pt.Y = pt.Y + distance * Math.Sin(Loc.Yaw); return(pt); }
/// <summary> /// Get the absolute robot coordinates (Used for Line elements: Trigger) /// </summary> /// <param name="distance">Distance along the Loc vector (angle of Yaw) </param> /// <param name="applyToolOffset"> Ad the too offset to adjust for tool moounting position</param> /// <returns></returns> public G3DPoint GetRobotLoc(Millimeters distance, bool applyToolOffset) { if (Loc.XYYawMode == G3DPoint.eMode.Absolute) { double x = distance * Math.Cos(Loc.Yaw); double y = distance * Math.Sin(Loc.Yaw); if (Loc.ZMode == G3DPoint.eMode.Absolute) { return(new G3DPoint(Loc.X + x, Loc.Y + y, Loc.Z)); } else { return(new G3DPoint(Loc.X + x, Loc.Y + y, 0)); } } G3DPoint pt = TransformXY(distance, 0); if (Loc.ZMode == G3DPoint.eMode.Absolute) { pt.Z.Val = Loc.Z; } else { pt.Z.Val = TransformZ(0); } if (applyToolOffset && ToolComp != null) { if (_offsetComp == null && _offsetCompCheck == false) { _offsetComp = ToolComp.FilterByTypeSingle <G3DCompBase>(); _offsetCompCheck = true; } if (_offsetComp != null) { pt.X += _offsetComp.Loc.X; pt.Y += _offsetComp.Loc.Y; if (Loc.ZMode == G3DPoint.eMode.Relative) { pt.Z += _offsetComp.Loc.Z; } } } return(pt); }
/// <summary> /// MoveRel /// </summary> /// <param name="position"></param> /// <param name="speed"></param> /// <param name="waitForCompletion"></param> public override void MoveRel(Millimeters position, MillimetersPerSecond speed, bool waitForCompletion) { if (!Homed && !_motionSystem.Simulated) { U.LogPopup("Please Home the axis first"); return; } MoveDone = false; // Set tgt motor counts if (((position > 0) && ((CurrentPosition + position) < MaxLimit)) || ((position < 0) && ((CurrentPosition + position) > MinLimit))) { this.TargetMotorCounts = ToMotorCounts(position); // Now do the move _motionSystem.MoveRelAxis(this, speed, waitForCompletion); } }
/// <summary> /// Transform a point (x,y) to the /// </summary> /// <param name="X"></param> /// <param name="Y"></param> /// <returns></returns> public G3DPoint TransformXY(Millimeters X, Millimeters Y) { G3DCompBase reference = Container; if (Loc.XYYawMode == G3DPoint.eMode.Absolute) { return(new G3DPoint(Loc.X + X, Loc.Y + Y, 0.0)); } G3DPoint outerPt = outerPt = new G3DPoint(Loc); double dist = Math.Sqrt(X * X + Y * Y); double theta = Math.Atan2(Y, X); outerPt.X += dist * Math.Cos(theta + Loc.Yaw); outerPt.Y += dist * Math.Sin(theta + Loc.Yaw); if (reference == null) { return(outerPt); } return(reference.TransformXY(outerPt.X, outerPt.Y)); }
private void FindSignalChange(BoolInput boolInput, Millimeters distance, MillimetersPerSecond speed) { bool origVal = boolInput.Value; //System.Diagnostics.Debug.WriteLine(string.Format("About to move axis at {0}, {1}, {2}, {3}", TargetPosition, CurrentPosition, InPosition, boolInput.Value)); MoveRel(distance, speed, false); U.SleepWithEvents(200); while (origVal == boolInput.Value) { //System.Diagnostics.Debug.WriteLine(string.Format("Moving axis at {0}, {1}, {2}, {3}", TargetPosition, CurrentPosition, InPosition, boolInput.Value)); if (MoveDone) { StopAxis(); throw new MCoreExceptionPopup("Error. Expected '{0}' signal to change.", boolInput.Nickname); } U.SleepWithEvents(20); } //System.Diagnostics.Debug.WriteLine(string.Format("Stopping axis at {0}, {1}, {2}, {3}", TargetPosition, CurrentPosition, InPosition, boolInput.Value)); StopAxis(); U.SleepWithEvents(1000); //System.Diagnostics.Debug.WriteLine(string.Format("Axis Stopped at {0}, {1}, {2}, {3}", TargetPosition, CurrentPosition, InPosition, boolInput.Value)); }
/// <summary> /// Convert the location to pixel space, (Used for TriggerPts) /// </summary> /// <param name="applyTrajLead"></param> public PointF ToPixel(Millimeters distance, bool applyToolOffset, bool applyTrajLead) { return(AbsPointToPixel(GetRobotLoc(distance, applyToolOffset, applyTrajLead))); }
/// <summary> /// Get the absolute robot coordinates for the Loc Used for Begin /// Tool Offset and trajectory leads are applied automatically /// </summary> /// <param name="distance">Distance along the Loc vector (angle of Yaw) </param> /// <returns></returns> public G3DPoint GetRobotLoc(Millimeters distance) { return(GetRobotLoc(distance, true, true)); }
public void AutoFillTriggers(int numDots) { int minDots = 1; if (numDots > 0) { List <TrigGrp> trGrps = new List <TrigGrp>(); TrigGrp trCurGrp = null; foreach (GTriggerPoint tPt in _listTriggerPts) { tPt.LeadTime = 0.0; Millimeters thisDist = new Millimeters(tPt.Distance.Val); if (trCurGrp == null) { trCurGrp = new TrigGrp(); trGrps.Add(trCurGrp); } if (tPt.Anchor && minDots == 1) { minDots = 2; } if (tPt.Anchor && trCurGrp.Start != null) { // End of a group. Signal to start a new one trCurGrp.End = thisDist; trCurGrp = null; } else { // Contiue to add to a group if (trCurGrp.Start == null) { trCurGrp.Start = thisDist; } trCurGrp.End = thisDist; } } if (trGrps.Count == 0) { trGrps.Add(new TrigGrp() { Start = 0.0, End = XYLength }); } // Handle case for not enough dots int numEachGrp = Math.Max(minDots, numDots / trGrps.Count); numDots = numEachGrp * trGrps.Count; int iTrg = 0; foreach (TrigGrp trGrp in trGrps) { double gap = (trGrp.End - trGrp.Start) / (numEachGrp - 1); for (int i = 0; i < numEachGrp; i++, iTrg++) { bool bAnchor = i == 0 || (i == numEachGrp - 1); double thisDist = double.IsInfinity(gap) || double.IsNaN(gap) ? XYLength / 2.0 : (double)(gap * i + trGrp.Start); if (iTrg < _listTriggerPts.Count) { _listTriggerPts[iTrg].Distance.Val = thisDist; _listTriggerPts[iTrg].Anchor = bAnchor; } else { GTriggerPoint newTriggerPt = CreateTrigger(); newTriggerPt.Distance.Val = thisDist; newTriggerPt.Anchor = bAnchor; _listTriggerPts.Add(newTriggerPt); newTriggerPt.Initialize(this); } } } } // Get rid of excess while (_listTriggerPts.Count > numDots) { RemoveTrigger(_listTriggerPts[numDots]); } U.LogChange(string.Format("{0}.AutoFillTriggers", Nickname)); SortTriggers(); FireOnChangedEvent(); }
/// <summary> /// Manual creation constructor /// </summary> /// <param name="line"></param> /// <param name="mmDistance"></param> public GTriggerPoint(GLine line, Millimeters mmDistance) : base(line) { Distance = mmDistance; AssignTriggerID(); }
/// <summary> /// Move this point a distance along Theta direction /// </summary> /// <param name="mmDist"></param> public void Move(Millimeters mmDist) { X += mmDist * Math.Cos(Yaw); Y += mmDist * Math.Sin(Yaw); }
/// <summary> /// Home an Axis /// </summary> /// <param name="axis"></param> public override void MoveAbsAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion) { Millimeters mmSpeed = new Millimeters(speed); MoveAbsAxis(axis, axis.TargetPosition, (float)mmSpeed, waitForCompletion); }
/// <summary> /// Convert the point to pixel /// </summary> /// <param name="applyTrajLead"></param> /// <param name="applyToolOffset"></param> /// <param name="lenFarther"></param> /// <returns></returns> public PointF ToPixel(bool applyToolOffset, bool applyTrajLead, Millimeters lenFarther) { return(_line.ToPixel(DistanceWithLead + lenFarther, applyToolOffset, applyTrajLead)); }
public PointF ToPixel(Millimeters X, Millimeters Y) { return(AbsPointToPixel(TransformXY(X, Y))); }