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