Package lejos.robotics.navigation

Source Code of lejos.robotics.navigation.SimpleNavigator

package lejos.robotics.navigation;

import lejos.robotics.Pose;
import lejos.geom.Point;
import lejos.robotics.*;

/*
* WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
* DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
*/

/**
* The SimpleNavigator class uses dead reckoning to  keep track of its robot pose (the location in the plane
* and its heading -  the direction in which it moves).  While dead reckoning is relatively
* easy to implement and very quick to caolculate,  its disadvantage
* is that errors in the estimated pose accumulate.<br>
* SimpleNavigator can perform  three elementary movements  in a plane:  trvel in  a straignt line,
* move in the arc of a circle, and a rotate  in place.  The movement commands
* have an immediate return option which is useful, for example, for a client
* class that uses s SimpleNavigataor to detect obstacles or monitor incoming messages while moving.
<br>
* This class uses a private Pilot  object to execute these moves.  The Pilot directly
* controls the hardware, which must be able to  turn in place,
* for example using 2 wheel differential steering.  The  Pilot is passed to the Navitator
* a the parameter of its constructor.  After the Navigator is constructed,
* the client has no further need for the Pilot, but issues commands to the Navigator.
* If the client  bypasses the navigator and issues commsnds directly to the Pilot, this  will destroy
* the accuracy of the Navigataor's pose.<br>
*<b>A note about coordinates:</b> All angles are in degrees, distances in the units used to specify robot dimensions.
* Angles related to positions in the plane are relative to the X axis ;  direction of the Y axis is 90 degrees.
* The x and y coordinate values and the direction angle are all initialized to 0, so if the first move is forward() the robot will run along
* the x axis.<br>
*
*/
public class SimpleNavigator {


    /**
     * Allocates a SimpleNavigator with a Pilot that you supply.
     * @param  pilot can be  any class that implements the pilot interface
     */
    public SimpleNavigator(Pilot pilot) {
        this.pilot = pilot;
    }

