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)));
}
