/// <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.");
}
}
}
}
}
/// <summary>
/// Move stepper rotation motor.
/// </summary>
/// <param name="angle"></param>
/// <param name="mode"></param>
/// <param name="procedure"></param>
public void MoveAngleMotor(double angle, MoveModeAMotor mode, ActionType procedure)
{
var stepperMotor = MotorA;
switch (procedure)
{
case ActionType.None:
break;
case ActionType.Load:
stepperMotor = MotorA;
break;
case ActionType.Unload:
stepperMotor = MotorAUnload;
break;
default:
break;
}
switch (mode)
{
case MoveModeAMotor.None:
//Motor goes to its current position, meet the wait till end condition.
_mc.MoveToTarget(MotorA, MotorA.TargetPosition);
_mc.MoveToTarget(MotorAUnload, MotorA.TargetPosition);
break;
case MoveModeAMotor.Abs:
_mc.MoveToTarget(stepperMotor, angle);
break;
case MoveModeAMotor.Relative:
_mc.MoveToTargetRelative(stepperMotor, angle);
break;
default:
break;
}
}
public void MoveAngleMotor(double angle, MoveModeAMotor mode,
ActionType type = ActionType.Load)
{
switch (mode)
{
case MoveModeAMotor.None:
//Motor goes to its current position, meet the wait till end condition.
_mc.MoveToTarget(MotorA, MotorA.TargetPosition);
break;
case MoveModeAMotor.Abs:
_mc.MoveToTarget(MotorA, angle);
break;
case MoveModeAMotor.Relative:
_mc.MoveToTargetRelative(MotorA, angle);
break;
default:
break;
}
}
/// <summary>
/// Correct direction counter clockwise
/// </summary>
/// <param name="relativeAngle"></param>
/// <param name="mode"></param>
/// <param name="action"></param>
public void CorrectAngle(double relativeAngle, MoveModeAMotor mode,
ActionType action = ActionType.Load)
{
_mc.MoveToTargetRelativeTillEnd(MotorA, relativeAngle);
}
public void MoveToTarget(Pose target, MoveModeAMotor mode = MoveModeAMotor.Abs,
ActionType type = ActionType.Load)
{
MoveTo(target, mode);
}
public void MoveToTarget(CapturePosition target, MoveModeAMotor mode, ActionType type = ActionType.Load)
{
var tar = Helper.ConvertToPose(target);
MoveTo(tar, MoveModeAMotor.Relative);
}
public void MoveToTarget(CapturePosition target, MoveModeAMotor mode, ActionType type)
{
throw new NotImplementedException();
}
public void MoveTo(Pose target, MoveModeAMotor mode, ActionType type)
{
throw new NotImplementedException();
}
public void MoveToTarget(CapturePosition target, MoveModeAMotor mode, ActionType type)
{
var tar = Helper.ConvertToPose(target);
MoveToTarget(tar, mode, type);
}
public void MoveToTarget(Pose target, MoveModeAMotor mode, ActionType procedure)
{
MoveTo(target, mode, procedure);
}
