public XmlDocument createXMLFromMotion(Motion m)
{
XmlDocument xml = new XmlDocument();
try {
XmlNode rootNode = xml.CreateElement("Sen");
XmlAttribute attribute = xml.CreateAttribute("Type");
attribute.Value = "ImFree";
rootNode.Attributes.Append(attribute);
xml.AppendChild(rootNode);
XmlNode elementNode = xml.CreateElement("Point1X");
elementNode.InnerText = m.X.ToString();
rootNode.AppendChild(elementNode);
elementNode = xml.CreateElement("Point1Y");
elementNode.InnerText = m.Y.ToString();
rootNode.AppendChild(elementNode);
elementNode = xml.CreateElement("Point1Z");
elementNode.InnerText = m.Z.ToString();
rootNode.AppendChild(elementNode);
}
catch (Exception e)
{ Debug.WriteLine(e.ToString()); }
return xml;
}
public void WriteCircle(double CenterX, double CenterY, double Radius)
{
//Besoin de 3 point
motionArm = new Motion(CenterX + Radius, CenterY, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
//Point 1: le départ
motionArm = new Motion(CenterX + Radius, CenterY, down, 0, 0, 180, "LIN");
R.addMotion(motionArm);
//Point 2: un point du cercle
motionArm = new Motion(CenterX, CenterY + Radius, down, 0, 0, 180, "CIRC");
R.addMotion(motionArm);
// Point 3: point d'arrivé;depart du suivant
motionArm = new Motion(CenterX - Radius, CenterY, down, 0, 0, 180, "CIRC");
R.addMotion(motionArm);
//Point 4=: un point du cercle
motionArm = new Motion(CenterX, CenterY - Radius, down, 0, 0, 180, "CIRC");
R.addMotion(motionArm);
//Point 5 = Point 1 fin du cercle
motionArm = new Motion(CenterX + Radius, CenterY, down, 0, 0, 180, "CIRC");
R.addMotion(motionArm);
// On releve la pointe
motionArm = new Motion(CenterX + Radius, CenterY, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(CenterX + Radius, CenterY, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
}
//Z = -1.5 => Contact avec le papier; 8.5 => pas de contact
public RobotWrite2(double X, double Y)
{
R = new Robot2();
X_Max = X;
Y_Max = Y;
motionArm = new Motion(0, 0, 8.5, 0, 0, 180, "PTP");
R.addMotion(motionArm);
}
public void addMotion(Motion motion)
{
Motion tempMotion;
lock (motionStack)
{
if (motion is MotionParameters)
{
tempMotion = new MotionParameters(motion.X, motion.Y, motion.Z, motion.A, motion.B, motion.C, motion.name, ((MotionParameters)motion).C_DIS, ((MotionParameters)motion).gripClose);
}
else
{
tempMotion = new Motion(motion.X, motion.Y, motion.Z, motion.A, motion.B, motion.C, motion.name);
}
}
lock (motionStack)
{
motionStack.Add(motion);
Debug.WriteLine("StackSize : " + stackSize());
}
}
public void WriteHalfCircle(double CenterX, double CenterY, double Radius, Boolean circledown)
{
Double Point2Y = CenterY + Radius;
//Besoin de 3 point
motionArm = new Motion(CenterX + Radius, CenterY, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
//Point 1: le départ
motionArm = new Motion(CenterX + Radius, CenterY, down, 0, 0, 180, "LIN");
R.addMotion(motionArm);
//Point 2: un point du cercle.
if (circledown) { Point2Y = CenterY - Radius; }
motionArmP = new MotionParameters(CenterX, Point2Y, down, 0, 0, 180, "CIRC", 5, true);
R.addMotion(motionArmP);
// Point 3: point d'arrivé
motionArmP = new MotionParameters(CenterX - Radius, CenterY, down, 0, 0, 180, "CIRC", 5, true);
R.addMotion(motionArmP);
// On releve la pointe
motionArm = new Motion(CenterX - Radius, CenterY, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(CenterX - Radius, CenterY, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
}
public void WriteLine(double X1, double Y1, double X2, double Y2)
{
motionArm = new Motion(X1, Y1, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(X1, Y1, down, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(X2, Y2, down, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(X2, Y2, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
}
public void MoveToPoint(double X1, double Y1, bool isUp)
{
double z = isUp ? up : down;
motionArm = new Motion(X1, Y1, z, 0, 0, 180, "LIN");
R.addMotion(motionArm);
}
/*public void WriteArc(double X1, double Y1,double X2,double Y2, double Radius)
{
//Calcul du centre du cercle
double Xc, Yc;
double Xd, Yd;
double a; //distance entre point 1 et point D
double b; //Distance entre point d et centre c
Xd = ((X2 - X1) / 2) + X1;
Yd = ((Y2 - Y1) / 2) + Y1;
a = Math.Sqrt(Math.Pow(Xd - X1, 2) + Math.Pow(Yd - Y1, 2));
//Pythagore
b = Math.Sqrt(Math.Pow(Radius, 2) - Math.Pow(a, 2));
//Besoin de 3 point
motionArm = new MotionParameters(X1, Y1, up, 0, 0, 180, "PTP", 5, true);
R.addMotion(motionArm);
//Point 1: le départ
motionArm = new MotionParameters(X1, Y1, down, 0, 0, 180, "PTP", 5, true);
R.addMotion(motionArm);
//Point 2: un point du cercle
motionArm = new MotionParameters(((X2 - X1)/2) + X1, Y1 + Radius, down, 0, 0, 180, "CIRC", 5, true);
R.addMotion(motionArm);
// Point 3: point d'arrivé
motionArm = new MotionParameters(X2, Y2, down, 0, 0, 180, "CIRC", 5, true);
R.addMotion(motionArm);
motionArm = new MotionParameters(X2, Y2, up, 0, 0, 180, "PTP", 5, true);
R.addMotion(motionArm);
}*/
public void WritePoint(double X, double Y)
{
motionArm = new Motion(X, Y, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(X, Y, down, 0, 0, 180, "LIN");
R.addMotion(motionArm);
motionArm = new Motion(X, Y, up, 0, 0, 180, "LIN");
R.addMotion(motionArm);
}
public void copy(Motion m)
{
this.copy((Coordinates)m);
_motionName.Remove(0, _motionName.Length);
_motionName.Append(m.name);
}
public void addMotion(Motion motion)
{
if (cli.clientReady)
{
Motion tempMotion;
//s'il y a beaucoup de mouvements, on ralenti pour
//éviter de saturer la mémoire
lock (motionStack)
{
if (motion is MotionParameters)
{
tempMotion = new MotionParameters(motion.X, motion.Y, motion.Z, motion.A, motion.B, motion.C, motion.name, ((MotionParameters)motion).C_DIS, ((MotionParameters)motion).gripClose);
}
else
{
tempMotion = new Motion(motion.X, motion.Y, motion.Z, motion.A, motion.B, motion.C, motion.name);
}
}
lock (motionStack)
{
motionStack.Add(tempMotion);
}
}
}
private void movePTP(Motion mot)
{
lock (XmlToRobot)
{
try
{
XmlNode tempRoot = XmlToRobot.DocumentElement;
tempRoot.SelectSingleNode("motionType").InnerText = "1";
tempRoot.SelectSingleNode("Point1X").InnerText = mot.X.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1Y").InnerText = mot.Y.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1Z").InnerText = mot.Z.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1A").InnerText = mot.A.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1B").InnerText = mot.B.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1C").InnerText = mot.C.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("CONTROL_LOOP").InnerText = motionControl();
}
catch (NullReferenceException e) { Console.WriteLine(e); }
do
{
//solicitation d'un document xml du serveur
if (cli.clientReady)
{
System.Threading.Thread.Sleep(tempo);
lock (cli.SendXml)
{
cli.SendXml.DocumentElement.InnerXml = XmlToRobot.DocumentElement.InnerXml;
}
}
} while (!cli.clientReady);
}
while (!ready)
{
if (((Coordinates)mot).distCartBool(PosState(), 10000, distAckMotion))
{
ready = true;
}
}
}
private void moveLIN(Motion mot, Boolean CDIS)
{
lock (XmlToRobot)
{
try
{
XmlNode tempRoot = XmlToRobot.DocumentElement;
if (CDIS == true)
{
tempRoot.SelectSingleNode("motionType").InnerText = "4";
}
else
{
tempRoot.SelectSingleNode("motionType").InnerText = "2";
}
tempRoot.SelectSingleNode("Point1X").InnerText = mot.X.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1Y").InnerText = mot.Y.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1Z").InnerText = mot.Z.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1A").InnerText = mot.A.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1B").InnerText = mot.B.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("Point1C").InnerText = mot.C.ToString().Replace(',', '.');
tempRoot.SelectSingleNode("CONTROL_LOOP").InnerText = motionControl();
}
catch (NullReferenceException e) { Console.WriteLine(e); }
do
{
if (cli.clientReady)
{
//sollicitation d'un doccument xml du serveur
System.Threading.Thread.Sleep(tempo);
lock (cli.SendXml)
{
cli.SendXml.DocumentElement.InnerXml = XmlToRobot.DocumentElement.InnerXml;
}
}
} while (!cli.clientReady);
}
//System.Windows.Forms.MessageBox.Show("VERIF COORDS LIN");
while (!ready)
{
if (((Coordinates)mot).distCartBool(PosState(), 10000, distAckMotion))
{
ready = true;
//System.Windows.Forms.MessageBox.Show("FIN LIN");
}
}
}
