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;
}
/// <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 + ".");
}
private void MotorYOutThenMotorZDown(TargetPosition target)
{
_motion.ToPointWaitTillEnd(_motion.MotorY, target.YPos);
_gripper.CheckEnabled();
_motion.ToPointWaitTillEnd(_motion.MotorZ, target.ZPos);
}
private void MoveToTargetPosition(Gripper gripper, TargetPosition target)
{
target = AddOffset(gripper, target);
TargetPosition currentPosition;
currentPosition = GetRobotPose();
//if (RobotHomeComplete==false)
//{
// throw new Exception("Robot is not home complete");
//}
//Todo
//if (target.Id==Location.Bin && door is opened, then that's not cool )
{
}
if (SetupComplete == false)
{
throw new Exception("Robot is not SetupComplete");
}
if (currentPosition.YPos > YIsNearHome)
{
throw new Exception("Y is out after previous movement");
} //Dangerous, may have to
if (currentPosition.XPos > _motion.ConveyorRightPosition.XPos) //Now on right side.
{
#region Move from right to ...
if (target.XPos > _motion.ConveyorRightPosition.XPos) //To right.
{
MoveFromRightToRight(gripper, target);
}
else
{
if (target.XPos < _motion.ConveyorLeftPosition.XPos) //To left.
{
if (target.ZPos > _motion.PickPosition.ApproachHeight) //To left top.
{
MoveFromRightToLeftTop(gripper, target, currentPosition);
}
else //To left bottom.
{
MoveFromRightToLeftBottom(gripper, target);
}
}
else //To Conveyor.
{
MoveFromRightToConveyor(gripper, target, currentPosition);
}
}
#endregion
}
else
{
if (currentPosition.XPos < _motion.ConveyorLeftPosition.XPos) //Now on left side.
{
#region Move from left to ...
if (target.XPos > _motion.ConveyorRightPosition.XPos) //To right
{
if (target.ZPos < _motion.PickPosition.ApproachHeight) // To right bottom.
{
MoveFromLeftToRightBottom(gripper, target);
}
else //To right top.
{
MoveFromLeftToRightTop(gripper, target, currentPosition);
}
}
else
{
if (target.XPos < _motion.ConveyorLeftPosition.XPos)//To left.
{
MoveFromLeftToLeft(gripper, target);
}
else//To conveyor.
{
MoveFromLeftToConveyor(gripper, target, currentPosition);
}
}
#endregion
}
else //Now on conveyor side.
{
#region Move from conveyor to ...
if (target.XPos > _motion.ConveyorRightPosition.XPos) //To right.
{
if (target.ZPos < _motion.PickPosition.ApproachHeight) // To right bottom.
{
MoveFromConveyorToRightBottom(gripper, target);
}
else //To right top.
{
MoveFromConveyorToRightTop(gripper, target);
}
}
else
{
if (target.XPos < _motion.ConveyorLeftPosition.XPos) //To left.
{
if (target.ZPos < _motion.PickPosition.ApproachHeight) // To left bottom.
{
MoveFromConveyorToLeftBottom(gripper, target);
}
else //To left top.
{
MoveFromConveyorToLeftTop(gripper, target);
}
}
else //To conveyor.
{
MoveFromConveyorToConveyor(gripper, target);
}
}
#endregion
}
}
}
public void HomeRobot(double homeSpeed = 5)
{
if (SetupComplete == false)
{
throw new Exception("Setup not Complete.");
}
_motion.SetSpeed(homeSpeed);
//Careful is robot is holding a phone.
//Box state should either be open or close.
TargetPosition currentPosition;
currentPosition = GetRobotPose();
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()
{
Id = Location.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;
}
}
if (currentHolder.Id != Location.Home)
{
_motion.ToPointWaitTillEnd(_motion.MotorZ, currentHolder.ApproachHeight);
_motion.ToPointWaitTillEnd(_motion.MotorR, currentHolder.RPos);
_motion.ToPointWaitTillEnd(_motion.MotorY, _motion.HomePosition.YPos);
_motion.ToPointWaitTillEnd(_motion.MotorZ, _motion.HomePosition.ZPos);
_motion.ToPointXWaitTillEnd(_motion.HomePosition.XPos);
_motion.ToPointWaitTillEnd(_motion.MotorR, _motion.HomePosition.RPos);
HomeGrippers();
}
else
{
throw new Exception("Gripper is in unknown conveyor area, please home Y and manually then retry.");
}
}
else
{
if (currentPosition.YPos < YIsNearHome)
{
_motion.ToPointWaitTillEnd(_motion.MotorY, _motion.HomePosition.YPos);
_motion.ToPointWaitTillEnd(_motion.MotorZ, _motion.HomePosition.ZPos);
_motion.ToPointXWaitTillEnd(_motion.HomePosition.XPos);
_motion.ToPointWaitTillEnd(_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.");
}
}
}
else //Robot in box zone.
{
if (currentPosition.YPos > YIsInBox) //Y is dangerous
{
//Todo, need to check X?
//X Y Z tolerance 50mm. then is inside box
TargetPosition currentHolder = new TargetPosition()
{
Id = Location.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.Id != Location.Home)
{
_motion.ToPointWaitTillEnd(_motion.MotorZ, currentHolder.ApproachHeight);
_motion.ToPointWaitTillEnd(_motion.MotorR, currentHolder.RPos);
_motion.ToPointWaitTillEnd(_motion.MotorY, _motion.HomePosition.YPos);
_motion.ToPointWaitTillEnd(_motion.MotorZ, _motion.HomePosition.ZPos);
_motion.ToPointXWaitTillEnd(_motion.HomePosition.XPos);
_motion.ToPointWaitTillEnd(_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)
{
_motion.ToPointWaitTillEnd(_motion.MotorY, _motion.HomePosition.YPos);
_motion.ToPointWaitTillEnd(_motion.MotorZ, _motion.HomePosition.ZPos);
_motion.ToPointXWaitTillEnd(_motion.HomePosition.XPos);
_motion.ToPointWaitTillEnd(_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.");
}
}
}
RobotHomeComplete = true;
}
private void MoveToTargetPosition(RackGripper gripper, TargetPosition target,
bool phoneSlipIn, bool addOffset = true)
{
if (RobotInSimulateMode)
{
return;
}
CheckSafety();
if (addOffset && gripper == RackGripper.Two)
{
target = AddOffset(gripper, target);
}
var currentPosition = GetRobotCurrentPose();
//if (RobotHomeComplete==false)
//{
// throw new Exception("Robot is not home complete");
//}
//Todo
//if (target.Id==Location.Bin && door is opened, then that's not cool )
{
}
if (SetupComplete == false)
{
throw new Exception("Robot is not SetupComplete");
}
if (currentPosition.YPos > YIsNearHome)
{
throw new Exception("Y is out after previous movement");
} //Dangerous, may have to
if (currentPosition.XPos > Motion.ConveyorRightPosition.XPos) //Now on right side.
{
#region Move from right to ...
if (target.XPos > Motion.ConveyorRightPosition.XPos) //To right.
{
MoveFromRightToRight(gripper, target, phoneSlipIn);
}
else
{
if (target.XPos < Motion.ConveyorLeftPosition.XPos) //To left.
{
if (target.ZPos > Motion.PickPosition.ApproachHeight) //To left top.
{
MoveFromRightToLeftTop(gripper, target, currentPosition, phoneSlipIn);
}
else //To left bottom.
{
MoveFromRightToLeftBottom(gripper, target, phoneSlipIn);
}
}
else //To Conveyor.
{
MoveFromRightToConveyor(gripper, target, currentPosition);
}
}
#endregion
}
else
{
if (currentPosition.XPos < Motion.ConveyorLeftPosition.XPos) //Now on left side.
{
#region Move from left to ...
if (target.XPos > Motion.ConveyorRightPosition.XPos) //To right
{
if (target.ZPos < Motion.PickPosition.ApproachHeight) // To right bottom.
{
MoveFromLeftToRightBottom(gripper, target, phoneSlipIn);
}
else //To right top.
{
MoveFromLeftToRightTop(gripper, target, currentPosition, phoneSlipIn);
}
}
else
{
if (target.XPos < Motion.ConveyorLeftPosition.XPos)//To left.
{
MoveFromLeftToLeft(gripper, target, phoneSlipIn);
}
else//To conveyor.
{
MoveFromLeftToConveyor(gripper, target, currentPosition);
}
}
#endregion
}
else //Now on conveyor side.
{
#region Move from conveyor to ...
if (target.XPos > Motion.ConveyorRightPosition.XPos) //To right.
{
if (target.ZPos < Motion.PickPosition.ApproachHeight) // To right bottom.
{
MoveFromConveyorToRightBottom(gripper, target, phoneSlipIn);
}
else //To right top.
{
MoveFromConveyorToRightTop(gripper, target, phoneSlipIn);
}
}
else
{
if (target.XPos < Motion.ConveyorLeftPosition.XPos) //To left.
{
if (target.ZPos < Motion.PickPosition.ApproachHeight) // To left bottom.
{
MoveFromConveyorToLeftBottom(gripper, target, phoneSlipIn);
}
else //To left top.
{
MoveFromConveyorToLeftTop(gripper, target, phoneSlipIn);
}
}
else //To conveyor.
{
MoveFromConveyorToConveyor(gripper, target);
}
}
#endregion
}
}
}
private void MoveFromLeftToConveyor(RackGripper gripper, TargetPosition target, TargetPosition currentPosition)
{
Motion.ToPoint(Motion.MotorZ, target.ApproachHeight);
ToPointR(target, gripper);
Motion.WaitTillZBiggerThan(Motion.PickPosition.ApproachHeight - 20);
Motion.ToPointX(target.XPos);
Motion.WaitTillEndX();
Motion.WaitTillEnd(Motion.MotorR);
MotorYOutThenBreakMotorZDown(target, gripper);
}
