public Data_State(int id, double x, double y, double z, double a, double b, double c, bool gripper)
{
this.id = id;
this.gripper = gripper;
this.location = new NLX.Robot.Kuka.Controller.CartesianPosition();
this.location.A = a;
this.location.B = b;
this.location.C = c;
this.location.X = x;
this.location.Y = y;
this.location.Z = z;
}
public Data_State(dynamic s)
{
this.id = s.id;
this.gripper = s.gripper;
this.location = new NLX.Robot.Kuka.Controller.CartesianPosition();
this.location.A = s.location.A;
this.location.B = s.location.B;
this.location.C = s.location.C;
this.location.X = s.location.X;
this.location.Y = s.location.Y;
this.location.Z = s.location.Z;
}
public void changeLocation(NLX.Robot.Kuka.Controller.CartesianPosition currentMouvement)
{
lastMouvement.A = currentMouvement.A;
lastMouvement.B = currentMouvement.B;
lastMouvement.C = currentMouvement.C;
lastMouvement.X = currentMouvement.X;
lastMouvement.Y = currentMouvement.Y;
lastMouvement.Z = currentMouvement.Z;
NLX.Robot.Kuka.Controller.CartesianPosition posRelRobot = new NLX.Robot.Kuka.Controller.CartesianPosition();
posRelRobot.A = lastMouvement.A * Program.robot.getRotSpeed(); // B
posRelRobot.B = lastMouvement.C * Program.robot.getRotSpeed(); // C
posRelRobot.C = lastMouvement.B * Program.robot.getRotSpeed(); // A
posRelRobot.X = -lastMouvement.Z * Program.robot.getSpeed();
posRelRobot.Y = -lastMouvement.X * Program.robot.getSpeed();
posRelRobot.Z = lastMouvement.Y * Program.robot.getSpeed();
//Console.WriteLine("[" + posRelRobot.A + ":" + posRelRobot.B + ":" + posRelRobot.C + "] [" + posRelRobot.X + ":" + posRelRobot.Y + ":" + posRelRobot.Z + "]");
Program.robot.translateRelative(posRelRobot);
}
static void traiterDeplacements(double _X, double _Y, double _Z, double _A, double _B, double _C)
{
// init robot
// get pos from souris
var pos = new NLX.Robot.Kuka.Controller.CartesianPosition();
pos.X = _X;
pos.Y = _Y;
pos.Z = _Z;
pos.A = _A;
pos.B = _B;
pos.C = _C;
if ((pos.X == 0) && (pos.Y == 0) && (pos.Z == 0) && (pos.A == 0) && (pos.B == 0) && (pos.C == 0))
{
//Console.WriteLine("0 pos");
if (started == true)
{
Console.WriteLine("about to stop");
theRobot.StopRelativeMovement();
started = false;
}
//Console.WriteLine("print after stop , n***a");
}
else
{
if (started == false)
{
Console.WriteLine("about to start");
started = true;
//Console.WriteLine("test7");
Console.WriteLine("before StartRelativeMovement");
theRobot.StartRelativeMovement();
Console.WriteLine("after StartRelativeMovement");
}
//var sensorTest = theRobot.ReadSensor();
//Console.WriteLine("b: " + sensorTest);
//Console.WriteLine("test5");
//theRobot.OpenGripper();
//Thread.Sleep(1000);
//theRobot.CloseGripper();
//Console.WriteLine("test6");
//var pos = theRobot.GetCurrentPosition();
//Console.WriteLine("pos.X: " + pos.X);
//Console.WriteLine("pos.X: " + pos.X);
Console.WriteLine("before setRelativeMovement");
theRobot.SetRelativeMovement(pos);
Console.WriteLine("after setRelativeMovement");
//Console.WriteLine("test8");
}
}
public void setLocation(NLX.Robot.Kuka.Controller.CartesianPosition val)
{
this.location = val;
}
public static void KeyboardThreadLoop()
{
ConsoleKeyInfo keyinfo;
do
{
keyinfo = Console.ReadKey();
Console.WriteLine(keyinfo.Key + " was pressed");
if ((keyinfo.KeyChar == 'a') || (keyinfo.KeyChar == 'A')) // demander un nouveau coeff translation
{
Console.WriteLine("donne le nouveau coeff de translation stp (entre 0 et 1.0, il vaut actuellement " + coeffTrans + "):");
string s = Console.ReadLine();
try {
double newCoeff = Double.Parse(s);
coeffTrans = newCoeff;
Console.WriteLine("le nouveau coeff de translation vaut: " + coeffTrans);
}
catch (FormatException e) {
Console.WriteLine("FormatException occured while trying to parse your data");
}
}
else if ((keyinfo.KeyChar == 'z') || (keyinfo.KeyChar == 'Z')) // demander un nouveau coeff rotation
{ // demander une nouvelle vitesse
Console.WriteLine("donne le nouveau coeff de rotation stp (entre 0 et 1.0, il vaut actuellement " + coeffRot + "):");
string s = Console.ReadLine();
try
{
double newCoeff = Double.Parse(s);
coeffRot = newCoeff;
Console.WriteLine("le nouveau coeff de rotation vaut: " + coeffRot);
}
catch (FormatException e)
{
Console.WriteLine("FormatException occured while trying to parse your data");
}
}
else if ((keyinfo.KeyChar == 'e') || (keyinfo.KeyChar == 'E')) // enregistrer la position current du robot
{ // demander une nouvelle vitesse
Console.WriteLine("enregistre la position current du robot");
NLX.Robot.Kuka.Controller.CartesianPosition currentPos = theRobot.GetCurrentPosition();
Console.WriteLine("current position:");
Console.WriteLine("x: " + currentPos.X);
Console.WriteLine("y: " + currentPos.Y);
Console.WriteLine("z: " + currentPos.Z);
Console.WriteLine("a: " + currentPos.A);
Console.WriteLine("b: " + currentPos.B);
Console.WriteLine("c: " + currentPos.C);
if (modeCapture == true)
{
//a finir , convertir double en string mais attention ! virgule ou point?!
sh.addInstruction(currentPos.X.ToString());
sh.addInstruction(currentPos.Y.ToString());
sh.addInstruction(currentPos.Z.ToString());
sh.addInstruction(currentPos.A.ToString());
sh.addInstruction(currentPos.B.ToString());
sh.addInstruction(currentPos.C.ToString());
sh.addInstruction("");
}
}
else if ((keyinfo.KeyChar == 'd') || (keyinfo.KeyChar == 'D')) // enregistrer la position current du robot en tant que p0, p1, p2, et p3
{ // demander une nouvelle vitesse
string point = "p";
if (pointAEnregistrer == 0)
{
point = "p0";
pointAEnregistrer += 1;
}
else if (pointAEnregistrer == 1)
{
point = "p1";
pointAEnregistrer += 1;
}
else if (pointAEnregistrer == 2)
{
point = "p0";
pointAEnregistrer += 1;
}
else if (pointAEnregistrer == 3)
{
point = "p2";
pointAEnregistrer += 1;
}
else if (pointAEnregistrer == 4)
{
point = "p3";
pointAEnregistrer += 1;
}
Console.WriteLine("enregistre la position current du robot");
NLX.Robot.Kuka.Controller.CartesianPosition currentPos = theRobot.GetCurrentPosition();
if (modeCapture == true)
{
//a finir , convertir double en string mais attention ! virgule ou point?!
sh.addInstruction(point);
sh.addInstruction(currentPos.X.ToString());
sh.addInstruction(currentPos.Y.ToString());
sh.addInstruction(currentPos.Z.ToString());
sh.addInstruction(currentPos.A.ToString());
sh.addInstruction(currentPos.B.ToString());
sh.addInstruction(currentPos.C.ToString());
sh.addInstruction("");
}
}
else if ((keyinfo.KeyChar == 'r') || (keyinfo.KeyChar == 'R')) // incr coeff trans
{ // demander une nouvelle vitesse
coeffTrans += 0.1;
if (coeffTrans > 1.0)
{
coeffTrans = 1.0;
}
Console.WriteLine("coeffTrans: " + coeffTrans);
}
else if ((keyinfo.KeyChar == 't') || (keyinfo.KeyChar == 'T')) // desincr coeff trans
{ // demander une nouvelle vitesse
coeffTrans -= 0.1;
if (coeffTrans < 0.0)
{
coeffTrans = 0.0;
}
Console.WriteLine("coeffTrans: " + coeffTrans);
}
else if ((keyinfo.KeyChar == 'y') || (keyinfo.KeyChar == 'Y')) // incr coeff rot
{ // demander une nouvelle vitesse
coeffRot += 0.1;
if (coeffRot > 1.0)
{
coeffRot = 1.0;
}
Console.WriteLine("coeffRot: " + coeffRot);
}
else if ((keyinfo.KeyChar == 'u') || (keyinfo.KeyChar == 'U')) // desincr coeff rot
{ // demander une nouvelle vitesse
coeffRot -= 0.1;
if (coeffRot < 0.0)
{
coeffRot = 0.0;
}
Console.WriteLine("coeffRot: " + coeffRot);
}
else if ((keyinfo.KeyChar == 'i') || (keyinfo.KeyChar == 'I')) // open grip
{ // demander une nouvelle vitesse
theRobot.OpenGripper();
Console.WriteLine("open grip");
if (modeCapture == true)
{
sh.addInstruction("open");
sh.addInstruction("");
}
}
else if ((keyinfo.KeyChar == 'o') || (keyinfo.KeyChar == 'O')) // close grip
{ // demander une nouvelle vitesse
theRobot.CloseGripper();
Console.WriteLine("close grip");
if (modeCapture == true)
{
sh.addInstruction("close");
sh.addInstruction("");
}
}
else if ((keyinfo.KeyChar == 'p') || (keyinfo.KeyChar == 'P')) // changer mode
{
modeDefaut = !modeDefaut;
Console.WriteLine(modeDefaut);
}
else if ((keyinfo.KeyChar == 'q') || (keyinfo.KeyChar == 'Q')) // lancer scenario
{
if (modeDefaut == true)
{
Console.WriteLine("in Q default");
ScenarioHandler sh = new ScenarioHandler();
sh.readAndInterpretFile2(scenarioDefaut, theRobot);
}
else
{
Console.WriteLine("in Q genered");
ScenarioHandler sh = new ScenarioHandler();
sh.readAndInterpretFile2(scenarioGenere, theRobot);
}
}
else if ((keyinfo.KeyChar == 's') || (keyinfo.KeyChar == 'S')) // changer Capture / terminer capture
{
if (modeCapture == false)
{
modeCapture = true;
sh.openFile(scenarioGenere);
Console.WriteLine("Lancement capture");
Thread myThreadCapture;
myThreadCapture = new Thread(new ThreadStart(lancerModeCapture));
myThreadCapture.Start();
}
else
{
//terminer capture
Console.WriteLine("testArretcapture");
modeCapture = false;
sh.closeFile(scenarioGenere);
Console.WriteLine("Arretcapture");
}
}
}while (Thread.CurrentThread.IsAlive);
}
static void traiterDeplacements(double _X, double _Y, double _Z, double _A, double _B, double _C)
{
// init robot
// get pos from souris
var pos = new NLX.Robot.Kuka.Controller.CartesianPosition();
pos.X = _X;
pos.Y = _Y;
pos.Z = _Z;
pos.A = _A;
pos.B = _B;
pos.C = _C;
if ((pos.X == 0) && (pos.Y == 0) && (pos.Z == 0) && (pos.A == 0) && (pos.B == 0) && (pos.C == 0))
{
//Console.WriteLine("0 pos");
if (started == true)
{
Console.WriteLine("about to stop");
theRobot.StopRelativeMovement();
started = false;
}
//Console.WriteLine("print after stop , n***a");
}
else
{
if (started == false)
{
Console.WriteLine("about to start");
started = true;
//Console.WriteLine("test7");
Console.WriteLine("before StartRelativeMovement");
theRobot.StartRelativeMovement();
Console.WriteLine("after StartRelativeMovement");
}
//var sensorTest = theRobot.ReadSensor();
//Console.WriteLine("b: " + sensorTest);
//Console.WriteLine("test5");
//theRobot.OpenGripper();
//Thread.Sleep(1000);
//theRobot.CloseGripper();
//Console.WriteLine("test6");
//var pos = theRobot.GetCurrentPosition();
//Console.WriteLine("pos.X: " + pos.X);
//Console.WriteLine("pos.X: " + pos.X);
Console.WriteLine("before setRelativeMovement");
theRobot.SetRelativeMovement(pos);
Console.WriteLine("after setRelativeMovement");
//Console.WriteLine("test8");
}
}
public void Update()
{
//double xMax = 0, yMax = 0, zMax = 0, aMax = 0, bMax = 0, cMax = 0;
double xMax = 2047, yMax = 2189, zMax = 948, aMax = 2062, bMax = 2062, cMax = 2062;
while (true)
{
if (device.IsConnected)
{
System.Threading.Thread.Sleep(100);
NLX.Robot.Kuka.Controller.CartesianPosition currentMouvement = new NLX.Robot.Kuka.Controller.CartesianPosition();
if (device.Keyboard.IsKeyDown(15))
Console.WriteLine("gneeeeeeeeeeeee");
//Program.robot.swapGripper();
// WITHOUT DOMINANT CONFIG
/* TEST 1 MAX calibrage*/
/*
// ROTATION
if (Math.Abs(device.Sensor.Rotation.X) > aMax)
aMax = Math.Abs(device.Sensor.Rotation.X);
if (Math.Abs(device.Sensor.Rotation.Y) > bMax)
bMax = Math.Abs(device.Sensor.Rotation.Y);
if (Math.Abs(device.Sensor.Rotation.Z) > cMax)
cMax = Math.Abs(device.Sensor.Rotation.Z);
// TRANSLATION
if (Math.Abs(device.Sensor.Translation.Z) > xMax)
xMax = Math.Abs(device.Sensor.Translation.X);
if (Math.Abs(device.Sensor.Translation.Y) > yMax)
yMax = Math.Abs(device.Sensor.Translation.Y);
if (Math.Abs(device.Sensor.Translation.Z) > zMax)
zMax = Math.Abs(device.Sensor.Translation.Z);
*/
//Console.WriteLine("MAX: (x:" + xMax + ")(y:" + yMax + ")(z:" + zMax + ")(A:" + aMax + ")(B:" + bMax + ")(C:" + cMax + ")");
//Console.WriteLine("MAX: (x:" + device.Sensor.Translation.X + ")(y:" + device.Sensor.Translation.Y + ")(z:" + device.Sensor.Translation.Z + ")(A:" + device.Sensor.Rotation.X + ")(B:" + device.Sensor.Rotation.Y + ")(C:" + device.Sensor.Rotation.Z + ")");
/* TEST 2 pourcentage*/
/*
double px = Math.Abs(Math.Round((device.Sensor.Translation.X / xMax) * 100)/100);
double py = Math.Abs(Math.Round((device.Sensor.Translation.Y / yMax) * 100)/ 100);
double pz = Math.Abs(Math.Round((device.Sensor.Translation.Z / zMax) * 100)/ 100);
double pa = Math.Abs(Math.Round((device.Sensor.Rotation.X / aMax) * 100)/ 100);
double pb = Math.Abs(Math.Round((device.Sensor.Rotation.Y / bMax) * 100)/ 100);
double pc = Math.Abs(Math.Round((device.Sensor.Rotation.Z / cMax) * 100)/ 100);
//Console.WriteLine("MAX: (x:" + px + ")(y:" + py + ")(z:" + pz + ")(A:" + pa + ")(B:" + pb + ")(C:" + pc + ")");
if (px > 0.3 && px > py && px > pz)
currentMouvement.X = Math.Round((device.Sensor.Translation.X / xMax) * 1000) / 1000;
else if (py > 0.3 && py > px && py > pz)
currentMouvement.Y = Math.Round((device.Sensor.Translation.Y / yMax) * 1000) / 1000;
else if (pz > 0.3 && pz > px && pz > py)
currentMouvement.Z = Math.Round((device.Sensor.Translation.Z / zMax) * 1000) / 1000;
else if (pa > pb && pa > pc)
currentMouvement.A = Math.Round((device.Sensor.Rotation.X / aMax) * 1000) / 1000;
else if (pb > pa && pb > pc)
currentMouvement.B = Math.Round((device.Sensor.Rotation.Y / bMax) * 1000) / 1000;
else if (pc > pa && pc > pb)
currentMouvement.C = Math.Round((device.Sensor.Rotation.Z / cMax) * 1000) / 1000;
//Console.WriteLine("CurrentMouvement: (x:" + currentMouvement.X + ")(y:" + currentMouvement.Y + ")(z:" + currentMouvement.Z + ")(A:" + currentMouvement.A + ")(B:" + currentMouvement.B + ")(C:" + currentMouvement.C + ")");
*/
// WITH DOMINANT CONFIG
currentMouvement.X = Math.Round((device.Sensor.Translation.X / xMax) * 1000) / 1000;
currentMouvement.Y = Math.Round((device.Sensor.Translation.Y / yMax) * 1000) / 1000;
currentMouvement.Z = Math.Round((device.Sensor.Translation.Z / zMax) * 1000) / 1000;
currentMouvement.A = Math.Round((device.Sensor.Rotation.X * device.Sensor.Rotation.Angle / aMax) * 1000) / 1000;
currentMouvement.B = Math.Round((device.Sensor.Rotation.Y * device.Sensor.Rotation.Angle / bMax) * 1000) / 1000;
currentMouvement.C = Math.Round((device.Sensor.Rotation.Z * device.Sensor.Rotation.Angle / cMax) * 1000) / 1000;
if (lastMouvement != null)
{
if (currentMouvement.A != lastMouvement.A ||
currentMouvement.B != lastMouvement.B ||
currentMouvement.C != lastMouvement.C ||
currentMouvement.X != lastMouvement.X ||
currentMouvement.Y != lastMouvement.Y ||
currentMouvement.Z != lastMouvement.Z)
{
Console.WriteLine("CurrentMouvement: (x:" + currentMouvement.X + ")(y:" + currentMouvement.Y + ")(z:" + currentMouvement.Z + ")(A:" + currentMouvement.A + ")(B:" + currentMouvement.B + ")(C:" + currentMouvement.C + ")");
changeLocation(currentMouvement);
}
}
else
{
lastMouvement = new NLX.Robot.Kuka.Controller.CartesianPosition();
changeLocation(currentMouvement);
}
}
}
}
public void readAndInterpretFile2(string path, NLX.Robot.Kuka.Controller.RobotController robot)
{
Console.WriteLine("in readAndInterpretFile2");
Console.WriteLine("path: " + path);
int nbLignesPalette = 4;
int nbColonnesPalette = 4;
parseFile(path);
double xdecx = point1.X - point2.X;
double ydecx = point1.Y - point2.Y;
double xdecy = point1.X - point3.X;
double ydecy = point1.Y - point3.Y;
Console.WriteLine("xdecx: " + xdecx);
Console.WriteLine("ydecx: " + ydecx);
Console.WriteLine("xdecy: " + xdecy);
Console.WriteLine("ydecy: " + ydecy);
try
{
//for (int ligne = 0; ligne <= nbLignesPalette; ligne++)
for (int colonne = 0; colonne <= nbColonnesPalette; colonne++)
{
for (int ligne = 0; ligne <= nbLignesPalette; ligne++)
//for (int colonne = 0; colonne <= nbColonnesPalette; colonne++)
{
Console.WriteLine("ligne: " + ligne + ",colonne: " + colonne);
if (File.Exists(path))
{
using (StreamReader sr = new StreamReader(path))
{
double d;
//new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
listPos = new List <NLX.Robot.Kuka.Controller.CartesianPosition>();
while (sr.Peek() >= 0)
{
string str = sr.ReadLine();
if (Double.TryParse(str, out d))
{
//
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
Console.WriteLine("x: " + d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("y: " + d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("z: " + d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("a: " + d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("b: " + d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("c: " + d);
cpos.C = d;
str = sr.ReadLine();
Console.WriteLine(str);
Console.WriteLine("robot: " + robot);
Console.WriteLine("listPos: " + listPos);
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else
{
// play the list of positions if it's not empty
Console.WriteLine("rob: " + robot);
Console.WriteLine("listpos: " + listPos);
fctPlay(robot);
if (str == "open")
{
// do the open
Console.WriteLine("doing open gripper");
robot.OpenGripper();
}
else if (str == "close")
{
Thread.Sleep(1000);
// do the close
Console.WriteLine("doing close gripper");
robot.CloseGripper();
}
else if (str == "p0")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Console.WriteLine("str: " + str);
Double.TryParse(str, out d);
//cpos.X = d + ligne*xdecy + colonne*xdecy;
Console.WriteLine("ligne: " + ligne);
Console.WriteLine("colonne: " + colonne);
cpos.X = d + (-1) * ligne * xdecx + (-1) * colonne * xdecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
//cpos.Y = d + ligne*ydecx + colonne*ydecy;
cpos.Y = d + (-1) * ligne * ydecx + (-1) * colonne * ydecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else if (str == "p1")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Double.TryParse(str, out d);
//cpos.X = d + ligne * xdecx + colonne * xdecy;
cpos.X = d + (-1) * ligne * xdecx + (-1) * colonne * xdecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
//cpos.Y = d + ligne * ydecx + colonne * ydecy;
cpos.Y = d + (-1) * ligne * ydecx + (-1) * colonne * ydecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else if (str == "p2")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
}
else if (str == "p3")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
}
}
}
fctPlay(robot);
}
}
else
{
Console.WriteLine("it doesn't exist");
}
}
}
Console.WriteLine("in readAndInterpretFile");
Console.WriteLine("path: " + path);
listPos = new List <NLX.Robot.Kuka.Controller.CartesianPosition>();
}
catch (Exception e)
{
Console.WriteLine("The process failed: {0}", e.ToString());
}
}
private void launchStockAction()
{
locations1.Clear();
locations2.Clear();
areaStageTmp1 = false;
areaStageTmp2 = false;
Data_Area area = Program.data.getAreaByName(this.currentAction["area"].ToString());
locations1.Add(area.getP0());
if (this.currentAction["stockType"].ToString().Equals("drop"))
{
Data_Stock stockLocation = area.getBestStockLocationToDrop();
stockLocation.setEmpty(false);
NLX.Robot.Kuka.Controller.CartesianPosition posUp = new NLX.Robot.Kuka.Controller.CartesianPosition(stockLocation.getLocation());
posUp.Z = area.getP0().Z;
NLX.Robot.Kuka.Controller.CartesianPosition posDown = new NLX.Robot.Kuka.Controller.CartesianPosition(stockLocation.getLocation());
locations1.Add(posUp);
locations1.Add(posDown);
locations2.Add(posUp);
locations2.Add(area.getP0());
}
else if (this.currentAction["stockType"].ToString().Equals("pick"))
{
Program.robot.openGripper();
Data_Stock stockLocation = area.getBestStockLocationToPick();
stockLocation.setEmpty(true);
NLX.Robot.Kuka.Controller.CartesianPosition posUp = new NLX.Robot.Kuka.Controller.CartesianPosition(stockLocation.getLocation());
posUp.Z = area.getP0().Z;
NLX.Robot.Kuka.Controller.CartesianPosition posDown = new NLX.Robot.Kuka.Controller.CartesianPosition(stockLocation.getLocation());
locations1.Add(posUp);
locations1.Add(posDown);
locations2.Add(posUp);
locations2.Add(area.getP0());
stockLocation.setEmpty(true);
}
Program.robot.clearTrajectory();
foreach (NLX.Robot.Kuka.Controller.CartesianPosition pos in locations1)
Program.robot.addCartesianPointToTrajectory(pos);
Program.robot.sendTrajectory();
}
public void readAndInterpretFile(string path, NLX.Robot.Kuka.Controller.RobotController robot)
{
Console.WriteLine("in readAndInterpretFile");
Console.WriteLine("path: " + path);
listPos = new List <NLX.Robot.Kuka.Controller.CartesianPosition>();
try
{
if (File.Exists(path))
{
using (StreamReader sr = new StreamReader(path))
{
double d;
new List <NLX.Robot.Kuka.Controller.CartesianPosition>();
while (sr.Peek() >= 0)
{
string str = sr.ReadLine();
if (Double.TryParse(str, out d))
{
//
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
Console.WriteLine("x: " + d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("y: " + d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("z: " + d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("a: " + d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("b: " + d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("c: " + d);
cpos.C = d;
str = sr.ReadLine();
Console.WriteLine(str);
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else
{
// play the list of positions if it's not empty
Console.WriteLine("rob: " + robot);
Console.WriteLine("listpos: " + listPos);
fctPlay(robot);
if (str == "open")
{
// do the open
Console.WriteLine("doing open gripper");
robot.OpenGripper();
}
else if (str == "close")
{
Thread.Sleep(1000);
// do the close
Console.WriteLine("doing close gripper");
robot.CloseGripper();
}
}
}
fctPlay(robot);
}
}
else
{
Console.WriteLine("it doesn't exist");
}
}
catch (Exception e)
{
Console.WriteLine("The process failed: {0}", e.ToString());
}
}
public void parseFile(string path)
{
point0 = new NLX.Robot.Kuka.Controller.CartesianPosition();
point1 = new NLX.Robot.Kuka.Controller.CartesianPosition();
point2 = new NLX.Robot.Kuka.Controller.CartesianPosition();
point3 = new NLX.Robot.Kuka.Controller.CartesianPosition();
try
{
if (File.Exists(path))
{
using (StreamReader sr = new StreamReader(path))
{
double d;
new List <NLX.Robot.Kuka.Controller.CartesianPosition>();
while (sr.Peek() >= 0)
{
string str = sr.ReadLine();
if (str == "p0")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.C = d;
str = sr.ReadLine();
}
else if (str == "p1")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.C = d;
str = sr.ReadLine();
}
else if (str == "p2")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.C = d;
str = sr.ReadLine();
}
else if (str == "p3")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.C = d;
str = sr.ReadLine();
}
}
}
}
else
{
Console.WriteLine("it doesn't exist");
}
}
catch (Exception e)
{
Console.WriteLine("The process failed: {0}", e.ToString());
}
}
public void parseFile(string path)
{
point0 = new NLX.Robot.Kuka.Controller.CartesianPosition();
point1 = new NLX.Robot.Kuka.Controller.CartesianPosition();
point2 = new NLX.Robot.Kuka.Controller.CartesianPosition();
point3 = new NLX.Robot.Kuka.Controller.CartesianPosition();
try
{
if (File.Exists(path))
{
using (StreamReader sr = new StreamReader(path))
{
double d;
new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
while (sr.Peek() >= 0)
{
string str = sr.ReadLine();
if (str == "p0")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point0.C = d;
str = sr.ReadLine();
}
else if (str == "p1")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point1.C = d;
str = sr.ReadLine();
}
else if (str == "p2")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point2.C = d;
str = sr.ReadLine();
}
else if (str == "p3")
{
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
point3.C = d;
str = sr.ReadLine();
}
}
}
}
else
{
Console.WriteLine("it doesn't exist");
}
}
catch (Exception e)
{
Console.WriteLine("The process failed: {0}", e.ToString());
}
}
public void readAndInterpretFile2(string path, NLX.Robot.Kuka.Controller.RobotController robot)
{
Console.WriteLine("in readAndInterpretFile2");
Console.WriteLine("path: " + path);
int nbLignesPalette = 4;
int nbColonnesPalette = 4;
parseFile(path);
double xdecx = point1.X - point2.X;
double ydecx = point1.Y - point2.Y;
double xdecy = point1.X - point3.X;
double ydecy = point1.Y - point3.Y;
Console.WriteLine("xdecx: " + xdecx);
Console.WriteLine("ydecx: " + ydecx);
Console.WriteLine("xdecy: " + xdecy);
Console.WriteLine("ydecy: " + ydecy);
try
{
//for (int ligne = 0; ligne <= nbLignesPalette; ligne++)
for (int colonne = 0; colonne <= nbColonnesPalette; colonne++)
{
for (int ligne = 0; ligne <= nbLignesPalette; ligne++)
//for (int colonne = 0; colonne <= nbColonnesPalette; colonne++)
{
Console.WriteLine("ligne: " + ligne + ",colonne: " + colonne);
if (File.Exists(path))
{
using (StreamReader sr = new StreamReader(path))
{
double d;
//new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
listPos = new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
while (sr.Peek() >= 0)
{
string str = sr.ReadLine();
if (Double.TryParse(str, out d))
{
//
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
Console.WriteLine("x: " + d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("y: " + d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("z: " + d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("a: " + d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("b: " + d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("c: " + d);
cpos.C = d;
str = sr.ReadLine();
Console.WriteLine(str);
Console.WriteLine("robot: " + robot);
Console.WriteLine("listPos: " + listPos);
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else
{
// play the list of positions if it's not empty
Console.WriteLine("rob: " + robot);
Console.WriteLine("listpos: " + listPos);
fctPlay(robot);
if (str == "open")
{
// do the open
Console.WriteLine("doing open gripper");
robot.OpenGripper();
}
else if (str == "close")
{
Thread.Sleep(1000);
// do the close
Console.WriteLine("doing close gripper");
robot.CloseGripper();
}
else if (str == "p0")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Console.WriteLine("str: " + str);
Double.TryParse(str, out d);
//cpos.X = d + ligne*xdecy + colonne*xdecy;
Console.WriteLine("ligne: " + ligne);
Console.WriteLine("colonne: " + colonne);
cpos.X = d + (-1)*ligne*xdecx + (-1)*colonne*xdecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
//cpos.Y = d + ligne*ydecx + colonne*ydecy;
cpos.Y = d + (-1)*ligne*ydecx + (-1)*colonne*ydecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else if (str == "p1")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Double.TryParse(str, out d);
//cpos.X = d + ligne * xdecx + colonne * xdecy;
cpos.X = d + (-1) * ligne * xdecx + (-1) * colonne * xdecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
//cpos.Y = d + ligne * ydecx + colonne * ydecy;
cpos.Y = d + (-1) * ligne * ydecx + (-1) * colonne * ydecy;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
listPos.Add(robot.GetCurrentPosition());
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else if (str == "p2")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
}
else if (str == "p3")
{
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
cpos.C = d;
str = sr.ReadLine();
}
}
}
fctPlay(robot);
}
}
else
{
Console.WriteLine("it doesn't exist");
}
}
}
Console.WriteLine("in readAndInterpretFile");
Console.WriteLine("path: " + path);
listPos = new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
}
catch (Exception e)
{
Console.WriteLine("The process failed: {0}", e.ToString());
}
}
public void readAndInterpretFile(string path, NLX.Robot.Kuka.Controller.RobotController robot)
{
Console.WriteLine("in readAndInterpretFile");
Console.WriteLine("path: " + path);
listPos = new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
try
{
if (File.Exists(path))
{
using (StreamReader sr = new StreamReader(path))
{
double d;
new List<NLX.Robot.Kuka.Controller.CartesianPosition>();
while (sr.Peek() >= 0)
{
string str = sr.ReadLine();
if (Double.TryParse(str, out d))
{
//
NLX.Robot.Kuka.Controller.CartesianPosition cpos = new NLX.Robot.Kuka.Controller.CartesianPosition();
Console.WriteLine("x: " + d);
cpos.X = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("y: " + d);
cpos.Y = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("z: " + d);
cpos.Z = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("a: " + d);
cpos.A = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("b: " + d);
cpos.B = d;
str = sr.ReadLine();
Double.TryParse(str, out d);
Console.WriteLine("c: " + d);
cpos.C = d;
str = sr.ReadLine();
Console.WriteLine(str);
listPos.Add( robot.GetCurrentPosition() );
// add cpos to the listPos
listPos.Add(cpos);
fctPlay(robot);
}
else
{
// play the list of positions if it's not empty
Console.WriteLine("rob: " + robot);
Console.WriteLine("listpos: " + listPos);
fctPlay(robot);
if (str == "open")
{
// do the open
Console.WriteLine("doing open gripper");
robot.OpenGripper();
}
else if (str == "close")
{
Thread.Sleep(1000);
// do the close
Console.WriteLine("doing close gripper");
robot.CloseGripper();
}
}
}
fctPlay(robot);
}
}
else
{
Console.WriteLine("it doesn't exist");
}
}
catch (Exception e)
{
Console.WriteLine("The process failed: {0}", e.ToString());
}
}
