﻿// Listing 10.8. Mechanizm tłumaczący PUMT — program będący implementacją rozszerzonego scenariusza pracy robota

    3    
    4    import java.io.DataInputStream;
    5    import java.io.DataOutputStream;
    6    import lejos.hardware.sensor.NXTUltrasonicSensor;
    7    import lejos.hardware.*;
    8    import lejos.hardware.ev3.LocalEV3;
    9    import lejos.hardware.port.SensorPort;
   10    import lejos.hardware.sensor.SensorModes;
   11    import lejos.hardware.port.Port;
   12    import lejos.hardware.lcd.LCD;
   13    import java.net.ServerSocket;
   14    import java.net.Socket;
   15    import lejos.hardware.sensor.HiTechnicColorSensor;
   16    import lejos.hardware.sensor.EV3UltrasonicSensor;
   17    import lejos.robotics.navigation.*;
   18    import lejos.robotics.navigation.DifferentialPilot;
   19    import lejos.robotics.localization.OdometryPoseProvider;
   20    import lejos.robotics.SampleProvider;
   21    import lejos.hardware.device.tetrix.*;
   22    import lejos.hardware.device.tetrix.TetrixRegulatedMotor;
   23    import lejos.robotics.navigation.Pose;
   24    import lejos.robotics.navigation.Navigator;
   25    import lejos.robotics.pathfinding.Path;
   26    import java.lang.Math.*;
   27    import java.io.PrintWriter;
   28    import java.io.File;
   29    import java.util.ArrayList;
   30    
//AKCJE: SEKCJA 2.
   31    class action{
   32       protected softbot Robot;
   33       public action()
   34       {
   35       }
   36       public action(softbot Bot)
   37       {
   38           Robot = Bot;
   39    
   40       }
   41       public void task() throws Exception
   42       {
   43       }
   44    }
   45    
   46    class scenario_action1  extends action
   47    {
   48        
   49       public scenario_action1(softbot Bot)
   50       {
   51           super(Bot);
   52       }
   53       public void task() throws Exception
   54       {
   55           Robot.moveToObject();                   
   56    
   57       }
   58    
   59    } 
   60     
   61     
   62    class scenario_action2 extends action
   63    {
   64    
   65       public  scenario_action2(softbot Bot)
   66       {
   67            
   68           super(Bot);
   69       }
   70         
   71       public  void task() throws Exception
   72       {
   73           Robot.scanObject();
   74         
   75       }
   76          
   77    
   78    }
   79            
