/// <summary>
/// For robot to go.
/// </summary>
/// <param name="capPos"></param>
/// <returns></returns>
public static Pose ConvertToPose(CapturePosition capPos)
{
return(new Pose()
{
X = capPos.XPosition,
Y = capPos.YPosition,
Z = capPos.ZPosition,
A = capPos.Angle,
});
}
public override int GetHashCode()
{
unchecked
{
int hashCode = Owner.GetHashCode();
hashCode = (hashCode * 397) ^ (From != null ? From.GetHashCode() : 0);
hashCode = (hashCode * 397) ^ (To != null ? To.GetHashCode() : 0);
hashCode = (hashCode * 397) ^ (CapturePosition != null ? CapturePosition.GetHashCode() : 0);
return(hashCode);
}
}
/// <summary>
/// Get vision result and add user offsets.
/// </summary>
/// <param name="pos"></param>
/// <returns></returns>
public Pose GetVisionResult(CapturePosition pos)
{
var visionOffset = GetRawVisionResult(pos);
var userOffset = GetCapturePositionOffset(pos.CaptureId);
return(new Pose()
{
X = visionOffset.XOffset + userOffset.XPosition,
Y = visionOffset.YOffset + userOffset.YPosition,
Z = GetZHeight(pos.CaptureId) + userOffset.ZPosition,
A = visionOffset.ROffset + userOffset.Angle,
});
}
public AxisOffset RequestVisionCalibration(CapturePosition capturePosition, int timeoutMs = 10000)
{
if (Connected == false)
{
throw new Exception("Vision server not connected.");
}
//log.Info("Sending to server:" + Environment.NewLine +
// Helper.ConvertToJsonString(capturePosition));
//Clean the last unhandled vision response.
while (ResultFound(capturePosition.CaptureId, out AxisOffset offsetResultDump))
{
}
AxisOffset offsetResult = null;
string JsonCommand = Handle.Instance.ObjToJsonstring(capturePosition);
Send(JsonCommand);
var stopwatch = new Stopwatch();
stopwatch.Start();
while (ResultFound(capturePosition.CaptureId, out offsetResult) == false)
{
if (stopwatch.ElapsedMilliseconds > timeoutMs)
{
throw new Exception("RequestVisionCalibration fail, no response from Vision");
}
//_visionResponseManualResetEvent.Reset();
//_visionResponseManualResetEvent.WaitOne();
Thread.Sleep(50);
}
if (offsetResult.ResultOK == false)
{
//log.Info("No Recieved to server of :" + Environment.NewLine +
//Helper.ConvertToJsonString(capturePosition));
throw new VisionException("Vision NG")
{
CaptureId = capturePosition.CaptureId,
};
}
//log.Info("Recieved to server:" + Environment.NewLine +
// Helper.ConvertToJsonString(offsetResult));
return(offsetResult);
}
private int GetFramePosition(double position, CapturePosition units)
{
switch (units)
{
case CapturePosition.Milliseconds:
return((int)(position * captureFps / 1000.0));
case CapturePosition.AviRatio:
var frameCount = captureCache.GetProperty(CaptureProperty.FrameCount);
return((int)(position * frameCount));
case CapturePosition.Frames:
default: return((int)position);
}
}
public AxisOffset GetRawVisionResult(CapturePosition capturePosition)
{
if (VisionSimulateMode)
{
return(new AxisOffset()
{
XOffset = capturePosition.XPosition + 0,
YOffset = capturePosition.YPosition,
});
}
else
{
return(_vision.RequestVisionCalibration(capturePosition));
}
}
/// <summary>
/// Request vision server data.
/// </summary>
/// <param name="capturePosition"></param>
/// <returns></returns>
public AxisOffset GetRawVisionResult(CapturePosition capturePosition)
{
if (VisionSimulateMode)
{
return(new AxisOffset()
{
XOffset = capturePosition.XPosition + 65, //Camera to sucker distance.
YOffset = capturePosition.YPosition,
});
}
else
{
//Todo fail check.
return(_vision.RequestVisionCalibration(capturePosition));
}
}
public AxisOffset GetRawVisionResult(CapturePosition capturePosition, int retryTimes)
{
int retryCount = 0;
do
{
try
{
return(GetRawVisionResult(capturePosition));
}
catch (Exception)
{
retryCount++;
}
} while (retryCount < 3);
throw new VisionException("Vision fail three times.");
}
public Part GetNextPartForLoad(Part part)
{
//Assume
int xIndex, yIndex;
xIndex = part.XIndex + 1;
yIndex = part.YIndex;
double x = part.CapturePos.XPosition + XOffset;
double y = part.CapturePos.YPosition;
//Go to next line.
if (part.XIndex == ColumneCount - 1)
{
//Go to next line, index change.
xIndex = 0; yIndex = yIndex + 1;
//Coordinate change.
x = BaseCapturePosition.XPosition;
y = y + YIncreaseDirection * YOffset;
}
if (part.YIndex == RowCount - 1 && part.XIndex == ColumneCount - 1)
{
NeedChanged = true;
}
CapturePosition newCapture = new CapturePosition()
{
CaptureId = part.CapturePos.CaptureId,
XPosition = x,
YPosition = y,
ZPosition = BaseCapturePosition.ZPosition
};
Pose pose = new Pose()
{
Z = BaseCapturePosition.ZPosition,
};
return(new Part()
{
CapturePos = newCapture, TargetPose = pose, XIndex = xIndex, YIndex = yIndex
});
}
/// <summary>
/// Try to get vision result less than three times.
/// </summary>
/// <param name="capPos"></param>
/// <param name="retryTimes"></param>
/// <returns></returns>
public Pose GetVisionResult(CapturePosition capPos, int retryTimes = 3)
{
int retryCount = 0;
do
{
try
{
return(GetVisionResult(capPos));
}
catch (Exception)
{
retryCount++;
}
} while (retryCount < 3);
throw new VisionException()
{
CaptureId = capPos.CaptureId
};
}
public void MoveToTarget(CapturePosition target, MoveModeAMotor mode, ActionType type = ActionType.Load)
{
var tar = Helper.ConvertToPose(target);
MoveTo(tar, MoveModeAMotor.Relative);
}
/// <summary>
/// Move to capture position, angle rotation is not included.
/// </summary>
/// <param name="target"></param>
public void MoveToCapture(CapturePosition target)
{
var tar = Helper.ConvertToPose(target);
MoveTo(tar, MoveModeAMotor.Abs);
}
public void MoveToTarget(CapturePosition target, MoveModeAMotor mode, ActionType type)
{
throw new NotImplementedException();
}
public void MoveToCapture(CapturePosition target)
{
var tar = Helper.ConvertToPose(target);
MoveTo(tar);
}
protected override void Move()
{
CapturePosition tmp = CamCommunicator.CalcMovementPosition(Cam);
if (!tmp.valid)
{
return;
}
Vector3 originalPosition = Cam.ScreenToWorldPoint(tmp.position);
if (enableHelper)
{
//get distance to point to move
Vector2 tmp2D = new Vector2(originalPosition.x, originalPosition.y);
RaycastHit2D ray = Physics2D.Raycast(transform.position, originalPosition - transform.position, helperDistance);
if (!ray)
{
//get all colliders in range
colls = Physics2D.OverlapCircleAll(transform.position, helperDistance);
//Build "view-Frustum" in direction of point to move
orthogonale = new Vector2((originalPosition - transform.position).y, -(originalPosition - transform.position).x);
triangleA = transform.position;
triangleB = tmp2D + orthogonale.normalized * helper;
triangleC = tmp2D - orthogonale.normalized * helper;
abDist = Vector2.Distance(triangleA, triangleB);
acDist = Vector2.Distance(triangleA, triangleC);
foreach (Collider2D col in colls)
{
if (!col.gameObject == gameObject && col.GetComponent <CircleCollider2D>())
{
//check if collider is in View-Frustum, move through collider if yes
float xValue = orthogonale.normalized.x;
float yValue = orthogonale.normalized.y;
if (col.transform.position.x < orthogonale.x)
{
xValue = -xValue;
}
if (col.transform.position.y < orthogonale.y)
{
yValue = -yValue;
}
Vector2 movePointToMovement = new Vector2(xValue, yValue);
Vector2 potentialPoint = new Vector2(col.transform.position.x, col.transform.position.y) + movePointToMovement; //* (col.GetComponent<CircleCollider2D>().radius -5);
float distance = Vector2.Distance(potentialPoint, transform.position);
if (distance <= abDist && distance <= acDist && distance >= 0)
{
Vector2 dir = (potentialPoint - new Vector2(transform.position.x, transform.position.y)).normalized * (originalPosition - transform.position); //.normalized * helperDistance;
transform.position = Vector3.MoveTowards(transform.position, dir, helperSpeed * Time.deltaTime);
return;
}
}
}
}
//No collider? move normally like motion tells
transform.position = originalPosition;
}
else
{
transform.position = originalPosition;
}
}
public void MoveToTarget(CapturePosition target, MoveModeAMotor mode, ActionType type)
{
var tar = Helper.ConvertToPose(target);
MoveToTarget(tar, mode, type);
}
