// Listing 9.1. Mechanizm tłumaczący PUMT — procedura inicjalizacji

/*Rama projektowa Softbot
Nazwa:  Unit1

Początek procedury inicjalizacji:
Krok 1.: Inicjalizuj czujnik ultradźwiękowy.
Krok 2.: Inicjalizuj czujnik koloru.
Krok 3.: Skonfiguruj silnik lewy i silnik prawy.
Krok 4.: Inicjalizuj serwomotory ramienia i ustaw ramię w początkowej pozycji (pod kątem określanym przez wartość 100).
Krok 5.: Zdefiniuj współrzędne początkowej pozycji robota jako 0,0.
Krok 6.: Za średnicę koła przyjmij wartość 7, a za szerokość gąsienicy przyjmij wartość 32

        ...
Jeżeli procedura rozruchowa robota zostanie poprawnie zakończona, to kontynuuj wykonywanie programu, w przeciwnym wypadku zwróć informacje o błędach i wyłącz się
Koniec procedury inicjalizacji
Koniec ramy projektowej*/

//Początek konstruktora

       public unit1() throws InterruptedException,Exception
       {
    1      public unit1() throws InterruptedException,Exception
    2      {
    3	 
    4	     try{ 
    5	          Exception SoftbotError;
    6	          //Konfiguruj czujnik ultradźwiękowy                    
    7	          Vision = new UltrasonicSensor(SensorPort.S4); 
    8	          if(Vision == null){
    9	             Messages.add("Błąd inicjalizacji czujnika ultradźwiękowego na porcie 4.");
   10	             SoftbotError = new Exception("101");
   11	             throw SoftbotError;
   12	          }
   13	          Vision.enable();
   14	          //Konfiguruj czujnik koloru
   15	          ColorVision = new HiTechnicColorSensor(SensorPort.S2);
   16	          if(ColorVision == null){
   17	              Messages.add("Błąd inicjalizacji czujnika koloru na porcie 2.");
   18	              SoftbotError = new Exception("100");
   19	              throw SoftbotError;
   20	          }
   21	          // Konfiguruj silniki
   22	          CF = new TetrixControllerFactory(SensorPort.S1);
   23	          if(CF == null){
   24	             Messages.add("Błąd konfiguracji portu serwomotoru");
   25	             SoftbotError = new Exception("102");
   26	             throw SoftbotError;
   27	          }	     
   28	             
   29		  LeftMotor = MC.getRegulatedMotor(TetrixMotorController.MOTOR_1);
   30		  RightMotor = MC.getRegulatedMotor(TetrixMotorController.MOTOR_2);
   31	          if(LeftMotor == null || RightMotor == null){
   32	             Messages.add("Błąd inicjalizacji silników");
   33	             SoftbotError = new Exception("103");
   34	             throw SoftbotError;
   35	          }
   36	
   37	          LeftMotor.setReverse(true);
   38		  RightMotor.setReverse(false);
   39		  LeftMotor.resetTachoCount();
   40		  RightMotor.resetTachoCount();
   41	             
   42	          //Konfiguruj serwomotory ramienia
   43	          Messages.add("Skonstruowano współczynnik mikrokontrolera Tetrix");	      
   44		  MC = CF.newMotorController();
   45	          SC = CF.newServoController();                     
   46	          Gripper = SC.getServo(TetrixServoController.SERVO_2);
   47	
   48	
   49	          Arm = SC.getServo(TetrixServoController.SERVO_1);	
   50	          if(Arm == null){
   51	             Messages.add("Błąd inicjalizacji ramienia");
   52	             SoftbotError = new Exception("104");
   53	             throw SoftbotError;
   54	          }
   55	          Arm.setRange(750,2250,180);
   56	          Arm.setAngle(100);
   57	          // Określ początkowe położenie robota
   58	          RobotLocation = new location();
   59	          RobotLocation.X = 0;
   60	          RobotLocation.Y = 0;     
   61	
   62	               
   63	          //Określ średnicę koła i szerokość gąsienicy
   64	          WheelDiameter = 7.0f;
   65	          TrackWidth = 32.0f;
   66	                  
   67	          Situation1 = new situation();  // Tworzy nową sytuację
   68	               
   69		
   70      }	
   71		
   72   } 