//Scenariusz i sytuacja
   80            
   81    class x_location{
   82       public int X;
   83       public int Y;
   84       public x_location()
   85       {
   86          
   87           X = 0;
   88           Y = 0;
   89       } 
   90      
   91    }
   92    
   93    
   94    class something{
   95       x_location Location;
   96       int Color;
   97       public something()
   98       {
   99           Location = new x_location();
  100           Location.X = 0;
  101           Location.Y = 0;
  102           Color = 0;
  103       }
  104       public void setLocation(int X,int Y)
  105       {
  106      
  107           Location.X = X;
  108           Location.Y = Y;
  109      
  110       }
  111       public int getXLocation()
  112       {
  113           return(Location.X);
  114       }    
  115      
  116       public int getYLocation()
  117       {
  118           return(Location.Y);
  119      
  120       }
  121      
  122       public void setColor(int X)
  123       {
  124      
  125           Color = X;
  126       }
  127      
  128       public int getColor()
  129       {
  130           return(Color);
  131       }     
  132      
  133    }
  134    
  135    class room{
  136       protected int Length = 300;
  137       protected int Width = 200;
  138       protected int Area;
  139       public something SomeObject;
  140    
  141       public  room() 
  142       {
  143           SomeObject =  new something();
  144           SomeObject.setLocation(20,50);
  145           
  146       
  147       }
  148       public int  area()  
  149       {
  150           Area = Length * Width;
  151           return(Area); 
  152       }
  153    
  154       public  int length()
  155       {
  156          
  157           return(Length);
  158       }
  159    
  160       public int width()
  161       {
  162    
  163           return(Width);
  164       }
  165    }
  166    
  179    class situation{
  180    
  181       public room Area;
  182       int ActionNum = 0;
  183       public ArrayList<action>  Actions;
  184       action RobotAction;
  185       public situation(softbot  Bot)
  186       {
  187           RobotAction = new action();
  188           Actions = new ArrayList<action>();
  189           scenario_action1 Task1 = new scenario_action1(Bot);
  190           scenario_action2 Task2 = new scenario_action2(Bot);
  191           Actions.add(Task1);
  192           Actions.add(Task2);
  193           Area = new room();
  194    
  195       }
  196       public void nextAction() throws Exception
  197       {
  198              
  199           if(ActionNum < Actions.size()){
  200              RobotAction = Actions.get(ActionNum);
  201           }
  202           RobotAction.task(); 
  203           ActionNum++;
  204                 
  205            
  206       }
  207       public int numTasks()
  208       {
  209           return(Actions.size());
  210      
  211       }
  212    
  213    }
  214    
  215    public class softbot
  216    {
//CZĘŚCI: SEKCJA 1.
//Sekcja czujników
  217        public EV3UltrasonicSensor Vision;
  218        public HiTechnicColorSensor ColorVision;
  219        int CurrentColor;
  220        double  WheelDiameter;
  221        double TrackWidth;
  222        float  RobotLength;
  223        DifferentialPilot  D1R1Pilot;
  224        ArcMoveController  D1R1ArcPilot;
//Siłowniki
  225        TetrixControllerFactory  CF;
  226        TetrixMotorController MC;
  227        TetrixServoController SC;
  228        TetrixRegulatedMotor LeftMotor;
  229        TetrixRegulatedMotor RightMotor;
  230        TetrixServo  Arm;
  231        TetrixServo  Gripper;
  232        TetrixServo  SensorArray;
//Obsługa
  233        OdometryPoseProvider Odometer;
  234        Navigator D1R1Navigator;
  235        boolean PathReady = false;
  236        Pose CurrPos;
  237        int OneSecond = 1000;
  238        Sound  AudibleStatus;
  239        DataInputStream dis;
  240        DataOutputStream Dout;
  241        location  CurrentLocation;
  242        SampleProvider UltrasonicSample;
  243        SensorModes USensor;
  244        PrintWriter Log;
//Scenariusze i sytuacje: SEKCJA 4.
  245        situation Situation1;    
  246        x_location RobotLocation;
  247        ArrayList<String> Messages;  
  248        Exception SoftbotError;
  249          
  250        public softbot() throws InterruptedException,Exception
  251        {
  252     
  253            Messages = new ArrayList<String>();
  254            Vision = new EV3UltrasonicSensor(SensorPort.S3);
  255            if(Vision == null){
  256               Messages.add("Błąd inicjalizacji czujnika ultradźwiękowego");
  257               SoftbotError = new Exception("101");
  258               throw SoftbotError;
  259            }
  260            Vision.enable();
  261            Situation1 = new situation(this);
  262            RobotLocation = new x_location();
  263            RobotLocation.X = 0;
  264            RobotLocation.Y = 0;
  265                  
  266            ColorVision = new HiTechnicColorSensor(SensorPort.S2);
  267            if(ColorVision == null){
  268                Messages.add("Błąd inicjalizacji czujnika koloru");
  269                SoftbotError = new Exception("100");
  270                throw SoftbotError;
  271            }
  272            Log = new PrintWriter("softbot.log");
  273            Log.println("Skonstruowano czujniki");
  274            Thread.sleep(1000);
  275            WheelDiameter = 7.50f;
  276            TrackWidth = 32.5f;
  277            
  278            Port APort = LocalEV3.get().getPort("S1");          
  279            CF = new TetrixControllerFactory(SensorPort.S1);
  280            if(CF == null){
  281               Messages.add("Błąd konfiguracji portu serwomotoru");
  282               SoftbotError = new Exception("102");
  283               throw SoftbotError;
  284            }
  285            Log.println("Skonstruowano mikrokontroler Tetrix");
  286              
  287            MC = CF.newMotorController();
  288            SC = CF.newServoController();
  289              
  290            LeftMotor = MC.getRegulatedMotor(TetrixMotorController.MOTOR_1);
  291            RightMotor = MC.getRegulatedMotor(TetrixMotorController.MOTOR_2);
  292            if(LeftMotor == null || RightMotor == null){
  293               Messages.add("Błąd inicjalizacji silników");
  294               SoftbotError = new Exception("103");
  295               throw SoftbotError;
  296            }
  297            LeftMotor.setReverse(true);
  298            RightMotor.setReverse(false);
  299            LeftMotor.resetTachoCount();
  300            RightMotor.resetTachoCount();
  301            Log.println("Skonstruowano silniki");          
  302            Thread.sleep(2000);
  303              
  304              
  317            SensorArray = SC.getServo(TetrixServoController.SERVO_3);
  318            if(SensorArray == null){
  319               Messages.add("Błąd inicjalizacji tablicy SensorArray");
  320               SoftbotError = new Exception("107");
  321               throw SoftbotError;
  322            }     
  323            Messages.add("Skonstruowano serwomotory");            
  324            Log.println("Skonstruowano serwomotory");
  325            Thread.sleep(1000);
  326              
  327            SC.setStepTime(7);
  328            Arm.setRange(750,2250,180);
  329            Arm.setAngle(100);
  330            Thread.sleep(1000);
  331             
  335              
  336              
  337            SensorArray.setRange(750,2250,180);
  338            SensorArray.setAngle(20);
  339            Thread.sleep(1000);
  340            D1R1Pilot = new DifferentialPilot(WheelDiameter,TrackWidth,LeftMotor,RightMotor);
  341            D1R1Pilot.reset();
  342            D1R1Pilot.setTravelSpeed(10);
  343            D1R1Pilot.setRotateSpeed(20);
  344            D1R1Pilot.setMinRadius(0);
  345              
  346            Log.println("Skonstruowano pilota");
  347            Thread.sleep(1000);
  348            CurrPos = new Pose();
  349            CurrPos.setLocation(0,0);
  350            Odometer = new OdometryPoseProvider(D1R1Pilot);
  351            Odometer.setPose(CurrPos);
  352
  353              
  354            D1R1Navigator = new Navigator(D1R1Pilot);
  355            D1R1Navigator.singleStep(true);
  356            Log.println("Skonstruowano hodometr");
  357                  
  358            Log.println("Szerokość pomieszczenia: " + Situation1.Area.width());
  359            room SomeRoom = Situation1.Area;
  360            Log.println("Współrzędne: " + SomeRoom.SomeObject.getXLocation() + "," + SomeRoom.SomeObject.getYLocation());
  361            Messages.add("Skonstruowano elementy ramy softbot");
  362            Thread.sleep(1000);
  363    
  364            
  365        
  366        }
  367    
  434        
//ZADANIA: SEKCJA 3.
  435        public void doNextTask() throws Exception
  436        {
  437            Situation1.nextAction();
  438            
  439        }
  440            
  441        public void moveToObject() throws Exception
  442        {
  443            RobotLocation.X = (Situation1.Area.SomeObject.getXLocation() - RobotLocation.X);
  444            travel(RobotLocation.X);
  445            waitUntilStop(RobotLocation.X);
  446            rotate(90);
  447            waitForRotate(90);
  448            RobotLocation.Y = (Situation1.Area.SomeObject.getYLocation() - RobotLocation.Y);            
  449            travel(RobotLocation.Y);
  450            waitUntilStop(RobotLocation.Y);
  451            Messages.add("moveToObject");
  452                 
  453        } 
  454            
  455        public void scanObject()throws Exception
  456        {
  457                  
  458            float Distance = 0;
  459            resetArm();
  460            moveSensorArray(110);
  461            Thread.sleep(2000);
  462            Distance = readUltrasonicSensor();
  463            Thread.sleep(4000);
  464            if(Distance <= 10.0){
  465               getColor();
  466               Thread.sleep(3000);
  467            }
  468            moveSensorArray(50);
  469            Thread.sleep(2000);
  470            
  471        }
  472            
  473        public int numTasks()
  474        {
  475            return(Situation1.numTasks());
  476        }
  477        
  512    
  513        public float readUltrasonicSensor()
  514        {
  515            UltrasonicSample =  Vision.getDistanceMode();
  516            float X[] = new float[UltrasonicSample.sampleSize()];
  517            Log.print("rozmiar próbki ");
  518            Log.println(UltrasonicSample.sampleSize());
  519                 
  520            UltrasonicSample.fetchSample(X,0);
  521            int Line = 3;
  522            for(int N = 0; N < UltrasonicSample.sampleSize();N++)
  523            {
  524                    
  525                Float Temp = new Float(X[N]); 
  526                Log.println(Temp.intValue());
  527                Messages.add(Temp.toString());
  528                Line++;
  529    
  530            }
  531            if(UltrasonicSample.sampleSize() >= 1){
  532               return(X[0]);
  533            }
  534            else{
  535                    return(-1.0f);
  536            }
  537    
  538        }
  539        public int getColor()
  540        {
  541        
  542            return(ColorVision.getColorID());
  543        }
  544            
  545    
  546        public void identifyColor() throws Exception
  547        {
  548            LCD.clear();
  549            LCD.drawString("Określono kolor",0,3);
  550            LCD.drawInt(getColor(),0,4);           
  551            Log.println("Określono kolor");
  552            Log.println("kolor = " + getColor());          
  553        }
  554        public void rotate(int Degrees)
  555        {
  556            D1R1Pilot.rotate(Degrees);
  557        }
  558        public void forward()
  559        {
  560        
  561            D1R1Pilot.forward();
  562        }
  563        public void backward()
  564        {
  565            D1R1Pilot.backward();
  566        }      
  567        
  568        public void travel(int Centimeters)
  569        {
  570            D1R1Pilot.travel(Centimeters);
  571        
  572        }    
  573        
  601        public void moveSensorArray(float X) throws Exception
  602        {
  603           
  604            SensorArray.setAngle(X);
  605            while(SC.isMoving())
  606            {
  607                  Thread.sleep(1500);
  608            }
  609        
  610        
  611        }
  612        
  641    
  642        public boolean waitForStop()
  643        {
  644            return(D1R1Navigator.waitForStop());
  645               
  646        }
  647        
  648           
  703           
  704          
  705           
  706        public void waitUntilStop(int Distance) throws Exception
  707        {
  708              
  709            Distance = Math.abs(Distance);
  710            Double  TravelUnit = new Double (Distance/D1R1Pilot.getTravelSpeed());
  711            Thread.sleep(Math.round(TravelUnit.doubleValue()) * OneSecond);
  712            D1R1Pilot.stop();
  713            Log.println("Prędkość ruchu " + D1R1Pilot.getTravelSpeed());
  714            Log.println("Dystans:  " + Distance);
  715            Log.println("Jednostka: " + TravelUnit);
  716            Log.println("Czekaj przez: " + Math.round(TravelUnit.doubleValue()) * OneSecond);
  717              
  718            } 
  719            public void waitUntilStop()
  720            {
  721                do{
  722                
  723                }while(D1R1Pilot.isMoving());
  724                D1R1Pilot.stop();
  725           
  726            }
  727           
  728            public void waitForRotate(double Degrees) throws Exception
  729            {
  730          
  731                Degrees = Math.abs(Degrees);
  732                Double DegreeUnit = new Double
                                         (Degrees/D1R1Pilot.getRotateSpeed());
  733                Thread.sleep(Math.round(DegreeUnit.doubleValue()) * OneSecond);
  734                D1R1Pilot.stop();
  735                Log.println("Jednostka obrotu: " + DegreeUnit);
  736                Log.println("Czekaj przez: " + Math.round(DegreeUnit.doubleValue()) * OneSecond);
  737           
  738    
  739          
  740            } 
  741           
  742       
  743           
  744            public  void closeLog()
  745            {
  746                Log.close();
  747            }
  748    
  749    
  750            public void report() throws Exception
  751            {
  752                ServerSocket Client = new ServerSocket(1111);
  753                Socket SomeSocket = Client.accept();
  754                DataOutputStream Dout = new DataOutputStream(SomeSocket.getOutputStream());
  755                Dout.writeInt(Messages.size());
  756                for(int N = 0;N < Messages.size();N++)
  757                {
  758                    Dout.writeUTF(Messages.get(N));
  759                    Dout.flush();
  760                    Thread.sleep(1000);
  761             
  762                }
  763                Thread.sleep(1000);
  764                Dout.close();
  765                Client.close();
  766            }   
  767    
  768    
  769            public void report(int X) throws Exception
  770            {
  771                 
  772                ServerSocket Client = new ServerSocket(1111);
  773                Socket SomeSocket = Client.accept();
  774                DataOutputStream Dout = new DataOutputStream(SomeSocket.getOutputStream());
  775                Dout.writeInt(X);
  776                Thread.sleep(5000);
  777                Dout.close();
  778                Client.close();
  779    
  780    
  781            }
  782            public void addMessage(String X)
  783            {
  784                Messages.add(X);
  785            }
  786    
  812            public static void main(String [] args)  throws Exception 
  813            {
  814                 
  815               
  816                softbot Unit1; 
  817                float Distance = 0;
  818                int TaskNum = 0;
  819                     
  820                try{
  821                        Unit1 = new softbot();
  822                        TaskNum = Unit1.numTasks();
  823                        for(int N = 0; N < TaskNum; N++)
  824                        {
  825                            Unit1.doNextTask();
  826                           
  827                        }
  828                        Unit1.report();
  829                        Unit1.closeLog();
  830                         
  831                }
  832                catch(Exception E)
  833                {
  834                      Integer Error;
  835                      System.out.println("Wystąpił błąd: " + E);
  836                      Error = new Integer(0);
  837                      int RetCode = Error.intValue();
  838                      if(RetCode == 0){
  839                         RetCode = 999;
  840                      }
  841                      ServerSocket Client = new ServerSocket(1111);
  842                      Socket SomeSocket = Client.accept();
  843                      DataOutputStream Dout = new DataOutputStream(SomeSocket.getOutputStream());
  844                      Dout.writeInt(RetCode);
  845                      Dout.close();
  846                      Client.close();
  847                        
  848                        
  849                }
  850                   
  851                
  852                      
  853        }
  854        
  855        
  856        
  857        
  858    }
