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