public void Pick(Gripper gripper)
{
if (_conveyor.ReadyForPicking)
{
_conveyor.ReadyForPicking = false;
}
else
{
throw new Exception("Phone is not ready.");
}
if (gripper == Gripper.One)
{
if (!_io.GetInput(Input.Gripper01Loose))
{
throw new Exception("Gripper one is not opened.");
}
}
else
{
if (!_io.GetInput(Input.Gripper02Loose))
{
throw new Exception("Gripper two is not opened.");
}
}
//If system is OK, gripper is free and opened, conveyor is ready
//If the other gripper is holding a phone, then conveyor can not reload.
MoveToTargetPosition(gripper, _motion.PickPosition);
//Close cylinder.
CloseGripper(gripper);
_motion.ToPointWaitTillEnd(_motion.MotorZ, _motion.PickPosition.ApproachHeight);
//Check.
}
/// <summary>
/// Send command to motor through serial port, and wait for response.
/// </summary>
/// <param name="cmd"></param>
/// <seealso cref="SendCommand"/>
public string SendCommand(Gripper motor, string cmd)
{
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;
SerialPort.Write(buffer, 0, buffer.Length);
return(GetResponse());
}
catch (Exception)
{
failCount++;
Thread.Sleep(50);
PartialResponse = string.Empty;
if (failCount > 5)
{
throw new Exception(command + " get no response.");
}
}
}
}
}
/// <summary>
/// Method to close the gripper in order to grab the main payload
/// </summary>
public void CloseClaw()
{
//code for closing claw and retracting arm to UGV
int writeVal = (int)(Gripper.DXL_MAXIMUM_POSITION_VALUE * 0.8);
Gripper.Move_To(writeVal);
}
/// <summary>
/// Wait motor get in position.
/// </summary>
/// <param name="motor"></param>
/// <param name="targetAngle"></param>
/// <param name="timeout"></param>
public void WaitTillEnd(Gripper 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.");
}
}
/// <summary>
/// Check if motor receives command.
/// </summary>
/// <param name="motor"></param>
/// <param name="response"></param>
/// <returns></returns>
private bool MotorAcknowledged(Gripper motor, string response)
{
bool idMatch = response.Substring(0, 1) == GetMotorId(motor);
bool endMatch = response.Substring(1, 1) == "%" | response.Substring(1, 1) == "*";
return(idMatch & endMatch);
}
/// <summary>
/// Reset all alarm of motor.
/// </summary>
/// <param name="motor"></param>
public void ResetAlarm(Gripper motor)
{
string res = SendCommand(motor, "AR");
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
Thread.Sleep(20);
if (GetStatus(motor, StatusCode.Alarm) == true)
{
throw new Exception("Drive's alarm can not be reset");
}
res = SendCommand(motor, "ME");
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
if (GetStatus(motor, StatusCode.Enabled) == false)
{
throw new Exception("Drive is can not be enabled.");
}
}
public void Bin(Gripper gripper)
{
//TODO make sure pick position is empty, conveyor is not stucked and gripper is full.
MoveToTargetPosition(gripper, _motion.BinPosition);
//Open cylinder.
_motion.ToPointWaitTillEnd(_motion.MotorZ, _motion.BinPosition.ApproachHeight);
//Check phone is on conveyor.
}
public void Stop(Gripper motor)
{
string res = SendCommand(motor, "STD");
if (MotorAcknowledged(motor, res) != true)
{
throw new Exception("Drive is NOT enabled");
}
}
/// <summary>
/// Set driver feedback format to decimal.
/// </summary>
/// <param name="motor"></param>
public void SetFeedbackFormatDecimal(Gripper motor)
{
string res = SendCommand(motor, "IFD");
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
}
private bool jump; // whether the jump button is currently pressed
private void Awake()
{
// Set up the reference.
gripper = GetComponent <Gripper> ();
initialMouseX = Input.mousePosition.x;
initialMouseY = Input.mousePosition.y;
rotate = this.transform.localRotation;
//Cursor.visible = false;
}
public void Load(Gripper gripper, TargetPosition holder)
{
//Todo make sure box is open.
MoveToTargetPosition(gripper, holder);
OpenGripper(gripper);
_motion.ToPointWaitTillEnd(_motion.MotorZ, holder.ApproachHeight);
//Box is OK to close.
_motion.ToPointWaitTillEnd(_motion.MotorY, _motion.PickPosition.YPos);
}
private void MoveFromRightToRight(Gripper gripper, TargetPosition target)
{
_motion.ToPoint(_motion.MotorZ, target.ApproachHeight);
_motion.ToPointX(target.XPos);
ToPointWaitTillEndGripper(target, gripper);
_motion.WaitTillEndX();
_motion.WaitTillEnd(_motion.MotorZ);
MotorYOutThenMotorZDown(target);
}
/// <summary>
/// Get current position of motor.
/// </summary>
/// <param name="motor"></param>
/// <returns></returns>
public double GetPosition(Gripper motor)
{
// Response of "IP" is always hexadecimal
string res = SendCommand(motor, "IP");
string posString = res.Substring(4, res.Length - 4);
int pos = Convert.ToInt32(posString);
return(pos / _countPerDegree);
}
/// <summary>
/// Get state of input pin of driver.
/// </summary>
/// <param name="motor"></param>
/// <param name="input"></param>
/// <returns></returns>
public bool GetInput(Gripper motor, Input input)
{
string res = SendCommand(motor, "IS");
if (res.Length != 12)
{
throw new Exception("Input status response length error");
}
return(res.Substring(12 - (int)input, 1) == "0");
}
private TargetPosition AddOffset(Gripper gripper, TargetPosition target)
{
if (gripper == Gripper.Two & target.Id != Location.Pick)
{
target.XPos = target.XPos + 1.44467773437;
target.YPos = target.YPos - 0.918862304687;
target.APos = target.APos - 0.15;
}
return(target);
}
private Gripper CreateGripper(string bindingPath)
{
var gripper = Gripper.CreateCircleGripper();
var binding = new Binding(bindingPath);
binding.Source = Line;
binding.Mode = BindingMode.TwoWay;
BindingOperations.SetBinding(gripper, Gripper.PositionProperty, binding);
return(gripper);
}
public Robot(Body body, Joystick steeringJoystick, Joystick thrustJoystick, Joystick frontHeightJoystick, Joystick backHeightJoystick, Gripper gripper)
{
this.body = body;
this.steeringJoystick = steeringJoystick;
this.thrustJoystick = thrustJoystick;
this.frontHeightJoystick = frontHeightJoystick;
this.backHeightJoystick = backHeightJoystick;
this.gripper = gripper;
}
private void MoveFromLeftToConveyor(Gripper gripper, TargetPosition target, TargetPosition currentPosition)
{
_motion.ToPoint(_motion.MotorZ, target.ApproachHeight);
ToPointGripper(target, gripper);
_motion.WaitTillZBiggerThan(_motion.PickPosition.ApproachHeight - 20);
_motion.ToPointX(target.XPos);
_motion.WaitTillEndX();
_motion.WaitTillEnd(_motion.MotorZ);
WaitTillEndGripper(target, gripper);
MotorYOutThenMotorZDown(target);
}
public void UnloadAndLoad(TargetPosition target, Gripper gripper)
{
//Todo make sure box is open.
MoveToTargetPosition(gripper, target);
CloseGripper(gripper);
_motion.ToPointWaitTillEnd(_motion.MotorZ, target.ApproachHeight); //Up.
//Todo add offset.
SwitchGripper(target, ref gripper); //Switch gripper.
_motion.ToPointWaitTillEnd(_motion.MotorZ, target.ZPos); //Down.
OpenGripper(gripper);
_motion.ToPointWaitTillEnd(_motion.MotorZ, target.ApproachHeight); //Up.
_motion.ToPointWaitTillEnd(_motion.MotorY, _motion.PickPosition.YPos); //Back.
}
public void SwitchGripper(TargetPosition target, ref Gripper gripper)
{
gripper = gripper == Gripper.One ? Gripper.Two : Gripper.One;
target = AddOffset(gripper, target);
_motion.ToPointX(target.XPos);
_motion.ToPoint(_motion.MotorY, target.YPos);
ToPointWaitTillEndGripper(target, gripper);
_motion.WaitTillEndX();
_motion.WaitTillEnd(_motion.MotorY);
}
/// <summary>
/// Disable motor.
/// </summary>
/// <param name="motor"></param>
public void Disable(Gripper motor)
{
string res = SendCommand(motor, "MD");
if (MotorAcknowledged(motor, res) != true)
{
throw new Exception("Drive is NOT disabled");
}
if (GetStatus(motor, StatusCode.Enabled) == true)
{
throw new Exception("Motor is not disabled");
}
}
private void WaitTillEndGripper(TargetPosition target, Gripper gripper)
{
_motion.WaitTillEnd(_motion.MotorR);
if (gripper == Gripper.One)
{
_gripper.WaitTillEnd(Gripper.One, target.APos);
_gripper.WaitTillEnd(Gripper.Two, 0);
}
else
{
_gripper.WaitTillEnd(Gripper.One, 0);
_gripper.WaitTillEnd(Gripper.Two, target.APos);
}
}
public void ToPointWaitTillEnd(Gripper motor, double angle)
{
int target = Convert.ToInt32(angle * _countPerDegree);
string res = SendCommand(motor, "FP" + target);
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
Thread.Sleep(50);
WaitTillEnd(motor, angle);
}
private void MoveFromConveyorToLeftBottom(Gripper gripper, TargetPosition target)
{
_motion.ToPointX(target.XPos);
_motion.ToPoint(_motion.MotorZ, _motion.PickPosition.ApproachHeight);
ToPointGripper(target, gripper);
_motion.WaitTillXSmallerThan(_motion.ConveyorLeftPosition.XPos);
_motion.BreakToPoint(_motion.MotorZ, target.ApproachHeight);
_motion.WaitTillEndX();
_motion.WaitTillEnd(_motion.MotorZ);
WaitTillEndGripper(target, gripper);
MotorYOutThenMotorZDown(target);
}
/// <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(Gripper motor, double angle, int timeout = 10)
{
double lastPos = GetPosition(motor);
int target = Convert.ToInt32(angle * _countPerDegree);
string res = SendCommand(motor, "FL" + target);
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
Thread.Sleep(50);
WaitTillEnd(motor, lastPos + angle, timeout);
}
private void ToPointGripper(TargetPosition target, Gripper gripper)
{
if (gripper == Gripper.One)
{
_motion.ToPoint(_motion.MotorR, target.RPos);
_gripper.ToPoint(Gripper.One, target.APos);
_gripper.ToPoint(Gripper.Two, 0);
}
else
{
_motion.ToPoint(_motion.MotorR, target.RPos - 60);
_gripper.ToPoint(Gripper.Two, target.APos);
_gripper.ToPoint(Gripper.One, 0);
}
}
public void OpenGripper(Gripper gripper, int timeout = 1000)
{
_io.SetOutput(gripper == Gripper.One ? Output.GripperOne : Output.GripperTwo, true);
Stopwatch sw = new Stopwatch();
sw.Start();
Input sensor = gripper == Gripper.One ? Input.Gripper01Loose : Input.Gripper02Loose;
while (!_io.GetInput(sensor))
{
if (sw.ElapsedMilliseconds > timeout)
{
throw new TimeoutException();
}
Thread.Sleep(10);
}
}
/// <summary>
/// Set speed of motor.
/// </summary>
/// <param name="motor"></param>
/// <param name="velocity">rps</param>
public void SetVelocity(Gripper 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");
string res = SendCommand(motor, "VE" + velocity);
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
}
/// <summary>
/// Move motor to specific angle.
/// </summary>
/// <param name="motor"></param>
/// <param name="angle"></param>
public void ToPoint(Gripper motor, double angle)
{
int target = Convert.ToInt32(angle * _countPerDegree);
string res = SendCommand(motor, "FP" + target);
if (MotorAcknowledged(motor, res) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
if (GetStatus(motor, StatusCode.Alarm))
{
throw new Exception("Drive alarm");
}
Thread.Sleep(50);
}
public async Task <bool> ToPointAsync(Gripper motor1, double angle1, Gripper motor2, double angle2,
int timeout = 10)
{
return(await Task.Run(() =>
{
try
{
int target1 = Convert.ToInt32(angle1 *_countPerDegree);
string res1 = SendCommand(motor1, "FP" + target1);
if (MotorAcknowledged(motor1, res1) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
int target2 = Convert.ToInt32(angle2 *_countPerDegree);
string res2 = SendCommand(motor2, "FP" + target2);
if (MotorAcknowledged(motor2, res2) == false)
{
throw new Exception("Drive is NOT acknowledged");
}
Thread.Sleep(50);
WaitTillEnd(motor1, angle1, timeout);
WaitTillEnd(motor2, angle2, timeout);
double currentPos1 = GetPosition(motor1);
if (Math.Abs(currentPos1 - angle1) > 0.5)
{
throw new Exception("Position error is bigger than 0.5 degree.");
}
double currentPos2 = GetPosition(motor2);
if (Math.Abs(currentPos2 - angle2) > 0.5)
{
throw new Exception("Position error is bigger than 0.5 degree.");
}
return true;
}
catch (Exception)
{
return false;
}
}));
}
public GripperAdapter(Gripper target)
{
_target = target;
}