    /**
     * Allocates a SimpleNavigator object and initializes it with a new TachoPilot <br>
     * If you want to use a different Pilot class, use the single parameter constructor.
     * @param wheelDiameter The diameter of the wheel, usually printed on the Lego tire.
     *  Use any units you wish (e.g. 56 mm = 5.6 cm = 2.36 in)
     * @param trackWidth The distance from the center of the left tire to the center
     * of the right tire, same units as wheel diameter
     * @param rightMotor The motor used to drive the right wheel e.g. Motor.C.
     * @param leftMotor The motor used to drive the left wheel e.g. Motor.A.
     * @param reverse  If motor.forward() dives the robot backwars, set this parameter true.
     *
     * @deprecated The correct way is to create the Pilot in advance and to use that in construction of the
     *             SimpleNavigator. Otherwise the SimpleNavigator needs to know detail it should not care about!
     */
    @Deprecated
    public SimpleNavigator(float wheelDiameter, float trackWidth, TachoMotor leftMotor, TachoMotor rightMotor, boolean reverse) {
      // In the signature Motor was not changed to TachoMotor. This method only saves one to write "new TachoPilot" at the
      // cost of maintaining this method and comments, thus it should not be used!
        pilot = new TachoPilot(wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
    }

    /**
     * Allocates a SimpleNavigator object and initializes it with a new TachoPilot.<br>
     * If you want to use a different Pilot class, use the single parameter constructor.
     * @param wheelDiameter The diameter of the wheel, usually printed on the Lego tire.
     *  Use any units you wish (e.g. 56 mm = 5.6 cm = 2.36 in)
     * @param trackWidth The distance from the center of the left tire to the center
     * of the right tire, sane units as wheel diameter
     * @param rightMotor The motor used to drive the right wheel e.g. Motor.C.
     * @param leftMotor The motor used to drive the left wheel e.g. Motor.A
     *
     * @deprecated The correct way is to create the Pilot in advance and to use that in construction of the
     *             SimpleNavigator. Otherwise the SimpleNavigator needs to know detail it should not care about!
     */
    @Deprecated
    public SimpleNavigator(float wheelDiameter, float trackWidth, TachoMotor leftMotor, TachoMotor rightMotor) {
      // In the signature Motor was not changed to TachoMotor. This method only saves one to write "new TachoPilot" at the
      // cost of maintaining this method and comments, thus it should not be used!
        pilot = new TachoPilot(wheelDiameter, trackWidth, leftMotor, rightMotor);
    }
/**
*gets the current value of the X coordinate
* @return current x
*/
  public float getX()
  {
    updatePose();
    _current = !isMoving();
    return _pose.getX();
  }
/**
* gets the current value of the Y coordinate
* @return current Y
*/
  public float getY()
  {
    updatePose();
    _current = !isMoving();
    return _pose.getY();
  }
/**
* gets the current value of the robot heading
* @return current heading
*/
  public float getHeading()
  {
    updatePose();
        _current = !isMoving();
    return _pose.getHeading();
  }
  /**
   * gets the current robot pose
   * @return current pose
   */
public float getAngle()
{
  return getHeading();
}
public void forward()
{
   updatePose();
   _current = false;
   pilot.forward();
}
public void backward()
{
    updatePose();
   _current = false;
   pilot.backward();
}
public void rotateLeft()
{
   steer(200);
}
public void rotateRight()
{
   steer(-200);
}
  public Pose getPose()
  {
    updatePose();
        _current = !isMoving();
    return _pose;
  }
  public void updatePosition()
  {
    updatePose();
  }

/**
* sets the robot pose to the new coordinates and heading
* @param x coordinate
* @param y coordinate
* @param heading direction of robot forward movement
*/
  public void setPose(float x, float y, float heading)
  {
    _pose.setLocation(new Point(x, y));
    _pose.setHeading(heading);
  }
/**
* sets the robot pose
* @param pose new pose
*/
  public void setPose(Pose pose)
  {
    _pose = pose;
  }
  public void setPosition(float x, float y, float heading)
  {
    setPose(x,y,heading);
  }
/**
* sets the robots movement speed  - distance units/second
* @param speed
*/
  public void setMoveSpeed(float speed)
  {
    pilot.setMoveSpeed(speed);
  }
/**
* sets the robot turn speed  -degrees/second
* @param speed
*/
  public void setTurnSpeed(float speed)
  {
    pilot.setTurnSpeed(speed);
  }
/**
* Stops the robot. Depending on the robot speed, it travels a bit before actually
* coming to a complete halt.
*/
  public void stop()
  {
    pilot.stop();
    updatePose();
    _current = true;
  }
/**
* returns true if the robot is moving
* @return true if it is moving
*/
  public boolean isMoving()
  {
    return pilot.isMoving();
  }

  /**
   * Moves the NXT robot a specific distance. A positive value moves it forwards and
   * a negative value moves it backwards.
   * @param distance The positive or negative distance to move the robot, same units as _wheelDiameter
   */
  public void travel(float distance)
  {
    travel(distance, false);
  }

  /**
   * Moves the NXT robot a specific distance. A positive value moves it forwards and
   * a negative value moves it backwards.
   * @param distance The positive or negative distance to move the robot, same units as _wheelDiameter
   * @param immediateReturn if true, the method returns immediately
   */
  public void travel(float distance, boolean immediateReturn)
  {
    updatePose();
        _current = false;
    pilot.travel(distance, immediateReturn);
  }

  /**
   * Rotates the NXT robot through a specific number of degrees in a direction (+ or -).
   * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right.
   **/
  public void rotate(float angle)
  {
    rotate(angle, false);
  }

  /**
   * Rotates the NXT robot through a specific number of degrees in a direction (+ or -).
   *  If immediateReturn is true, method returns immidiately.
   * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right.
   * @param immediateReturn if true, the method returns immediately
   */
  public void rotate(float angle, boolean immediateReturn)
  {
    int turnAngle = Math.round(angle);
    updatePose();
      _current = false;
    pilot.rotate(turnAngle, immediateReturn);
  }

  /**
   * Rotates the NXT robot to point in a specific direction, using the smallest
   * rotation necessary
   * @param angle The angle to rotate to, in degrees.
   */
  public void rotateTo(float angle)
  {
    rotateTo(angle, false);
  }

  /**
   * Rotates the NXT robot to point in a specific direction relative to the x axis.  It make the smallest
   * rotation  necessary .
   * If immediateReturn is true, method returns immidiately
   * @param angle The angle to rotate to, in degrees.
   * @param immediateReturn if true,  method returns immediately
   */
  public void rotateTo(float angle, boolean immediateReturn)
  {
    float turnAngle = angle - _pose.getHeading();
    while (turnAngle < -180)turnAngle += 360;
    while (turnAngle > 180) turnAngle -= 360;
    rotate(turnAngle, immediateReturn);
  }
/**
* Robot moves to grid coordinates x,y.  First it rotates to the proper direction
* then travels in a straight line to that point
* @param x destination X coordinate
* @param y destination Y coordinate
*/
  public void goTo(float x, float y)
  {
    goTo(x, y, false);
  }
/**
* Robot moves to grid coordinates x,y.  First it rotates to the proper direction
* then travels in a straight line to that point
* @param x destination X coordinate
* @param y destination Y coordinate
*/
  public void goTo(float x, float y, boolean immediateReturn)
  {
    rotateTo(angleTo(x, y));
    travel(distanceTo(x, y), immediateReturn);
  }
/**
* Returns the distance from robot current location to the point with coordinates x,y
* @param x coordinate of destination
* @param y coordinate of destination
* @return the distance
*/
  public float distanceTo(float x, float y)
  {
    updatePose();
    _current = !isMoving();
    return _pose.distanceTo(new Point(x, y));
  }
/**
* Returns the  angle from robot current location to the point with coordinates x,y
* @param x coordinate of destination
* @param y coordinate of destination
* @return angle
*/
  public float angleTo(float x, float y)
  {
    updatePose();
    _current = !isMoving();
    return _pose.angleTo(new Point(x, y));
  }

    public void updatePose()
  {
      if(_current) return;
    float distance = pilot.getTravelDistance() - _distance0;
    float turnAngle = pilot.getAngle() - _angle0;
    double dx = 0;
    double dy = 0;
    double headingRad = (Math.toRadians(_pose.getHeading()));
    if (Math.abs(turnAngle) > .5)
    {
      double turnRad = Math.toRadians(turnAngle);
      double radius = distance / turnRad;
      dy = radius * (Math.cos(headingRad) - Math.cos(headingRad + turnRad));
      dx = radius * (Math.sin(headingRad + turnRad) - Math.sin(headingRad));
    } else if (Math.abs(distance) > .01)
    {
      dx = distance * (float) Math.cos(headingRad);
      dy = distance * (float) Math.sin(headingRad);
    }
    _pose.translate((float) dx, (float) dy);
    _pose.rotateUpdate(turnAngle);
    _angle0 = pilot.getAngle();
    _distance0 = pilot.getTravelDistance();
  }

    /**
     * Starts  the NXT robot moving in a circular path with a specified radius; <br>
     * The center of the turning circle is on the left side of the robot if parameter radius is positive
     * and on the right if negative.  <br>
     * @param radius - 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.
     */
    public void arc(float radius) {
        pilot.arc(radius);
    }

    /**
     * Moves the NXT robot in a circular arc through the specified angle;  <br>
     * The center of the turning circle is on the left side of the robot if parameter radius is positive
     * and on the right if negative.
     * Robot will stop when total rotation equals angle. If angle is negative, robot will move travel backwards.
     * <br>See also {@link #travelArc(float radius, float distance)}
   * @param radius - 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.
     */
    public void arc(float radius, int angle) {
        arc(radius, angle, false);
    }

    /**
     * Moves the NXT robot in a circular arc through a specific angle; <br>
     * The center of the turning circle is on the left side of the robot if parameter radius is positive
     * and on the right if negative.
     * Robot will stop when total rotation equals angle. If angle is negative, robot will travel backwards.
     * <br>See also {@link #travelArc(float radius, float distance, boolean immedisteReturn)}
      * @param radius - 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.
     * @param angle The sign of the angle determines the direction of robot motion
     * @param immediateReturn if true, the method returns immediately
     */
    public void arc(float radius, int angle, boolean immediateReturn) {
        updatePose();
        _current = false;
        pilot.arc(radius, angle, immediateReturn);
    }
   
     /**
     * Moves the NXT robot in a circular arc through a specific distance; <br>
     * The center of the turning circle is on the left side of the robot if parameter radius is positive
     * and on the right if negative.
     * Robot will stop when distance traveled equals distance. If distance is negative, robot will travel backwards.
     * <br>See also {@link #arc(float radius, int angle)}
  
     * @param radius  of the turning circle; the sign determines if the center if the turn is left or right of the robot.
     * @param distance The sign of the distance determines the direction of robot motion
     * updatePosition() before the robot moves again.
     */
    public void travelArc(float radius, float distance){
      travelArc(radius,distance,false);
    }
   
    /**
     * Moves the NXT robot in a circular arc through a specific distance; <br>
     * The center of the turning circle is on the left side of the robot if parameter radius is positive
     * and on the right if negative.
     * Robot will stop when distance traveled equals distance. If distance is negative, robot will travel backwards.
     * <br>See also {@link #arc(float radius, int angle, boolean immediateReturn)}
     * @param radius  of the turning circle; the sign determines if the center if the turn is left or right of the robot.
     * @param distance The sign of the distance determines the direction of robot motion
     * @param immediateReturn if true, the method returns immediately.
     */
    public void travelArc(float radius, float distance, boolean immediateReturn)
    {
      updatePose();
        pilot.travelArc(radius, distance, immediateReturn);
    }

      /**
   * Starts the robot moving along a curved path. This method is similar to the
   * {@link #arc(float radius)} method except it uses a ratio of motor
   * speeds to determine the curvature of the path and therefore has the ability to drive straight. This makes
   * it useful for line following applications.
   * <p>
   * The <code>turnRate</code> specifies the sharpness of the turn, between -200 and +200.<br>
   * The <code>turnRate</code> is used to calculate the  ratio of inner wheel speed to outer wheel speed <b>as a percent</b>.<br>
   * <I>Formula:</I> <code>ratio = 100 - abs(turnRate)</code>.<br>
   * When the ratio is negative, the outer and inner wheels rotate in
   * opposite directions.
   * <p>
   * If <code>turnRate</code> is positive, the center of the turning circle is on the left side of the robot.<br>
   * If <code>turnRate</code> is negative, the center of the turning circle is on the right side of the robot.<br>
   * If <code>turnRate</code> is zero, the robot travels in a straight line
   * <p>
   * Examples of how the formula works:
   * <UL>
   * <LI><code>steer(0)</code> -> inner and outer wheels turn at the same speed, travel  straight
   * <LI><code>steer(25)</code> -> the inner wheel turns at 75% of the speed of the outer wheel, turn left
   * <LI><code>steer(100)</code> -> the inner wheel stops and the outer wheel is at 100 percent, turn left
   * <LI><code>steer(200)</code> -> the inner wheel turns at the same speed as the outer wheel - a zero radius turn.
   * </UL>
   * <p>
   * Note: If you have specified a drift correction in the constructor it will not be applied in this method.
   *
   * @param turnRate If positive, the left side of the robot is on the inside of the turn. If negative,
   * the left side is on the outside.
   */
  public void steer(int turnRate)
  {
    updatePose();
    _current = false;
    pilot.steer(turnRate);
  }

  /**
   * Moves the robot along a curved path through a specified turn angle. This method is similar to the
   * {@link #arc(float radius , int angle)} method except it uses a ratio of motor
   * speeds to determine the curvature of the  path and therefore has the ability to drive straight. This makes
   * it useful for line following applications. This method does not return until the robot has
   * completed moving <code>angle</code> degrees along the arc.<br>
   * The <code>turnRate</code> specifies the sharpness of the turn, between -200 and +200.<br>
   * For details about how this paramet works. see {@link #steer(int turnRate) }
   * <p>
   * The robot will stop when the degrees it has moved along the arc equals <code>angle</code>.<br>
   * If <code>angle</code> is positive, the robot will move travel forwards.<br>
   * If <code>angle</code> is negative, the robot will move travel backwards.
   * If <code>angle</code> is zero, the robot will not move and the method returns immediately.
   * <p>
   * Note: If you have specified a drift correction in the constructor it will not be applied in this method.
   *
   * @param turnRate If positive, the left side of the robot is on the inside of the turn. If negative,
   * the left side is on the outside.
   * @param angle The angle through which the robot will rotate. If negative, robot traces the turning circle backwards.
   */
  public void steer(int turnRate, int angle)
  {
    pilot.steer(turnRate, angle, false);
  }
/**
   * Moves the robot along a curved path for a specified angle of rotation. This method is similar to the
   * {@link #arc(float radius, int angle, boolean immediateReturn)} method except it uses a ratio of motor
   * speeds to speeds to determine the curvature of the path and therefore has the ability to drive straight.
   * This makes it useful for line following applications. This method has the ability to return immediately
   * by using the <code>immediateReturn</code> parameter set to <b>true</b>.
   * <p>
   * The <code>turnRate</code> specifies the sharpness of the turn, between -200 and +200.<br>
   * For details about how this paramet works, see {@link #steer(int turnRate) }
   * <p>
   * The robot will stop when the degrees it has moved along the arc equals <code>angle</code>.<br>
   * If <code>angle</code> is positive, the robot will move travel forwards.<br>
   * If <code>angle</code> is negative, the robot will move travel backwards.
   * If <code>angle</code> is zero, the robot will not move and the method returns immediately.
   * <p>
   * Note: If you have specified a drift correction in the constructor it will not be applied in this method.
   *
   * @param turnRate If positive, the left side of the robot is on the inside of the turn. If negative,
   * the left side is on the outside.
   * @param angle The angle through which the robot will rotate. If negative, robot traces the turning circle backwards.
   * @param immediateReturn If immediateReturn is true then the method returns immediately.
   */
  public void steer(int turnRate, int angle, boolean immediateReturn)
  {
    updatePose();
    _current = false;
    pilot.steer(turnRate, angle, immediateReturn);
  }
        // orientation and co-ordinate data
    private Pose _pose = new Pose();
    private float _distance0 = 0;
    private float _angle0 = 0;
    private boolean _current = false; //prevent unnecessary pose updates
    private Pilot pilot;
}
TOP

Related Classes of lejos.robotics.navigation.SimpleNavigator

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.