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