public RackGripper GetAvailableGripper()
{
if (Motion.GetPosition(Motion.MotorR) > 0) //At gripper one.
{
if (EcatIo.GetInput(Input.Gripper01Loose) && !EcatIo.GetInput(Input.Gripper01))
{
return(RackGripper.One);
}
if (EcatIo.GetInput(Input.Gripper02Loose) && !EcatIo.GetInput(Input.Gripper02))
{
return(RackGripper.Two);
}
}
else
{
if (EcatIo.GetInput(Input.Gripper02Loose) && !EcatIo.GetInput(Input.Gripper02))
{
return(RackGripper.Two);
}
if (EcatIo.GetInput(Input.Gripper01Loose) && !EcatIo.GetInput(Input.Gripper01))
{
return(RackGripper.One);
}
}
throw new Exception("GetAvailableGripper failed.");
}
private void CheckGripperAvailable(RackGripper gripper)
{
if (gripper == RackGripper.One)
{
if (!EcatIo.GetInput(Input.Gripper01Loose))
{
throw new Exception("Gripper one is not opened.");
}
}
else
{
if (!EcatIo.GetInput(Input.Gripper02Loose))
{
throw new Exception("Gripper two is not opened.");
}
}
if (gripper == RackGripper.One)
{
if (EcatIo.GetInput(Input.Gripper01))
{
throw new Exception("Gripper one is not empty.");
}
}
else
{
if (EcatIo.GetInput(Input.Gripper02))
{
throw new Exception("Gripper two is not empty.");
}
}
}
public void Pick(RackGripper gripper = RackGripper.None, bool okToReloadConveyor = true)
{
OnInfoOccured(20016, "Find gripper for pick.");
if (gripper == RackGripper.None)
{
gripper = GetAvailableGripper();
}
OnInfoOccured(20016, "About to pick phone on conveyor with gripper" + gripper + ".");
if (LatestPhone != null)
{
if (LatestPhone.OnGripper != RackGripper.None)
{
OnInfoOccured(20016, "Already got a phone in " + gripper + ".");
//Robot not release control on conveyor in this case.
OkToReloadOnConveyor();
return;
}
}
else
{
throw new Exception("Phone is not ready.");
}
if (EcatIo.GetInput(Input.PickHasPhone) == false)
{
throw new Exception("No phone in pick position. Quit picking.");
}
RobotTakeControlOnConveyor();
CheckGripperAvailable(gripper);
Conveyor.ReadyForPicking();
TargetPosition target = Motion.PickPosition;
if (gripper == RackGripper.Two)
{
target.XPos = target.XPos + Motion.PickOffset.XPos;
}
MoveToTargetPosition(gripper, target, false, false);
CloseGripper(gripper);
MoveToPointTillEnd(Motion.MotorZ, Motion.PickPosition.ApproachHeight);
CheckPhoneLost(gripper);
MoveToPointTillEnd(Motion.MotorY, Motion.HomePosition.YPos);
LatestPhone.OnGripper = gripper;
if (okToReloadConveyor)
{
OkToReloadOnConveyor();
RobotReleaseControlOnConveyor();
}
OnInfoOccured(20017, "Pick phone succeed.");
}
private bool GripperIsAvailable(RackGripper gripper)
{
if (gripper == RackGripper.One)
{
return(EcatIo.GetInput(Input.Gripper01Loose) && !EcatIo.GetInput(Input.Gripper01));
}
else
{
return(EcatIo.GetInput(Input.Gripper02Loose) && !EcatIo.GetInput(Input.Gripper02));
}
}
/// <summary>
/// Phone is not on gripper after deliver.
/// </summary>
/// <param name="gripper"></param>
private void CheckPhoneLost(RackGripper gripper)
{
if (gripper == RackGripper.One)
{
if (!EcatIo.GetInput(Input.Gripper01))
{
throw new Exception("Gripper one is empty, phone is lost.");
}
}
else
{
if (!EcatIo.GetInput(Input.Gripper02))
{
throw new Exception("Gripper two is empty, phone is lost.");
}
}
}
public void OpenGripper(RackGripper gripper, int timeout = 1000)
{
EcatIo.SetOutput(gripper == RackGripper.One ? Output.GripperOne : Output.GripperTwo, true);
Stopwatch sw = new Stopwatch();
sw.Start();
Input sensor = gripper == RackGripper.One ? Input.Gripper01Loose : Input.Gripper02Loose;
int failCount = 0;
while (!EcatIo.GetInput(sensor))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new Exception("Open gripper timeout");
}
Thread.Sleep(10);
if (failCount > 20)
{
EcatIo.SetOutput(gripper == RackGripper.One ? Output.GripperOne : Output.GripperTwo, true);
failCount = 0;
}
}
}
public void CloseGripper(RackGripper gripper, int timeout = 1000)
{
EcatIo.SetOutput(gripper == RackGripper.One ? Output.GripperOne : Output.GripperTwo, false);
Stopwatch sw = new Stopwatch();
sw.Start();
Input sensor = gripper == RackGripper.One ? Input.Gripper01Tight : Input.Gripper02Tight;
int failCount = 0;
while (!EcatIo.GetInput(sensor))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new Exception("Close gripper " + gripper + " timeout");
}
Thread.Sleep(10);
failCount++;
if (failCount > 20)
{
EcatIo.SetOutput(gripper == RackGripper.One ? Output.GripperOne : Output.GripperTwo, false);
failCount = 0;
}
}
}
public void ReadyThePhone(int timeout = 3000)
{
//RobotTakeControlOnConveyor();
Conveyor.Clamp(false);
Conveyor.UpBlockSeparate(false);
Conveyor.UpBlockPick(false);
Conveyor.SideBlockSeparate(false);
Conveyor.ConveyorMovingForward = !Conveyor.ConveyorMovingForward;
Conveyor.UpBlockSeparate(false);
Conveyor.UpBlockPick(false);
Conveyor.SideBlockSeparate(false);
Conveyor.ConveyorMovingForward = !Conveyor.ConveyorMovingForward;
EcatIo.SetOutput(Output.ClampPick, true);
Stopwatch sw = new Stopwatch();
sw.Start();
while (!EcatIo.GetInput(Input.ClampTightPick))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new Exception("ReadyThePhone timeout");
}
Thread.Sleep(10);
}
EcatIo.SetOutput(Output.SideBlockPick, true);
sw.Restart();
while (EcatIo.GetInput(Input.SideBlockPick))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new Exception("ReadyThePhone timeout");
}
Thread.Sleep(10);
}
Thread.Sleep(500);
EcatIo.SetOutput(Output.SideBlockPick, false);
sw.Restart();
while (!EcatIo.GetInput(Input.SideBlockPick))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new Exception("ReadyThePhone timeout");
}
Thread.Sleep(10);
}
EcatIo.SetOutput(Output.ClampPick, false);
sw = new Stopwatch();
sw.Restart();
while (!EcatIo.GetInput(Input.ClampLoosePick))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new Exception("ReadyThePhone timeout");
}
Thread.Sleep(10);
}
}
public void Bin(RackGripper gripper, Phone phone = null)
{
OnInfoOccured(20019, "Try binning phone with " + gripper + ".");
if (phone != null)
{
string footprint = string.Empty;
foreach (var foot in phone.TargetPositionFootprint)
{
footprint += (int)foot.TeachPos + ",";
}
OnProductionComplete(false, phone.SerialNumber, footprint, phone.FailDetail);
}
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
while (Conveyor.NgFullWarning)
{
if (stopwatch.ElapsedMilliseconds > 2 * 60 * 60 * 1000) // 2 hour waiting.
{
throw new Exception("Can't bin because ng conveyor is full.");
}
Delay(1000);
}
if (Conveyor.HasBinAPhone == true || EcatIo.GetInput(Input.ConveyorBinIn) == true ||
EcatIo.GetInput(Input.ConveyorBinInTwo) == true)
{
throw new Exception("Last bin movement has't finished.");
}
Conveyor.RobotBinning = true;
ShieldBox3.RobotBining = true;
bool needReopen = false;
if (gripper == RackGripper.One)
{
if (ShieldBox3.WasEnabled)
{
if (ShieldBox3.IsClosed() == false)
{
ShieldBox3.CloseBox(5000, false);
needReopen = true;
}
}
}
Conveyor.StopBeltBin();
MoveToTargetPosition(gripper, Motion.BinPosition, true);
OpenGripper(gripper);
Conveyor.HasBinAPhone = true;
MoveToPointTillEnd(Motion.MotorZ, Motion.BinPosition.ApproachHeight);
YRetractFromBox();
Conveyor.RunBeltBin();
if (needReopen && ShieldBox3.Enabled)
{
ShieldBox3.OpenBox(5000, true, true);
}
ShieldBox3.RobotBining = false;
Conveyor.RobotBinning = false;
OnInfoOccured(20019, "Finish binning phone with " + gripper + ".");
}
/// <summary>
///
/// </summary>
/// If a new phone went into pick position, it has to be picked before
/// place a pass phone, in <see cref="ArrangePhones"/> , it serve new
/// phone before wifi phone(second priority), and new phone will only come in
/// when there is empty enough Rf box.
/// <param name="gripper"></param>
public void Place(RackGripper gripper, Phone phone = null)
{
OnInfoOccured(20026, "Try placing with " + gripper + ".");
if (Conveyor.HasPlaceAPhone == true)
{
throw new Exception("Last place movement has't finished.");
}
if (phone != null)
{
string footprint = string.Empty;
foreach (var foot in phone.TargetPositionFootprint)
{
footprint += (int)foot.TeachPos + ",";
}
OnProductionComplete(true, phone.SerialNumber, footprint, "");
}
RobotTakeControlOnConveyor();
Conveyor.StopBeltPick();
RackGripper theOtherGripper = RackGripper.None;
if (Conveyor.PickPhoneSensor())
{
if (LatestPhone != null)
{
theOtherGripper = gripper == RackGripper.One ? RackGripper.Two : RackGripper.One;
Pick(theOtherGripper, false);
}
else
{
throw new Exception("When place, has an unknown phone.");
}
}
if (Conveyor.PickPhoneSensor())
{
throw new Exception("When place, has an unknown phone in pick position.");
}
if (EcatIo.GetInput(Input.UpBlockPickForward) ||
EcatIo.GetInput(Input.UpBlockPickBackward) || EcatIo.GetInput(Input.ClampTightPick))
{
throw new Exception("When place, cylinder on conveyor is up.");
}
CheckPhoneLost(gripper);
TargetPosition placePosition = Motion.PickPosition;
if (gripper == RackGripper.Two)
{
placePosition.XPos = placePosition.XPos + 1 + Motion.PickOffset.XPos;
}
else
{
placePosition.XPos = placePosition.XPos + 1;
}
placePosition.ZPos = placePosition.ZPos + 4;
if (theOtherGripper != RackGripper.None)
{
Motion.ToPointX(placePosition.XPos);
ToPointR(placePosition, gripper);
//ToPointGripper(placePosition, gripper);
ToPointGripperOnConveyorTillEnd(placePosition, gripper);
//WaitTillEndGripper(placePosition, gripper);
Motion.WaitTillEndX();
Motion.WaitTillEnd(Motion.MotorR);
MotorYOutThenBreakMotorZDown(placePosition, gripper, false);
}
else
{
MoveToTargetPosition(gripper, placePosition, true, false);
}
OpenGripper(gripper);
if (theOtherGripper != RackGripper.None)
{
Steppers.ToPoint(theOtherGripper, Motion.PickPosition.APos);
}
MoveToPointTillEnd(Motion.MotorZ, Motion.PickPosition.ApproachHeight);
MoveToPointTillEnd(Motion.MotorY, Motion.HomePosition.YPos);
Conveyor.HasPlaceAPhone = true;
if (theOtherGripper != RackGripper.None)
{
//Steppers.ToPoint(theOtherGripper, Motion.PickPosition.APos);
Steppers.WaitTillEnd(theOtherGripper, Motion.PickPosition.APos);
}
RobotReleaseControlOnConveyor();
OnInfoOccured(20026, "Finish placing with " + gripper + ".");
}
public void HomeRobot(double homeSpeed = 30.0, bool checkGripperEmpty = true)
{
if (SetupComplete == false)
{
throw new Exception("Setup not Complete.");
}
RobotHomeComplete = false;
Motion.SetSpeed(homeSpeed);
Motion.EnableAll();
Steppers.Enable(RackGripper.One);
Steppers.Enable(RackGripper.Two);
var currentPosition = GetRobotCurrentPose();
if (currentPosition.XPos <Motion.ConveyorRightPosition.XPos&
currentPosition.XPos> Motion.ConveyorLeftPosition.XPos) //Robot is in conveyor zone.
{
if (currentPosition.YPos > YIsInBox) //Y is dangerous
{
TargetPosition currentHolder = new TargetPosition()
{
TeachPos = TeachPos.Home
};
double tolerance = 50.0;
foreach (var pos in Motion.Locations)
{
if (Math.Abs(currentPosition.XPos - pos.XPos) < tolerance &
Math.Abs(currentPosition.YPos - pos.YPos) < tolerance &
(currentPosition.ZPos > pos.ZPos - tolerance && currentPosition.ZPos < pos.ApproachHeight + tolerance))
{
currentHolder = pos;
}
}
if (currentHolder.TeachPos != TeachPos.Home)
{
HomeGrippers();
MoveToPointTillEnd(Motion.MotorZ, currentHolder.ApproachHeight);
MoveToPointTillEnd(Motion.MotorR, currentHolder.RPos);
MoveToPointTillEnd(Motion.MotorY, Motion.HomePosition.YPos);
MoveToPointTillEnd(Motion.MotorZ, Motion.HomePosition.ZPos);
MoveX1X2ToPointTillEnd(Motion.HomePosition.XPos);
MoveToPointTillEnd(Motion.MotorR, Motion.HomePosition.RPos);
HomeGrippers();
}
else
{
throw new Exception("Gripper is in unknown conveyor area, please move Y and manually then retry.");
}
}
else
{
if (currentPosition.YPos < YIsNearHome)
{
HomeGrippers();
MoveToPointTillEnd(Motion.MotorZ, Motion.HomePosition.ZPos);
MoveToPointTillEnd(Motion.MotorY, Motion.HomePosition.YPos);
MoveX1X2ToPointTillEnd(Motion.HomePosition.XPos);
MoveToPointTillEnd(Motion.MotorR, Motion.HomePosition.RPos);
HomeGrippers();
}
else
{
throw new Exception("Gripper is in unknown position, please home Y and manually then retry.");
}
}
}
else //Robot in box zone.
{
if (currentPosition.YPos > YIsInBox) //Y is dangerous
{
//X Y Z tolerance 50mm. then is inside box
TargetPosition currentHolder = new TargetPosition()
{
TeachPos = TeachPos.Home
};
double tolerance = 50;
foreach (var pos in Motion.Locations)
{
if (Math.Abs(currentPosition.XPos - pos.XPos) < tolerance &
Math.Abs(currentPosition.YPos - pos.YPos) < tolerance &
(currentPosition.ZPos > pos.ZPos - tolerance && currentPosition.ZPos < pos.ApproachHeight + tolerance))
{
currentHolder = pos;
break;
}
}
if (currentHolder.TeachPos != TeachPos.Home)
{
MoveToPointTillEnd(Motion.MotorZ, currentHolder.ApproachHeight);
HomeGrippers();
MoveToPointTillEnd(Motion.MotorR, currentHolder.RPos);
MoveToPointTillEnd(Motion.MotorY, Motion.HomePosition.YPos);
MoveToPointTillEnd(Motion.MotorZ, Motion.HomePosition.ZPos);
MoveX1X2ToPointTillEnd(Motion.HomePosition.XPos);
MoveToPointTillEnd(Motion.MotorR, Motion.HomePosition.RPos);
HomeGrippers();
}
else
{
throw new Exception("Gripper is in unknown box, please home Y and manually then retry.");
}
}
else
{
if (currentPosition.YPos < YIsNearHome)
{
MoveToPointTillEnd(Motion.MotorY, Motion.HomePosition.YPos);
HomeGrippers();
MoveToPointTillEnd(Motion.MotorZ, Motion.HomePosition.ZPos);
MoveX1X2ToPointTillEnd(Motion.HomePosition.XPos);
MoveToPointTillEnd(Motion.MotorR, Motion.HomePosition.RPos);
//Disable one of the motor.
HomeGrippers();
}
else
{
throw new Exception("Gripper is in unknown position, please home Y and manually then retry.");
}
}
}
if (checkGripperEmpty)
{
if (EcatIo.GetInput(Input.Gripper01) | EcatIo.GetInput(Input.Gripper02))
{
throw new Exception("Has phone on gripper, take it down first.");
}
OpenGripper(RackGripper.One);
OpenGripper(RackGripper.Two);
}
RobotHomeComplete = true;
}