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());
            }
        }
예제 #2
0
        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);
        }
        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());
            }
        }