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

   1 package lejos.navigation;
   2 //import lejos.navigation.*;
   3 import lejos.nxt.Motor;
   4
   5
   6 /**
   7 * The Pilot class is a software abstraction of the Pilot mechanism of a NXT robot. It contains methods to control robot movents: travel forward or backward in a straight line or a circular path or rotate to a new direction.  <br>
   8 * Note: this class will only work with two independently controlled Pilot motors to steer differentially, so it can rotate within its own footprint (i.e. turn on one spot).<br>
   9 * It can be used with robots that have reversed motor design: the robot moves in the direction opposite to the the dirction of motor rotation.
  10 * Uses the Motor class, which regulates motor speed using the NXT motor's built in tachometer. <br>
  11 * Some methods optionally return immediately so the thread that called the method can monitor sensors and call stop() if necessary.  <br>
  12 * Uses the smoothAcceleration  property of Motors to improve motor symchronication
  13 *  Example:<p>
  14 * <code><pre>
  15 *       Pilot sc = new Pilot(2.1f,4.4f,Motor.A, Motor.C,true);
  16 *        sc.setSpeed(720);// 2 RPM
  17 *       sc.travel(12);
  18 *       sc.rotate(-90);
  19 *       sc.travel(-12,true);
  20 *       while(sc.isMoving())Thread.yield();
  21 *       sc.rotate(-90);
  22 *       sc.rotateTo(270);
  23 *       sc.steer(-50,180,true);
  24 *       while(sc.isMoving())Thread.yield();
  25 *       sc.steer(100);
  26 *       try{Thread.sleep(1000);}
  27 *   catch(InterruptedException e){}
  28 *       sc.stop();
  29 * </pre></code>
  30  **/
  31
  32 public class Pilot
  33 {
  34         /**
  35          *left motor
  36          */
  37         public Motor _left;
  38
  39         /**
  40          * right motor
  41          */
  42         public Motor _right;
  43
  44         /**
  45          * motor degrees per unit of travel
  46          */
  47         public final float _degPerDistance;
  48
  49         /**
  50          * Motor revolutions for 360 degree rotation of robot (motors running in opposite directions.
  51          * calculated from wheel diameter and track width.  Used by rotate() and steer() methods
  52          **/
  53         private final float _turnRatio;
  54
  55         /**
  56          * motor speed  degrees per second. Used by all methods that cause movememt
  57          */
  58         protected int _speed = 360;
  59
  60         /**
  61          * Motor rotation forward makes robot move forward iff parity == 1.
  62          */
  63         private byte _parity = 1;
  64
  65         /**
  66          * if true, motor speed regulation is turned on
  67          */
  68          private boolean _regulating = true;
  69
  70
  71         /**
  72          * distance between wheels - used in steer()
  73          */
  74         public  final float _trackWidth;
  75
  76         /**
  77          *      diameter of tires
  78          */
  79         public final float _wheelDiameter;
  80
  81         /**
  82          *  Allocates a Pilot object, and sets the physical parameters of the NXT robot. <br>
  83          *  Assumes  Motor.forward() causes the robot to move forward);
  84          *  @param wheelDiameter  Diameter of the tire, in any convenient units.  (The diameter in mm is usually printed on the tire).
  85          *  @param trackWidth Distance between center of right tire and center of left tire, in same units as wheelDiameter
  86          */
  87         public Pilot(float wheelDiameter,float trackWidth,Motor leftMotor, Motor rightMotor)
  88         {
  89                 _left = leftMotor;
  90                 _right = rightMotor;
  91                 _degPerDistance = 360/((float)Math.PI*wheelDiameter);
  92                 _turnRatio = trackWidth/wheelDiameter;
  93                 _left.regulateSpeed(true);
  94                 _left.smoothAcceleration(true);
  95                 _right.regulateSpeed(true);
  96                 _right.smoothAcceleration(true);
  97                 _trackWidth = trackWidth;
  98                 _wheelDiameter = wheelDiameter;
  99         }
 100
 101         /**
 102          *  Allocates a Pilot object, and sets the physical parameters of the NXT robot. <br>
 103          *  @param wheelDiameter  Diameter of the tire, in any convenient units.  (The diameter in mm is usually printed on the tire).
 104          *  @param trackWidth Distance between center of right tire and center of left tire, in the same units as wheelDiameter
 105          *  @param reverse if true, the NXT robot moves forward when the motors are running backward.
 106          */
 107         public Pilot(float wheelDiameter,float trackWidth,Motor leftMotor, Motor rightMotor, boolean reverse)
 108         {
 109                 this(wheelDiameter, trackWidth,leftMotor,rightMotor);
 110                 if(reverse) _parity = -1;
 111                 else _parity = 1;
 112         }
 113
 114         /**
 115          *      returns tachoCount of left motor; Positive value means motor has moved the robot forward;
 116          */
 117         public int getLeftCount(){ return _parity*_left.getTachoCount();}
 118
 119         /**
 120          *returns tachoCount of the right motor; Positive value means motor has moved the robot forward;
 121          */
 122         public int getRightCount(){ return _parity*_right.getTachoCount();}
 123
 124         /**
 125          *returns actual speed of left motor in degrees per second; a negative value if motor is rotating backwards  <br>
 126          * Updated avery 100 ms.
 127          **/
 128         public int getLeftActualSpeed(){ return _left.getActualSpeed();}
 129
 130         /**
 131          *returns actual speed of right motor in deg/sec;  a negative value if motor is rotating backwards. <br>
 132          *  Updated avery 100 ms.
 133          **/
 134         public int getRightActualSpeed() { return _right.getActualSpeed();}
 135
 136         /**
 137          * return ratatio of Motor revolutions per 360 degree rotation of the robot
 138          */
 139         public float getTurnRatio(){ return _turnRatio;}
 140
 141         /**
 142          * Sets speed of both motors,  degrees/sec; also sets retulate speed true
 143          */
 144         public void setSpeed(int speed)
 145         {
 146                 _speed = speed;
 147                 _left.regulateSpeed(_regulating);
 148                 _left.smoothAcceleration(true);
 149                 _right.regulateSpeed(_regulating);
 150                 _right.smoothAcceleration(true);
 151                 _left.setSpeed(speed);
 152                 _right.setSpeed(speed);
 153         }
 154
 155         /**
 156          *  Moves the NXT robot forward until stop() is called.
 157          */
 158         public void forward()
 159         {
 160                 setSpeed(_speed);
 161                 if(_parity == 1) fwd();
 162                 else bak();
 163         }
 164
 165         /**
 166          * Moves the NXT robot backward until stop() is called.
 167          */
 168         public void backward()
 169         {
 170                 setSpeed(_speed);
 171                 if(_parity == 1)bak();
 172                 else fwd();
 173         }
 174
 175         /**
 176          * Rotates the  NXT robot through a specific angle; Rotates left if angle is positive, right if negative,
 177          * Returns when angle is reached.
 178          * Wheels turn in opposite directions producing a  zero radius turn.
 179          * @param angle  degrees. Positive angle rotates to the left (clockwise); negative to the right. <br>Requires correct values for wheel diameter and track width.
 180          */
 181         public void rotate(int angle)
 182         {
 183                 rotate(angle,false);
 184         }
 185
 186         /**
 187          * Rotates the  NXT robot through a specific angle; Rotates left if angle is positive, right if negative;
 188          * Returns immediately  iff immediateReturn is true.
 189          * Wheels turn in opposite directions producing a  zero radius turn.
 190          * @param angle  degrees. Positive angle rotates to the left; negative to the right. <br>Requires correct values for wheel diameter and track width.
 191          * @param immediateReturn if true this method returns immediately
 192          */
 193         public void rotate(int angle, boolean immediateReturn )
 194         {
 195                 setSpeed(_speed);
 196                 int ta = _parity*(int)( angle*_turnRatio);
 197                 _left.rotate(-ta,true);
 198                 _right.rotate(ta,true);
 199                 if(immediateReturn)return;
 200                 while(isMoving())Thread.yield();
 201         }
 202
 203         /**
 204          * returns the angle of rotation of the robot since last call to reset of tacho count;
 205          */
 206         public int getAngle()
 207         {
 208                 return  _parity*Math.round((getRightCount()-getLeftCount())/(2*_turnRatio));
 209         }
 210
 211         /**
 212          * Stops the NXT robot
 213          */
 214         public void stop()
 215         {
 216                 _left.stop();
 217                 _right.stop();
 218         }
 219
 220         /**
 221          * returns true iff the NXT robot is moving
 222          **/
 223     public boolean isMoving()
 224         {
 225                 return _left.isMoving()||_right.isMoving()||_left.isRotating()||_right.isRotating();
 226         }
 227
 228     /**
 229      *resets tacho count for both motors
 230      **/
 231     public void resetTachoCount()
 232     {
 233         _left.resetTachoCount();
 234         _right.resetTachoCount();
 235     }
 236
 237     /**
 238      * returns distance taveled since last reset of tacho count
 239      **/
 240     public float getTravelDistance()
 241         {
 242                 int avg =( _left.getTachoCount()+_right.getTachoCount())/2;
 243                 return  _parity*avg/_degPerDistance;
 244         }
 245
 246     /**
 247      * Moves the NXT robot a specific distance;<br>
 248      * A positive distance causes forward motion;  negative distance  moves backward.
 249      * @param  distance of robot movement. Unit of measure for distance must be same as wheelDiameter and trackWidth
 250      **/
 251         public void travel(float distance)
 252         {
 253                 travel(distance,false);
 254         }
 255
 256         /**
 257          * Moves the NXT robot a specific distance; if immediateReturn is true, method returns immediately. <br>
 258          * A positive distance causes forward motion; negative distance moves backward.
 259          * @param immediateReturn  If true, method returns immediately, and robot stops after traveling the distance.  If false, method returns immediately.
 260          */
 261         public void travel(float distance,boolean immediateReturn)
 262         {
 263                 setSpeed(_speed);
 264                 _left.rotate((int)(_parity*distance*_degPerDistance),true);
 265                 _right.rotate((int)(_parity*distance*_degPerDistance),true);
 266                 if(immediateReturn)return;
 267                 while(isMoving())Thread.yield();
 268         }
 269
 270         /**
 271          * Moves the NXT robot in a circular path at a specific turn rate. <br>
 272          * The center of the turning circle is on the right side of the robot iff parameter turnRate is negative;  <br>
 273          * turnRate values are between -200 and +200;
 274          * @param turnRate If positive, the left wheel is on the inside of the turn.  If negative, the left wheel is on the outside.
 275          * This parameter determines the ratio of inner wheel speed to outer wheel speed (as a percent). <br>
 276          * <I>Formula:</i>    ratio  =  100 - abs(turnRate). When the ratio is negative, the outer and inner wheels rotate in opposite directions.<br>
 277          * Examples: <UL><LI> steer(25)-> inner wheel turns at 75% of the speed of the outer wheel <br>
 278          *           <li>steer(100) -> inner wheel stops.<br>
 279          *                       <li>steer(200) -> means that the inner wheel turns at the same speed as the outer wheel - a zero radius turn. <br></UL>
 280          */
 281         public void steer(int turnRate)
 282         {
 283                 steer(turnRate,Integer.MAX_VALUE,true);
 284         }
 285
 286         /**
 287          * Moves the NXT robot in a circular path through a specific angle; <br>
 288          * Negative turnRate means center of turning circle is on right side of the robot; <br>
 289         * Range of turnRate values : -200 : 200 ;
 290         * Robot will stop when total rotation equals angle. If angle is negative, robot will move travel backwards.
 291         * @param turnRate If positive, the left wheel is on the inside of the turn.  If negative, the left wheel is on the outside.
 292         * This parameter determines the ratio of inner wheel speed to outer wheel speed (as a percent). <br>
 293         * @param angle  the angle through which the robot will rotate and then stop. If negative, robot traces the turning circle backwards.
 294         */
 295         public void steer(int turnRate, int angle)
 296         {
 297                 steer(turnRate,angle,false);
 298         }
 299
 300         /**
 301          * Moves the NXT robot in a circular path, and stops when the direction it is facing has changed by a specific angle;  <br>
 302          * Returns immediately if immediateReturn is true.  The robot will stop automatically when the turm is complete.
 303          * The center of the turning circle is on right side of the robot iff parameter turnRate is negative  <br>
 304          * turnRate values are between -200 and +200;
 305          * @param turnRate If positive, the left wheel is on the inside of the turn.  If negative, the left wheel is on the outside.
 306          * This parameter determines the ratio of inner wheel speed to outer wheel speed (as a percent). <br>
 307          * @param angle  the angle through which the robot will rotate and then stop. If negative, robot traces the turning circle backwards.
 308          * @param immediateReturn iff true, method returns immedediately.
 309          *
 310          */
 311         public void steer(int turnRate, int angle, boolean immediateReturn)
 312         {
 313                 Motor inside;
 314                 Motor outside;
 315                 int rate = turnRate;
 316                 if(rate <- 200)rate = -200;
 317                 if(rate > 200)rate = 200;
 318                 if(rate==0)
 319                 {
 320                         if(angle<0)backward();
 321                         else forward();
 322                         return;
 323                 }
 324                 if (turnRate<0)
 325                 {
 326                         inside = _right;
 327                         outside = _left;
 328                         rate = -rate;
 329                 }
 330                 else
 331                 {
 332                         inside = _left;
 333                         outside = _right;
 334                 }
 335                 outside.setSpeed(_speed);
 336                 float steerRatio = 1 - rate/100.0f;
 337                 inside.setSpeed((int)(_speed*steerRatio));
 338                 if(angle == Integer.MAX_VALUE) //no limit angle for turn
 339                 {
 340                         if(_parity == 1) outside.forward();
 341                         else outside.backward();
 342                         if( _parity*steerRatio > 0) inside.forward();
 343                         else inside.backward();
 344                         return;
 345                 }
 346                 float rotAngle  = angle*_trackWidth*2/(_wheelDiameter*(1-steerRatio));
 347                 inside.rotate(_parity*(int)(rotAngle*steerRatio),true);
 348                 outside.rotate(_parity*(int)rotAngle,true);
 349                 if(immediateReturn)return;
 350                 while (isMoving())Thread.yield();
 351                 inside.setSpeed(outside.getSpeed());
 352         }
 353
 354         /**
 355          * motors backward  called by forward() and backward()
 356          */
 357         private void bak()
 358         {
 359                 _left.backward();
 360                 _right.backward();
 361         }
 362 /**
 363  *Sets motor speed regulation   on = true (default) or off = false; <br>
 364  *Allows steer() method to be called by (for example)
 365  *a line tracker or compass navigator so direction control is from sensor inputs
 366  */
 367         public void regulateSpeed(boolean yes)
 368         {
 369                 _regulating = yes;
 370                  _left.regulateSpeed(yes);
 371                 _right.regulateSpeed(yes);
 372         }
 373
 374         /**
 375          * motors forward called by forward() and backward()
 376          */
 377          private void fwd()
 378          {
 379                 _left.forward();
 380                 _right.forward();
 381          }
 382
 383 }