nxtOSEK/lejos_nxj/src/java/classes/lejos/navigation/CompassNavigator.java

   1 package lejos.navigation;
   2 import lejos.nxt.*;
   3
   4 /**
   5 * The CompassNavigatort class, like its superclass, can keep track of the robot position and the direction angle it faces. It uses a CompassPilot object to control NXT robot movements.<br>
   6 * The position and direction angle values are updated automatically when the movement command returns after the movement is complete and and after stop() command is issued.
   7 * However, some commands optionally return immediately, to permit sensor monitoring in the main thread.  It is then the programmers responsibility to
   8 * call updateXY() when the robot motion is completed.  All angles are in degrees, distances in the units used to specify robot dimensions.
   9 * As with pilot, the robot must be have two independently controlled drive wheels.
  10 * Uses the Compass Sensor to determine the robot heading.
  11 */
  12
  13 public class CompassNavigator extends TachoNavigator
  14 {
  15
  16         private CompassPilot compassPilot; //
  17
  18         /**
  19         * Allocates a CompassNavigator objects and its  CompassPilot object and initializes it with the proper motors and dimensions.
  20         * This is a subclass of TachoNavigator (see that API for other methods).
  21         * The x and y values and the direction angle are all initialized to 0, so if the first move is forward() the robot will run along
  22         * the x axis. <BR>
  23         * @param compassPort  the sensor port connected to the compass sensor   e.g SensorPort.S1
  24         * @param wheelDiameter The diameter of the wheel, usually printed right on the
  25         * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm = 1.95 in)
  26         * @param trackWidth The distance from the center of the left tire to the center
  27         * of the right tire, in units of your choice
  28         * @param rightMotor The motor used to drive the right wheel e.g. Motor.C.
  29         * @param leftMotor The motor used to drive the left wheel e.g. Motor.A.
  30         */
  31
  32         public CompassNavigator(SensorPort compassPort, float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor)
  33         {
  34                 this(compassPort, wheelDiameter,trackWidth,leftMotor, rightMotor, false);
  35         }
  36
  37         public CompassNavigator(SensorPort compassPort, float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor, boolean reverse)
  38         {
  39                 this(new CompassSensor(compassPort), wheelDiameter,trackWidth,leftMotor, rightMotor,reverse);
  40         }
  41
  42         public CompassNavigator(CompassSensor compass, float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor, boolean reverse)
  43         {
  44                 super(new CompassPilot(compass, wheelDiameter,trackWidth,leftMotor, rightMotor,reverse));
  45                 this.compassPilot = (CompassPilot) _pilot;
  46                 _heading = getAngle();
  47         }
  48
  49 /**
  50  * To use this constructor, you must first create a compass pilot.
  51  * @param compassPilot
  52  */
  53         public CompassNavigator(CompassPilot compassPilot)
  54         {
  55                 super(compassPilot);
  56                 compassPilot = (CompassPilot) _pilot;
  57                 _heading = getAngle();
  58         }
  59
  60     /**
  61      * returns the pilot of this navigator
  62      * @return compass pilot
  63      */
  64     CompassPilot getCompassPilot(){return compassPilot;}
  65
  66 /**
  67  * Robot rotates 360 degrees while calibrating the compass sensor
  68  */
  69         public void calibrateCompass() {compassPilot.calibrate();}
  70
  71  /**
  72     * Rotates the NXT robot to point in a specific direction. It will take the shortest
  73     * path necessary to point to the desired angle.
  74     * If immediateReturnis true, method returns immidiately and your code MUST call updatePostion()
  75     * when the robot has stopped.  Otherwise, the robot position is lost.
  76     * @param angle The angle to rotate to, in degrees.
  77     * @param immediateReturn iff true,  method returns immediately and the programmer is responsible for calling
  78     * updatePosition() before the robot moves again.
  79     */
  80         public void rotateTo(float angle, boolean immediateReturn)
  81         {
  82                 compassPilot.rotateTo((int)angle,false);  //???
  83                 if(immediateReturn)return;
  84                 while(compassPilot.isRotating())Thread.yield();
  85                 updateHeading();
  86         }
  87
  88 /**
  89  * Rotates the NXT robot by a specified angle.
  90  * If immediateReturnis true, method returns immidiately and your code MUST call updatePostion()
  91  * when the robot has stopped.  Otherwise, the robot position is lost.
  92  * @param angle The angle to rotate to, in degrees.
  93  * @param immediateReturn iff true,  method returns immediately and the programmer is responsible for calling
  94  * updatePosition() before the robot moves again.
  95  */
  96         public void rotate(float angle, boolean immediateReturn)
  97         {
  98                 compassPilot.rotate((int)angle,immediateReturn);
  99                 updateHeading();
 100         }
 101
 102         /**
 103          * Moves the NXT robot a specific distance. A positive value moves it forwards and
 104          * a negative value moves it backwards.
 105          *  If immediateReturnis true, method returns immidiately and your code MUST call updatePostion()
 106          * when the robot has stopped.  Otherwise, the robot position is lost.
 107          * @param distance The positive or negative distance to move the robot, same units as _wheelDiameter
 108          * @param immediateReturn iff true, the method returns immediately, in which case the programmer <br>
 109          *  is responsible for calling updatePosition() before the robot moves again.
 110          */
 111         public void travel(float distance,boolean immediateReturn)
 112         {
 113                 compassPilot.resetTachoCount();
 114                 compassPilot.travel(distance,immediateReturn);
 115                 if(immediateReturn)return;
 116                 while(compassPilot.isTraveling())Thread.yield();
 117                 updateXY();
 118         }
 119
 120         /**
 121          * Halts the NXT robot and calculates new x, y coordinates.
 122          */
 123         public void stop()
 124         {
 125                 compassPilot.stop();
 126                 updateXY();
 127         }
 128
 129 /**
 130  * Direction of robot facing is set equal to the current compass reading
 131  */
 132         public void updateHeading()
 133         {
 134                 _heading = (int)compassPilot.compass.getDegreesCartesian();
 135         }
 136
 137  /**
 138  * Updates x,y coordinates; assumes last compass angle was constant during travel
 139  */
 140         public void updateXY()
 141         {
 142                 updateHeading();
 143                 float angle = (float)Math.toRadians(_heading);
 144                 float dx = compassPilot.getTravelDistance() *(float) Math.cos(angle);
 145                 float dy = compassPilot.getTravelDistance() * (float)Math.sin(angle);
 146                 setPosition(dx + getX(),dy+getY(),_heading);
 147         }
 148 }
 149