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