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

   1 package lejos.navigation;
   2 //import lejos.navigation.*;
   3 import lejos.nxt.*;
   4 import lejos.navigation.*;
   5
   6
   7 /**
   8  * The TachoNavigator class can keep track of the robot position and the direction angle it faces; It uses a _pilot object to control NXT robot movements.<br>
   9  * 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.
  10  * However, some commands optionally return immediately, to permit sensor monitoring in the main thread.  It is then the programmers responsibility to
  11  * call updatePosition() when the robot motion is completed.  All angles are in degrees, distances in the units used to specify robot dimensions.
  12  * As with pilot, the robot must be have two independently controlled drive wheels.
  13  * The assumed initial position of the robot is at (0,0) and initial angle 0 i.e. pointing in the +X direction.
  14  */
  15
  16 public class TachoNavigator implements Navigator
  17 {
  18    // orientation and co-ordinate data
  19    protected float _heading = 0;
  20    private float _x = 0;
  21    private float _y = 0;
  22    protected int _left0 = 0;
  23    protected int _right0 = 0;
  24
  25    // The essential component
  26    protected Pilot _pilot;
  27
  28    /**
  29     * set false whenever the robot moves,  set to true by updatePosition();
  30     */
  31    private boolean _updated = false;
  32
  33    /**
  34     * Allocates a Navigator object and initializes it with the proper motors.
  35     * 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
  36     * the x axis. <BR>
  37     * @param wheelDiameter The diameter of the wheel, usually printed right on the
  38     * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm = 1.95 in)
  39     * @param trackWidth The distance from the center of the left tire to the center
  40     * of the right tire, in units of your choice
  41     * @param rightMotor The motor used to drive the right wheel e.g. Motor.C.
  42     * @param leftMotor The motor used to drive the left wheel e.g. Motor.A.
  43     * @param reverse  If motor.forward() dives the robot backwars, set this parameter true.
  44     */
  45    public TachoNavigator(float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor, boolean reverse)
  46    {
  47       _pilot = new Pilot(wheelDiameter,trackWidth,leftMotor, rightMotor,reverse);
  48    }
  49
  50    public TachoNavigator(float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor)
  51    {
  52       _pilot = new Pilot(wheelDiameter,trackWidth,leftMotor, rightMotor);
  53    }
  54
  55
  56    public TachoNavigator(Pilot pilot) {
  57       _pilot = pilot;
  58    }
  59
  60    /**
  61     * Overloaded TachoNavigator constructor that assumes the following:<BR>
  62     * Left motor = Motor.A   Right motor = Motor.C <BR>
  63     * @param wheelDiameter The diameter of the wheel, usually printed right on the
  64     * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm)
  65     * @param driveLength The distance from the center of the left tire to the center
  66     * of the right tire,
  67     */
  68    public TachoNavigator(float wheelDiameter, float driveLength)
  69    {
  70       this(wheelDiameter, driveLength, Motor.A, Motor.C);
  71    }
  72
  73    public Pilot getPilot(){ return _pilot;}
  74    /**
  75     * Returns the current x coordinate of the NXT.
  76     * @return float Present x coordinate.
  77     */
  78    public float getX()
  79    {
  80       return _x;
  81    }
  82
  83    /**
  84     * Returns the current y coordinate of the NXT.
  85     * Note: At present it will only give an updated reading when the NXT is stopped.
  86     * @return float Present y coordinate.
  87     */
  88    public float getY() { return _y;}
  89
  90    /**
  91     * Returns the current angle the NXT robot is facing, relative to the +X axis direction; the +Y direction is 90 degrees.
  92     * Note: At present it will only give an updated reading when the NXT is stopped.
  93     * @return float Angle in degrees.
  94     */
  95    public float getAngle() { return _heading;}
  96
  97    /**
  98     *sets robot location (x,y) and direction angle
  99     *@param x  the x coordinate of the robot
 100     *@param y the y coordinate of the robot
 101     *@param directionAngle  the angle the robot is heading, measured from the x axis.  90 degrees is the +Y direction
 102     */
 103    public void setPosition(float x, float y, float directionAngle)
 104    {
 105       _x = x;
 106       _y = y;
 107       _heading = directionAngle;
 108    }
 109    /**
 110     *sets the motor speed of the robot, in degrees/second.
 111     */
 112    public void setSpeed(int speed)
 113    {
 114       _pilot.setSpeed(speed);
 115    }
 116
 117    /**
 118     * Moves the NXT robot forward until stop() is called.
 119     * @see Navigator#stop().
 120     */
 121    public void forward()
 122    {
 123       _updated = false;
 124       _pilot.resetTachoCount();
 125       _right0 = 0;
 126       _left0 = 0;
 127       _pilot.forward();
 128    }
 129
 130    /**
 131     * Moves the NXT robot backward until stop() is called.
 132     */
 133    public void backward()
 134    {
 135       _updated = false;
 136       _pilot.resetTachoCount();
 137       _right0 = 0;
 138       _left0 = 0;
 139       _pilot.backward();
 140    }
 141
 142    /**
 143     * Halts the NXT robot and calculates new x, y coordinates.
 144     */
 145    public void stop()
 146    {
 147       _pilot.stop();
 148       updatePosition();
 149    }
 150
 151    /**
 152     *returns true iff the robot is moving under power
 153     */
 154    public boolean isMoving()
 155    {
 156       return _pilot.isMoving();
 157    }
 158
 159    /**
 160     * Moves the NXT robot a specific distance. A positive value moves it forwards and
 161     * a negative value moves it backwards.
 162     * The robot position is updated atomatically when the method returns.
 163     * @param distance The positive or negative distance to move the robot, same units as _wheelDiameter
 164     */
 165    public void travel(float distance)
 166    {
 167       travel(distance,false);
 168    }
 169
 170    /**
 171     * Moves the NXT robot a specific distance. A positive value moves it forwards and
 172     * a negative value moves it backwards.
 173     *  If immediateReturnis true, method returns immidiately and your code MUST call updatePostion()
 174     * when the robot has stopped.  Otherwise, the robot position is lost.
 175     * @param distance The positive or negative distance to move the robot, same units as _wheelDiameter
 176     * @param immediateReturn iff true, the method returns immediately, in which case the programmer <br>
 177     *  is responsible for calling updatePosition() before the robot moves again.
 178     */
 179    public void travel(float distance,boolean immediateReturn)
 180    {
 181       _updated = false;
 182       _pilot.resetTachoCount();
 183       _right0 = 0;
 184       _left0 = 0;
 185       _pilot.travel(distance,immediateReturn);
 186       if(!immediateReturn) updatePosition();
 187    }
 188
 189    /**
 190     *Rotates the NXT to the left (increasing angle) until stop() is called;
 191     */
 192    public void rotateLeft()
 193    {
 194       _updated = false;
 195       _pilot.resetTachoCount();
 196       _right0 = 0;
 197       _left0 = 0;
 198       _pilot.steer(200);
 199    }
 200
 201    /**
 202     *Rotates the NXT to the right (decreasing angle) until stop() is called;
 203     */
 204    public void rotateRight()
 205    {
 206       _updated = false;
 207       _pilot.resetTachoCount();
 208       _right0 = 0;
 209       _left0 = 0;
 210       _pilot.steer(-200);
 211    }
 212
 213    /**
 214     * Rotates the NXT robot a specific number of degrees in a direction (+ or -).
 215     * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right.
 216     **/
 217    public void rotate(float angle)
 218    {
 219       rotate(angle,false);
 220    }
 221
 222    /**
 223     * Rotates the NXT robot a specific number of degrees in a direction (+ or -).
 224     *  If immediateReturn is true, method returns immidiately and your code MUST call updatePostion()
 225     * when the robot has stopped.  Otherwise, the robot position is lost.
 226     * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right.
 227     * @param immediateReturn iff true, the method returns immediately, in which case the programmer <br>
 228     *  is responsible for calling updatePosition() before the robot moves again.
 229     */
 230    public void rotate(float angle,boolean immediateReturn)
 231    {
 232       _updated = false;
 233       int turnAngle = Math.round(normalize(angle));
 234       _pilot.resetTachoCount();
 235       _right0 = 0;
 236       _left0 = 0;
 237       _pilot.rotate(turnAngle,immediateReturn);
 238       if(!immediateReturn) updatePosition();
 239    }
 240
 241    /**
 242     * Rotates the NXT robot to point in a specific direction. It will take the shortest
 243     * path necessary to point to the desired angle.
 244     * @param angle The angle to rotate to, in degrees.
 245     */
 246    public void rotateTo(float angle)
 247    {
 248       rotateTo(angle,false);
 249    }
 250
 251    /**
 252     * Rotates the NXT robot to point in a specific direction. It will take the shortest
 253     * path necessary to point to the desired angle.
 254     * If immediateReturnis true, method returns immidiately and your code MUST call updatePostion()
 255     * when the robot has stopped.  Otherwise, the robot position is lost.
 256     * @param angle The angle to rotate to, in degrees.
 257     * @param immediateReturn iff true,  method returns immediately and the programmer is responsible for calling
 258     * updatePosition() before the robot moves again.
 259     */
 260    public void rotateTo(float angle,boolean immediateReturn)
 261    {
 262       float turnAngle = normalize( angle - _heading);
 263       rotate(turnAngle,immediateReturn);
 264    }
 265
 266    /**
 267     * Rotates the NXT robot towards the target point (x,y)  and moves the required distance.
 268     * Method returns when target point is reached, and the robot position is updated;
 269     * @param x The x coordinate to move to.
 270     * @param y The y coordinate to move to.
 271     */
 272    public void goTo(float x, float y)
 273    {
 274       rotateTo(angleTo(x,y));
 275       travel(distanceTo(x,y));
 276    }
 277
 278    /**
 279     * Rotates the NXT robot towards the target point (x,y)  and moves the required distance.
 280     * Method returns when target point is reached, and the robot position is updated;
 281     * @param x The x coordinate to move to.
 282     * @param y The y coordinate to move to.
 283     * @param immediateReturn iff true,  method returns immediately
 284     */
 285    public void goTo(float x, float y, boolean immediateReturn)
 286    {
 287       rotateTo(angleTo(x,y));
 288       travel(distanceTo(x,y), immediateReturn);
 289    }
 290
 291    /**
 292     * distance from robot to the point with coordinates (x,y) .
 293     * @param x coordinate of the point
 294     * @param y coordinate of the point
 295     * @return the distance from the robot current location to the point
 296     */
 297    public float distanceTo( float x, float y)
 298    {
 299       float dx = x -_x;
 300       float dy = y -_y;
 301       //use hypotenuse formula
 302       return (float)Math.sqrt(dx*dx+dy*dy);
 303    }
 304
 305    /**
 306     * returns the direction angle (degrees) to point with coordinates (x,y)
 307     * @param x coordinate of the point
 308     * @param y coordinate of the point
 309     * @return the direction angle to the point (x,y) from the NXT.  Rotate to this angle to head toward it.
 310     */
 311    public float angleTo(float x, float y)
 312    {
 313       float dx = x -_x;
 314       float dy = y -_y;
 315       return(float)Math.toDegrees(Math.atan2(dy,dx));
 316    }
 317
 318    /**
 319     * Updates robot location (x,y) and direction angle. Called by stop, and movement commands that terminate when complete.
 320     * If you use a movement command that returns immediately, you MUST call this method when the movement is complete.
 321     * It may also be called while movement is on progress.
 322     */
 323    public void updatePosition()
 324    {
 325       int temp = 0;
 326       int left = _pilot.getLeftCount();//left wheel rotation angle
 327       int right = _pilot.getRightCount();
 328       temp = left;
 329       left -=_left0;
 330       _left0 = temp;
 331       temp = right;
 332       right -= _right0;
 333       _right0 = temp ;
 334       if(left == 0 && right == 0)return; // no movement
 335       int outside = 0;
 336       int inside = 0;
 337       byte direction = 0;
 338       float dx = 0;
 339       float dy = 0;
 340       if(Math.abs(left)<Math.abs(right))
 341       {
 342          outside = right;
 343          inside = left;
 344          direction = 1;
 345       }
 346       else
 347       {
 348          outside = left;
 349          inside = right;
 350          direction = -1; // turn to right
 351       }
 352
 353       float turnAngle = direction*(outside-inside)*_pilot._wheelDiameter/(2*_pilot._trackWidth);
 354
 355       boolean approx = false;
 356       if(1.0f*inside/outside > .98) // probably movement was intended to be straight
 357       {
 358          float distance = 0.5f*(inside+outside)/_pilot._degPerDistance;
 359          float projection = (float)Math.toRadians(_heading+turnAngle/2);
 360          dx = distance*(float)Math.cos(projection);
 361          dy = distance*(float)Math.sin(projection);
 362          approx = true;
 363       }
 364       else
 365       {
 366          float turnRadius = _pilot._trackWidth*(outside+inside)/(2*(outside - inside));
 367          double  headingRad = (Math.toRadians(_heading));
 368          if(direction == -1)headingRad +=Math.PI;
 369          double  turnRad =  Math.toRadians(turnAngle);
 370          dx = turnRadius*(float)(Math.sin(headingRad + turnRad) - Math.sin(headingRad));
 371          dy = turnRadius*(float)(Math.cos(headingRad)- Math.cos(headingRad+turnRad));
 372       }
 373       _heading = normalize(_heading + turnAngle); // keep angle between -180 and 180
 374       _x += dx;
 375       _y += dy;
 376       if(approx) _heading -= turnAngle/2; // correct approximation
 377       _updated = true;
 378    }
 379
 380    /**
 381     * Moves the NXT robot in a circular path with a specified radius. <br>
 382     * The center of the turning circle is on the right side of the robot iff parameter radius is negative;  <br>
 383     * Postcondition:  motor speed is NOT restored to previous value;
 384     * @param radius is the radius of the circular path. If positive, the left wheel is on the inside of the turn.  If negative, the left wheel is on the outside.
 385     */
 386    public void turn(float radius)
 387    {
 388       _updated = false;
 389       _pilot.resetTachoCount();
 390       _right0 = 0;
 391       _left0 = 0;
 392       _pilot.steer(turnRate(radius));
 393    }
 394
 395    /**
 396     * Moves the NXT robot in a circular path through a specific angle; If waitForCompletion is true, returns when angle is reached. <br>
 397     * The center of the turning circle is on the right side of the robot iff parameter radius is negative.
 398     *  Robot will stop when total rotation equals angle. If angle is negative, robot will move travel backwards.
 399     * @param radius radius of the turning circle
 400     * @param angle the angle by which the robot heading changes, + or -
 401     */
 402    public void turn(float radius, int angle)
 403    {
 404       turn(radius,angle,false);
 405    }
 406
 407    /**
 408     * Moves the NXT robot in a circular path through a specific angle; If waitForCompletion is true, returns when angle is reached. <br>
 409     * The center of the turning circle is on the right side of the robot iff parameter radius is negative.
 410     * Robot will stop when total rotation equals angle. If angle is negative, robot will travel backwards.
 411     * @param radius  see turn(turnRage, angle)
 412     * @param immediateReturn iff true, the method returns immediately, in which case the programmer <br>
 413     * is responsible for calling updatePosition() before the robot moves again.
 414     */
 415    public void turn(float radius, int angle, boolean immediateReturn)
 416    {
 417       _updated = false;
 418       _pilot.resetTachoCount();
 419       _right0 = 0;
 420       _left0 = 0;
 421       _pilot.steer(turnRate(radius),angle,immediateReturn);
 422       if(!immediateReturn) updatePosition();
 423    }
 424
 425    /**
 426     * returns equivalent angle between -180 and +180
 427     */
 428    private float normalize(float angle)
 429    {
 430       float a = angle;
 431       while(a > 180) a -= 360;
 432       while(a < -180) a += 360;
 433       return a;
 434    }
 435
 436    private int turnRate(float radius)
 437    {
 438       int direction = 1;
 439       if(radius<0)
 440       {
 441          direction = -1;
 442          radius = -radius;
 443       }
 444       float ratio = (2*radius - _pilot._trackWidth)/(2*radius+_pilot._trackWidth);
 445       return Math.round(direction * 100*(1 - ratio));
 446    }
 447 }
 448