/// <summary>
/// Move robot to target.
/// </summary>
/// <param name="target"></param>
/// <param name="mode"></param>
/// <param name="procedure"></param>
public void MoveTo(Pose target, MoveModeAMotor mode = MoveModeAMotor.None,
ActionType procedure = ActionType.Load)
{
MoveToSafeHeight();
lock (_motionLocker)
{
if (IsMoveToSameArea(target))
{
_mc.MoveToTarget(MotorY, target.Y);
_mc.MoveToTarget(MotorX, target.X);
MoveAngleMotor(target.A, mode, procedure);
_mc.WaitTillEnd(MotorX);
_mc.WaitTillEnd(MotorY);
WaitTillEndAngleMotor();
CylinderHead(HeadCylinderState.Down, procedure);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
else
{
//Move from conveyor to table.
if (GetPosition(MotorY) > SafeYArea &&
target.Y < SafeYArea)
{
MoveAngleMotor(target.A, mode, procedure);
_mc.MoveToTargetTillEnd(MotorX, target.X);
_mc.MoveToTargetTillEnd(MotorY, target.Y);
WaitTillEndAngleMotor();
CylinderHead(HeadCylinderState.Down, procedure);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
else
{
//Move from table to conveyor.
if (GetPosition(MotorX) < SafeXArea &&
target.X > SafeXArea)
{
MoveAngleMotor(target.A, mode, procedure);
_mc.MoveToTargetTillEnd(MotorY, target.Y);
_mc.MoveToTargetTillEnd(MotorX, target.X);
WaitTillEndAngleMotor();
CylinderHead(HeadCylinderState.Down, procedure);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
else
{
throw new Exception("V robot move to routine goes into bug6516516498513.");
}
}
}
}
}
public void MoveTo(Pose target, MoveModeAMotor mode, ActionType type = ActionType.Load)
{
MoveToSafeHeight();
lock (_motionLocker)
{
if (IsMoveToSameArea(target))
{
_mc.MoveToTarget(MotorY, target.Y);
_mc.MoveToTarget(MotorX, target.X);
MoveAngleMotor(target.A, mode, ActionType.Load);
_mc.WaitTillEnd(MotorA);
_mc.WaitTillEnd(MotorX);
_mc.WaitTillEnd(MotorY);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
else
{
//Move from conveyor to table.
if (GetPosition(MotorY) < SafeYArea &&
target.Y > SafeYArea)
{
MoveAngleMotor(target.A, mode, ActionType.Load);
_mc.MoveToTargetTillEnd(MotorX, target.X);
_mc.MoveToTargetTillEnd(MotorY, target.Y);
_mc.WaitTillEnd(MotorA);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
else
{
//Move from table to conveyor.
if (GetPosition(MotorX) < SafeXArea &&
target.X > SafeXArea)
{
MoveAngleMotor(target.A, mode, ActionType.Load);
_mc.MoveToTargetTillEnd(MotorY, target.Y);
_mc.MoveToTargetTillEnd(MotorX, target.X);
_mc.WaitTillEnd(MotorA);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
else
{
throw new Exception("L robot move to target fail, unknow move stratege.");
}
}
}
}
}
private void MoveTo(Pose target)
{
MoveToSafeHeight();
lock (_motionLocker)
{
_mc.MoveToTarget(MotorY, target.Y);
_mc.MoveToTarget(MotorX, target.X);
_mc.WaitTillEnd(MotorX);
_mc.WaitTillEnd(MotorY);
_mc.MoveToTargetTillEnd(MotorZ, target.Z);
}
}
private double TouchAndFindPressureSensor(double touchPointHeight, int timeoutSec = 60)
{
double needleHeight = 0;
//Max down 0.5mm
touchPointHeight -= 0.5;
_mc.MoveToTarget(MotorZ, touchPointHeight);
var stopwatch = new Stopwatch();
stopwatch.Start();
do
{
//if (_mc.IsMoving(MotorZ) == false)
//{
// _mc.MoveToTargetRelative(MotorZ, -0.1);
// if (GetPosition(MotorZ)<=touchPointHeight)
// {
// throw new Exception("Find needle height timeout, maybe need to lower sensor: " + _coordinateId);
// }
//}
if (GetPressureValue() > NeedleTouchPressure)
{
_mc.Stop(MotorZ);
Delay(800);
needleHeight = _mc.GetPosition(MotorZ) + NeedleOnZHeightCompensation;
SetSpeed(1);
_mc.MoveToTargetRelativeTillEnd(MotorZ, 2);
return(needleHeight);
}
if (stopwatch.ElapsedMilliseconds > timeoutSec * 1000)
{
throw new Exception("Find needle height timeout, maybe need to lower sensor: " + _coordinateId);
}
} while (true);
}
public void Ready()
{
_mc.MoveToTarget(MotorTray, BottomFirstLayerHeight);
}
