Codigo: wumpus proyecto final

CODIGO:

import lejos.nxt.*;

import lejos.robotics.subsumption.*;

import lejos.robotics.navigation.*;

/**

 * Demonstration of use of the Behavior and Pilot classes to

 * implement a simple line following robot.

 *

 * Requires a wheeled vehicle with two independently controlled

 * wheels with motors connected to motor ports A and C, and a light

 * sensor mounted forwards and pointing down, connected to sensor port 1.

 *

 *

 */

public class otro {

               

                public static void main (String[] aArg)

                throws Exception

                {

               

                               final Pilot pilot = new TachoPilot(5.6f,16.0f,Motor.A, Motor.C, false);

                               final LightSensor light = new LightSensor(SensorPort.S1);

                               final LightSensor light2 = new LightSensor(SensorPort.S3);

                               final UltrasonicSensor ul = new UltrasonicSensor(SensorPort.S2);

        pilot.setTurnSpeed(180);

        /**

         * this behavior wants to take control when the light sensor sees the line

       

                               Behavior DriveForward = new Behavior()

                               {

                                               public boolean takeControl() {return light.readValue() <= 40 ;

                                               }

                              

                                               public void suppress() {

                                                               //pilot.stop();

                                               } //suppress

                                              

                                               public void action() {

                                                              

                                                              

                                                              

                                                               pilot.forward();

                                                              

                                                               Motor.A.setSpeed(1200);

                Motor.C.setSpeed(1200);

               

                               

                while(light.readValue()  <=40) Thread.yield(); //action complete when not on line

                                                               }//action

                                                                                                             

                               }; //drive forward

                              

                               Behavior OffLine = new Behavior()

                               {

                                               private boolean suppress = false;

                                              

                                               public boolean takeControl() {return light.readValue() > 40;

                               }//take control

                                               public void suppress() {

                                                               suppress = true;

                                               }//suppress

                                              

                                               public void action() {

                                                               int sweep = 9; //9

                                                               while (!suppress) {

                                                                             

                                                                              pilot.rotate(sweep,true);

                                                                              while (!suppress && pilot.isMoving()) Thread.yield();

                                                                              sweep *= -2; //-2

                                                               } //while

                                                               pilot.stop();

                                                               suppress = false;

                                               } //action

                               }; //behaviour

                                              

                              

                               Behavior[] bArray = {OffLine, DriveForward};

        LCD.drawString("WUMPUSSSSSS GRRRRRRRR...!!! ", 0, 1);

        Button.waitForPress();

       

        int c;

        for(;;)

        {

            c =ul.getDistance();

            LCD.drawString("La distancia es" + c, 0,0);

       

            if(c<100)

            {

                    (new Arbitrator(bArray)).start();

                   

            } //end if

            else

            {

                                               Motor.B.rotateTo(150);

                                               Motor.B.rotateTo(-150);

            } //end else

               

        }// end for

               

}//en main

                } // end class