public void Load(RackGripper gripper, TargetPosition position)
{
MoveToTargetPosition(gripper, position, true);
OpenGripper(gripper);
MoveToPointTillEnd(Motion.MotorZ, position.ApproachHeight);
YRetractFromBox();
}
/// <summary>
/// Wait motor get in position.
/// </summary>
/// <param name="motor"></param>
/// <param name="targetAngle"></param>
/// <param name="timeout"></param>
public void WaitTillEnd(RackGripper motor, double targetAngle, int timeout = 10)
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
bool inPos = false;
while (inPos == false)
{
if (stopwatch.ElapsedMilliseconds > timeout * 1000) //Should response within 2 second.
{
throw new Exception("Motor in position timeout.");
}
inPos = GetStatus(motor, StatusCode.Inpos);
Thread.Sleep(20);
}
if (GetStatus(motor, StatusCode.Enabled) == false)
{
throw new Exception("Stepper motor " + motor + " is disabled.");
}
double currentPos = GetPosition(motor);
if (Math.Abs(currentPos - targetAngle) > 0.5)
{
throw new Exception("Position error is bigger than 0.5 degree.");
}
}
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.");
}
}
}
/// <summary>
/// Find reference point of motor, AKA home position.
/// Must has the right sensor for homing.
/// </summary>
/// <param name="motor"></param>
public void HomeMotor(RackGripper motor, double homeOffset, int defaultWorkingSpeed = 10)
{
SetSpeed(motor, 1); //Default homing speed.
if (GetStatus(motor, StatusCode.Enabled) == false)
{
Enable(motor);
}
if (GetInput(motor, InputStepper.X1) == false)
{
//If home sensor has been triggered before homing.
// move out first.
ToPointRelative(motor, -60);
}
SendCommand(motor, "SH1H");
Thread.Sleep(50);
WaitCondition(motor, StatusCode.Homing, false);
WaitCondition(motor, StatusCode.Inpos, true);
ToPointRelative(motor, homeOffset);
SendCommand(motor, "EP0");
SendCommand(motor, "SP0");
SetSpeed(motor, defaultWorkingSpeed);
}
/// <summary>
/// Get current position of motor.
/// </summary>
/// <param name="motor"></param>
/// <returns></returns>
public double GetPosition(RackGripper motor)
{
// Response of "IP" is always hexadecimal
string res;
string posString;
int pos = 0;
int failCount = 0;
while (true)
{
try
{
res = SendCmd(motor, "IP");
posString = res.Substring(4, res.Length - 4);
pos = Convert.ToInt32(posString);
break;
}
catch (Exception e)
{
failCount++;
if (failCount > 3)
{
throw new Exception("GetPosition of " + motor + " failed." + e.Message);
}
}
}
return(pos / _countPerDegree);
}
private void SendCommand(RackGripper motor, string cmd)
{
string response;
bool idMatch, endMatch, result;
try
{
response = SendCmd(motor, cmd);
idMatch = response.Substring(0, 1) == GetMotorId(motor);
endMatch = response.Substring(1, 1) == "%" || response.Substring(1, 1) == "*";
result = idMatch && endMatch;
if (result == false)
{
throw new Exception();
}
}
catch (Exception)
{
response = SendCmd(motor, cmd);
idMatch = response.Substring(0, 1) == GetMotorId(motor);
endMatch = response.Substring(1, 1) == "%" || response.Substring(1, 1) == "*";
result = idMatch && endMatch;
if (result == false)
{
throw new Exception("Send command:" + cmd + " to motor " + motor + " failed.");
}
}
}
/// <summary>
/// Send command to motor through serial port, and wait for response.
/// </summary>
/// <param name="cmd"></param>
public string SendCmd(RackGripper motor, string cmd, int retryTimes = 5)
{
lock (PortWriteLocker)
{
int failCount = 0;
string command = GetMotorId(motor) + cmd + "\r"; //Add a carriage return.
byte[] buffer = Encoding.ASCII.GetBytes(command);
while (true)
{
try
{
DriverResponsed = false;
Response = string.Empty;
SerialPort.Write(buffer, 0, buffer.Length);
return(GetResponse());
}
catch (Exception)
{
failCount++;
Thread.Sleep(100);
if (failCount > retryTimes)
{
throw new Exception("Stepper command:" + command + " get no response.");
}
}
}
}
}
/// <summary>
/// Enalbe motor.
/// </summary>
/// <param name="motor"></param>
public void Enable(RackGripper motor)
{
try
{
if (GetStatus(motor, StatusCode.Enabled) == true)
{
return;
}
if (GetStatus(motor, StatusCode.Alarm) == true)
{
ResetAlarm(motor);
}
SendCommand(motor, "ME");
if (GetStatus(motor, StatusCode.Enabled) == false)
{
throw new Exception(motor + " can not be enabled");
}
}
catch (Exception ex)
{
throw new Exception("Enable motor fail due to:" + ex.Message);
}
}
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 void ToPointWaitTillEndGripper(TargetPosition target, RackGripper gripper)
{
if (gripper == RackGripper.One)
{
Motion.ToPoint(Motion.MotorR, target.RPos);
Steppers.ToPoint(RackGripper.One, target.APos);
Steppers.WaitTillEnd(RackGripper.One, target.APos);
//Steppers.ToPoint(RackGripper.Two, 0);
//Steppers.WaitTillEnd(RackGripper.Two, 0);
Motion.WaitTillEnd(Motion.MotorR);
}
else
{
Motion.ToPoint(Motion.MotorR, target.RPos - 60);
Steppers.ToPoint(RackGripper.Two, target.APos);
Steppers.WaitTillEnd(RackGripper.Two, target.APos);
//Steppers.ToPoint(RackGripper.One, 0);
//Steppers.WaitTillEnd(RackGripper.One, 0);
Motion.WaitTillEnd(Motion.MotorR);
}
}
public void ToPointWaitTillEnd(RackGripper motor, double angle)
{
int target = Convert.ToInt32(angle * _countPerDegree);
SendCommand(motor, "FP" + target);
Thread.Sleep(50);
WaitTillEnd(motor, angle);
}
/// <summary>
/// Disable motor.
/// </summary>
/// <param name="motor"></param>
public void Disable(RackGripper motor)
{
SendCommand(motor, "MD");
if (GetStatus(motor, StatusCode.Enabled) == true)
{
throw new Exception("Motor is not disabled");
}
}
public void LoadForTeaching(RackGripper gripper, TeachPos selectedTeachPos)
{
TargetPosition target = ConverterTeachPosToTargetPosition(selectedTeachPos);
target.ZPos = target.ZPos + 30;
MoveToTargetPosition(gripper, target, false);
DisableMotorsForTeaching();
Motion.SetSpeed(3);
}
private void ChangeGripper(TargetPosition target, ref RackGripper gripper)
{
gripper = gripper == RackGripper.One ? RackGripper.Two : RackGripper.One;
target = AddOffset(gripper, target);
Motion.ToPointX(target.XPos);
Motion.ToPoint(Motion.MotorY, target.YPos);
ToPointWaitTillEndGripper(target, gripper);
Motion.WaitTillEndX();
Motion.WaitTillEnd(Motion.MotorY);
}
private TargetPosition AddOffset(RackGripper gripper, TargetPosition target)
{
if (gripper == RackGripper.Two)
{
target.XPos = target.XPos + Motion.G1ToG2Offset.XPos;
target.YPos = target.YPos + Motion.G1ToG2Offset.YPos;
target.APos = target.APos + Motion.G1ToG2Offset.APos;
}
return(target);
}
private void MoveFromConveyorToLeftTop(RackGripper gripper, TargetPosition target, bool phoneSlipIn)
{
Motion.ToPointX(target.XPos);
ToPointR(target, gripper);
Motion.ToPoint(Motion.MotorZ, target.ApproachHeight);
Motion.WaitTillEndX();
Motion.WaitTillEnd(Motion.MotorZ);
Motion.WaitTillEnd(Motion.MotorR);
MotorYOutThenMotorZDown(target, gripper, phoneSlipIn);
}
/// <summary>
/// Move motor to specific angle relative to current postion.
/// allow error: 0.5 degree.
/// </summary>
/// <param name="motor"></param>
/// <param name="angle"></param>
public void ToPointRelative(RackGripper motor, double angle, int timeout = 10)
{
double lastPos = GetPosition(motor);
int target = Convert.ToInt32(angle * _countPerDegree);
SendCommand(motor, "FL" + target);
Thread.Sleep(50);
WaitTillEnd(motor, lastPos + angle, timeout);
}
public void Stop(RackGripper motor)
{
try
{
SendCommand(motor, "STD");
}
catch (Exception)
{
throw new Exception("Drive is can NOT stopped.");
}
}
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));
}
}
private void ToPointR(TargetPosition target, RackGripper gripper)
{
if (gripper == RackGripper.One)
{
Motion.ToPoint(Motion.MotorR, target.RPos);
}
else
{
Motion.ToPoint(Motion.MotorR, target.RPos - 60);
}
}
private void MoveFromConveyorToConveyor(RackGripper gripper, TargetPosition target)
{
Motion.ToPointX(target.XPos);
Motion.ToPoint(Motion.MotorZ, target.ApproachHeight);
ToPointR(target, gripper);
Motion.WaitTillEndX();
Motion.WaitTillEnd(Motion.MotorR);
MotorYOutThenBreakMotorZDown(target, gripper);
}
private void MotorYOutThenBreakMotorZDown(TargetPosition target, RackGripper gripper, bool moveStepper = true)
{
Motion.ToPoint(Motion.MotorY, target.YPos);
if (moveStepper)
{
ToPointGripperOnConveyorTillEnd(target, gripper);
}
Motion.WaitTillEnd(Motion.MotorY);
Motion.BreakToPointWaitTillEnd(Motion.MotorZ, target.ZPos);
}
/// <summary>
/// Set speed of motor.
/// </summary>
/// <param name="motor"></param>
/// <param name="velocity">rps</param>
public void SetVelocity(RackGripper motor, int velocity = 30)
{
//double vel = GearRatio * velocity / 360.0;
// ST-Q/Si, ST-S , STM, STAC5: 0.0042 - 80.0000 (resolution is 0.0042)
//int times = Convert.ToInt16(vel / 0.0042);
//times++;
//vel = Convert.ToDouble(times) * 0.0042;
//string velStr = vel.ToString("0.0000");
SendCommand(motor, "VE" + velocity);
}
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);
}
//public void UnloadAndLoad(TargetPosition target, RackGripper gripper)
//{
// CheckGripperAvailable(gripper);
// MoveToTargetPosition(gripper, target);
// CloseGripper(gripper);
// MoveToPointTillEnd(Motion.MotorZ, target.ApproachHeight); //Up.
// ChangeGripper(target, ref gripper); //Switch gripper.
// MoveToPointTillEnd(Motion.MotorZ, target.ZPos); //Down.
// OpenGripper(gripper);
// MoveToPointTillEnd(Motion.MotorZ, target.ApproachHeight); //Up.
// YRetractFromBox();
//}
public void UnloadAndLoad(ShieldBox box, RackGripper gripper, bool closeBox)
{
OnInfoOccured(20025, "Try unload and load box: " + box.Id + " with " + gripper + ".");
CheckGripperAvailable(gripper);
if (box.IsClosed() == true)
{
throw new Exception("Box " + box.Id + " is not opened.");
}
MoveToTargetPosition(gripper, box.Position, false);
CloseGripper(gripper);
Motion.ToPoint(Motion.MotorZ, box.Position.ZPos + 50);
Motion.WaitTillZBiggerThan(box.Position.ZPos + 30);
ChangeGripper(box.Position, ref gripper); //Switch gripper.
Motion.WaitTillEnd(Motion.MotorZ);
CheckPhoneLost(gripper);
//Slip in phone.
Task.Run(() => {
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
while (stopwatch.ElapsedMilliseconds < 5000)
{
if (Motion.GetPosition(Motion.MotorZ) < box.Position.ZPos + SlipInHeight)
{
try
{
OpenGripper(gripper);
}
catch (Exception)
{
break;
}
}
Delay(5);
}
});
MoveToPointTillEnd(Motion.MotorZ, box.Position.ZPos); //Down.
OpenGripper(gripper);
MoveToPointTillEnd(Motion.MotorZ, box.Position.ApproachHeight); //Up.
if (closeBox)
{
CloseBoxAsync(box);
}
YRetractFromBox();
OnInfoOccured(20025, "Finish unload and load box: " + box.Id + " with " + gripper + ".");
}
/// <summary>
/// Check if motor receives command.
/// </summary>
/// <param name="motor"></param>
/// <param name="response"></param>
/// <returns></returns>
private bool MotorAcknowledged(RackGripper motor, string response)
{
try
{
bool idMatch = response.Substring(0, 1) == GetMotorId(motor);
bool endMatch = response.Substring(1, 1) == "%" || response.Substring(1, 1) == "*";
return(idMatch && endMatch);
}
catch (Exception)
{
return(false);
}
}
/// <summary>
/// Move motor to specific angle.
/// </summary>
/// <param name="motor"></param>
/// <param name="angle"></param>
public void ToPoint(RackGripper motor, double angle)
{
int target = Convert.ToInt32(angle * _countPerDegree);
SendCommand(motor, "FP" + target);
if (GetStatus(motor, StatusCode.Alarm))
{
throw new Exception("Drive alarm");
}
Thread.Sleep(50);
}
/// <summary>
/// Set acceleration and deceleration of motor.
/// </summary>
/// <param name="motor"></param>
/// <param name="acceleration"> degree / sec / sec </param>
public void SetAcceleration(RackGripper motor, int acceleration = 30)
{
// rev / sec / sec
//double acc = GearRatio * acceleration / 360.0;
//// AC - Acceleration Rate 0.167 to 5461.167 (resolution is 0.167 rps/s)
//int times = Convert.ToInt16(acc / 0.167);
//times++;
//acc = Convert.ToDouble(times) * 0.167;
//string accStr = acc.ToString("0.000");
SendCommand(motor, "AC" + acceleration);
SendCommand(motor, "DE" + acceleration);
}
private void MoveFromLeftToRightTop(RackGripper gripper, TargetPosition target,
TargetPosition currentPosition, bool phoneSlipIn)
{
Motion.ToPoint(Motion.MotorZ, target.ApproachHeight);
ToPointR(target, gripper);
Motion.WaitTillZBiggerThan(Motion.PickPosition.ApproachHeight - 20);
Motion.ToPointX(target.XPos);
Motion.WaitTillEndX();
Motion.WaitTillEnd(Motion.MotorZ);
Motion.WaitTillEnd(Motion.MotorR);
MotorYOutThenMotorZDown(target, gripper, phoneSlipIn);
}
/// <summary>
/// Asks the drive to respond with what it’s doing
/// </summary>
/// <param name="motor"></param>
public bool GetStatus(RackGripper motor, StatusCode status)
{
string info = string.Empty;
string code;
string binaryValue;
try
{
info = SendCmd(motor, "SC");
code = info.Substring(4, info.Length - 4);
binaryValue = Convert.ToString(Convert.ToInt32(code, 16), 2);
}
catch (Exception)
{
try
{
Thread.Sleep(100);
info = SendCmd(motor, "SC");
code = info.Substring(4, info.Length - 4);
binaryValue = Convert.ToString(Convert.ToInt32(code, 16), 2);
}
catch (Exception)
{
try
{
Thread.Sleep(100);
info = SendCmd(motor, "SC");
code = info.Substring(4, info.Length - 4);
binaryValue = Convert.ToString(Convert.ToInt32(code, 16), 2);
}
catch (Exception ex)
{
throw new Exception("GetStatus fail due to:" + ex.Message + " response is: " + info);
}
}
}
bool[] result = binaryValue.Select(c => c == '1').ToArray();
Array.Reverse(result);
try
{
return(result[(int)status]);
}
catch (Exception)
{
return(false);
}
}