private bool movementT(ref double[] angles, RobotCommand robotCommand) { if (angles[4] < robotCommand.RotationT && angles[4] < joints[4].angleMax) { angles[4]++; } else if (angles[4] > robotCommand.RotationT && angles[4] > joints[4].angleMin) { angles[4]--; } else { return(true); } return(false); }
private bool movementX(ref double[] angles, RobotCommand robotCommand) { if (angles[0] < robotCommand.PositionX && angles[0] < joints[0].angleMax) { angles[0]++; } else if (angles[0] > robotCommand.PositionX && angles[0] > joints[0].angleMin) { angles[0]--; } else { return(true); } return(false); }
public void Movement(double PositionX, double PositionY, double RotationT, int Gripper) { if (Gripper == 2) { ParkingZone = true; } else if (Gripper == 3) { ParkingZone = false; } if (ParkingZone == false) { NewPositionX = PositionX; NewPositionY = PositionY; NewRotationT = RotationT; AnimationGripper = 0; if (NewGripper != Gripper) { NewGripper = Gripper; AnimationGripper = 1; if (NewGripper == 0) { NewGripperPos = joints[5].angleMax; } else if (NewGripper == 1) { NewGripperPos = joints[5].angleMin; } else { AnimationGripper = 0; } } lastRobotCommand = new RobotCommand(NewPositionX, NewPositionY, NewRotationT, AnimationGripper, NewGripperPos); joints[5].angleMin = 0; } else { NewPositionX = -45; AnimationGripper = 0; lastRobotCommand = new RobotCommand(NewPositionX, NewPositionY, NewRotationT, AnimationGripper, NewGripperPos); joints[5].angleMin = 0; } }
private void BtnClearAll_Click(object sender, RoutedEventArgs e) { lastRobotCommand = null; Boxs[0] = new box(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), -45), new Point3D(0, 0, 0)), new TranslateTransform3D(-700, -2600, joints[8].model.Bounds.SizeZ + 60)); Boxs[1] = new box(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), 45), new Point3D(0, 0, 0)), new TranslateTransform3D(700, -1240, joints[9].model.Bounds.SizeZ + 60)); Boxs[2] = new box(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), 45), new Point3D(0, 0, 0)), new TranslateTransform3D(560, -3840, joints[10].model.Bounds.SizeZ + 60)); Boxs[3] = new box(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), -45), new Point3D(0, 0, 0)), new TranslateTransform3D(1960, -2500, joints[11].model.Bounds.SizeZ + 60)); double[] angles = { joints[0].angle = 0, joints[1].angle = 0, joints[2].angle = 0, joints[3].angle = 90, joints[4].angle = 0, joints[5].angle = 70 }; ForwardKinematics(angles); CommandQueue.Items.Clear(); TbStatus.Text = TbLastN.Text = "0"; TbM11.Text = TbM12.Text = TbM13.Text = TbM14.Text = TbM15.Text = TbM16.Text = "0"; TbT11.Text = TbT12.Text = TbT13.Text = TbT14.Text = TbT15.Text = TbT16.Text = "0"; TbL11.Text = TbL12.Text = TbL13.Text = TbL14.Text = TbL15.Text = TbL16.Text = "0"; TbMR1n.Text = TbMR1x.Text = TbMR1y.Text = TbMR1t.Text = TbMR1g.Text = TbMR1l.Text = "0"; TbMR2n.Text = TbMR2x.Text = TbMR2y.Text = TbMR2t.Text = TbMR2g.Text = TbMR2l.Text = "0"; TbLogRobotSimulator.Text = ""; }
private bool movementG(ref double[] angles, RobotCommand robotCommand) { if (angles[5] < robotCommand.GripperPos && angles[5] < joints[5].angleMax) { angles[5]++; for (int i = 0; i < 4; i++) { if (Boxs[i].g == 1) { joints[5].angleMin = 0; joints[6].angleMin = 0; Boxs[i].g = 0; Boxs[i].T = new TranslateTransform3D(joints[8 + i].model.Bounds.Location.X + joints[8 + i].model.Bounds.SizeX / 2, joints[8 + i].model.Bounds.Location.Y + joints[8 + i].model.Bounds.SizeY / 2, joints[8 + i].model.Bounds.SizeZ + 60); Boxs[i].R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), joints[0].angle + joints[4].angle - 180), new Point3D(0, 0, 0)); break; } } } else if (angles[5] > robotCommand.GripperPos && angles[5] > joints[5].angleMin) { angles[5]--; if (Boxs[0].g == 0 && Boxs[1].g == 0 && Boxs[2].g == 0 && Boxs[3].g == 0) { for (int i = 0; i < 4; i++) { if (joints[8 + i].model.Bounds.IntersectsWith(joints[5].model.Bounds) && joints[8 + i].model.Bounds.IntersectsWith(joints[6].model.Bounds)) { joints[5].angleMin = 34; joints[6].angleMin = -34; Boxs[i].g = 1; Boxs[i].T = new TranslateTransform3D(0, 0, joints[8 + i].model.Bounds.SizeZ - 150); Boxs[i].R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 90), new Point3D(0, 0, 0)); break; } } } } else { return(true); } return(false); }
public void RenderTimer_Tick(object sender, EventArgs e) { if (FromCommandQueue.IsChecked == true && lastRobotCommand == null && CommandQueue.Items.Count > 0) { string str = this.CommandQueue.Items[0].ToString(); string[] separator = new string[] { ";" }; string[] strArray = str.Split(separator, StringSplitOptions.None); N = int.Parse(strArray[0]); X1 = int.Parse(strArray[1]); Y1 = int.Parse(strArray[2]); T1 = int.Parse(strArray[3]); G1 = int.Parse(strArray[4]); L1 = int.Parse(strArray[5]); СommandExecution(); CommandQueue.Items.RemoveAt(0); movements += 50000; TbStatus.Text = "1"; } if (lastRobotCommand != null) { double[] angles = { joints[0].angle, joints[1].angle, joints[2].angle, joints[3].angle, joints[4].angle, joints[5].angle }; switch (lastRobotCommand.Animation) { case 0: bool x = movementX(ref angles, lastRobotCommand); bool t = movementT(ref angles, lastRobotCommand); if (x && t) { lastRobotCommand.Animation++; lastRobotCommand.point = СhangeLengthVector(new Vector3D(joints[12].model.Bounds.Location.X, joints[12].model.Bounds.Location.Y, joints[12].model.Bounds.Location.Z), lastRobotCommand.PositionY); } break; case 1: angles = InverseKinematics(lastRobotCommand.point, angles); if (movements == 0) { lastRobotCommand.Animation++; movements = 50000; lastRobotCommand.point = new Vector3D(joints[12].model.Bounds.Location.X, joints[12].model.Bounds.Location.Y, joints[12].model.Bounds.Location.Z); } break; case 2: if (lastRobotCommand.GripperMode == 1) { angles = InverseKinematics(new Vector3D(lastRobotCommand.point.X, lastRobotCommand.point.Y, 470), angles); } else { movements = 0; } if (movements == 0) { lastRobotCommand.Animation++; movements = 50000; } break; case 3: if (lastRobotCommand.GripperMode == 0 || movementG(ref angles, lastRobotCommand)) { lastRobotCommand.Animation++; } break; case 4: if (lastRobotCommand.GripperMode == 1) { angles = InverseKinematics(lastRobotCommand.point, angles); } else { movements = 0; } if (movements == 0) { lastRobotCommand.Animation++; movements = 50000; } break; default: movements = 1; break; } ForwardKinematics(angles); joints[0].angle = angles[0]; joints[1].angle = angles[1]; joints[2].angle = angles[2]; joints[3].angle = angles[3]; joints[4].angle = angles[4]; joints[5].angle = angles[5]; if (movements % 90 == 0) { FillServomotors(); } } if ((--movements) <= 0) { lastRobotCommand = null; TbStatus.Text = "0"; } }