nxtOSEK/lejos_nxj/src/java/classes/lejos/nxt/Motor.java

   1 package lejos.nxt;\r
   2 import lejos.nxt.Battery;\r
   3 import lejos.util.*;\r
   4 \r
   5 \r
   6 \r
   7 \r
   8 /**\r
   9  * Abstraction for a NXT motor. Three instances of <code>Motor</code>\r
  10  * are available: <code>Motor.A</code>, <code>Motor.B</code>\r
  11  * and <code>Motor.C</code>. To control each motor use\r
  12  * methods <code>forward, backward, reverseDirection, stop</code>\r
  13  * and <code>flt</code>. To set each motor's speed, use\r
  14  * <code>setSpeed.  Speed is in degrees per second. </code>.\\r
  15  * Methods that use the tachometer:  regulateSpeed, rotate, rotateTo <br>\r
  16  * Motor has 2 modes : speedRegulation and smoothAcceleration. These are initially enabled. <>\r
  17  * They can be switched off/on by the methods regulateSpeed() and smoothAcceleration().\r
  18  * The actual maximum speed of the motor depends on battery voltage and load.. \r
  19  * Speed regulation fails if the target speed exceeds the capability of the motor.\r
  20  * \r
  21  * <p>\r
  22  * Example:<p>\r
  23  * <code><pre>\r
  24  *   Motor.A.setSpeed(720);// 2 RPM\r
  25  *   Motor.C.setSpeed(720);\r
  26  *   Motor.A.forward();\r
  27  *   Motor.C.forward();\r
  28  *   Thread.sleep (1000);\r
  29  *   Motor.A.stop();\r
  30  *   Motor.C.stop();\r
  31  *   Motor.A.regulateSpeed(true);\r
  32  *   Motor.A.rotateTo( 360);\r
  33  *   Motor.A.rotate(-720,true);\r
  34  *   while(Motor.A.isRotating();\r
  35  *   int angle = Motor.A.getTachoCount(); // should be -360\r
  36  * </pre></code>\r
  37  * @author Roger Glassey revised 26 March 2007\r
  38  */\r
  39 public class Motor extends BasicMotor implements TimerListener\r
  40 {\r
  41   private TachoMotorPort _port;\r
  42   private int _speed = 360;\r
  43   private int _speed0 = 360;\r
  44   // used for speed regulation\r
  45   private boolean _keepGoing = true;// for regulator\r
  46   /**\r
  47    * Initially true; changed only by regulateSpeed(),<br>\r
  48    * used by Regulator, updteState, reset*\r
  49    */\r
  50   private boolean _regulate = true;\r
  51   private boolean _wasRegulating = false;\r
  52   public Regulator regulator = new Regulator();\r
  53   private Timer timer = new Timer(100,this);\r
  54   // used for control of angle of rotation\r
  55   private int _direction = 1; // +1 is forward ; used by rotate();\r
  56   private int _limitAngle;\r
  57   private int _stopAngle;\r
  58   private boolean _rotating = false;\r
  59   private boolean _wasRotating = false;\r
  60   private boolean _rampUp = true;\r
  61   private int _lastTacho = 0;\r
  62   private int _actualSpeed;\r
  63   private float _voltage;\r
  64 \r
  65 \r
  66    /** initialized to be false(ramping enabled); changed only by smoothAcceleration\r
  67    */\r
  68   private boolean _noRamp = false;\r
  69  \r
  70   /**\r
  71    * Motor A.\r
  72    */\r
  73   public static final Motor A = new Motor (MotorPort.A);\r
  74   \r
  75   /**\r
  76    * Motor B.\r
  77    */\r
  78   public static final Motor B = new Motor (MotorPort.B);\r
  79   \r
  80   /**\r
  81    * Motor C.\r
  82    */\r
  83   public static final Motor C = new Motor (MotorPort.C);\r
  84    \r
  85   public Motor (MotorPort port)\r
  86   {\r
  87     _port = port;\r
  88     regulator.setDaemon(true);\r
  89     regulator.start();\r
  90     timer.start();\r
  91   }\r
  92    public int getStopAngle() { return (int)_stopAngle;}\r
  93    \r
  94         /**\r
  95          * Causes motor to rotate forward.\r
  96          */\r
  97         public void forward()\r
  98         { \r
  99                 synchronized(regulator) \r
 100                 {\r
 101                         _mode = 1;\r
 102                         updateState();\r
 103                 }\r
 104         }  \r
 105                 \r
 106         /**\r
 107         * Causes motor to rotate backwards.\r
 108          */\r
 109         public void backward()\r
 110         {\r
 111                 synchronized(regulator)\r
 112                 {\r
 113                         _mode = 2;\r
 114                         updateState();\r
 115                 }\r
 116         }\r
 117 \r
 118         /**\r
 119          * Reverses direction of the motor. It only has\r
 120          * effect if the motor is moving.\r
 121          */\r
 122         public void reverseDirection()\r
 123         {\r
 124                 synchronized(regulator)\r
 125                 {\r
 126                         if (_mode == 1 || _mode == 2)\r
 127                     {\r
 128                                 _mode = (3 - _mode);\r
 129                                 updateState();\r
 130                     }\r
 131                 }\r
 132         }\r
 133 \r
 134         /**\r
 135          * Causes motor to float. The motor will lose all power,\r
 136          * but this is not the same as stopping. Use this\r
 137          * method if you don't want your robot to trip in\r
 138          * abrupt turns.\r
 139          */   \r
 140         public void flt()\r
 141         {\r
 142                 synchronized(regulator)\r
 143                 {\r
 144                         _mode = 4;\r
 145                         updateState();\r
 146                 }\r
 147         }\r
 148 \r
 149         /**\r
 150          * Causes motor to stop, pretty much\r
 151          * instantaneously. In other words, the\r
 152          * motor doesn't just stop; it will resist\r
 153          * any further motion.\r
 154          * Cancels any rotate() orders in progress\r
 155          */\r
 156         public void stop()\r
 157         {\r
 158                 synchronized(regulator)\r
 159                 {\r
 160                         _mode = 3;\r
 161                     updateState();\r
 162                 }\r
 163         }\r
 164   /** \r
 165    *calls controlMotor, startRegating;  updates _direction, _rotating, _wasRotating\r
 166    */\r
 167   void updateState()\r
 168   {\r
 169           _rotating = false; //regulator should stop testing for rotation limit  ASAP\r
 170 //      synchronized(regulator)\r
 171         {\r
 172                 if(_wasRotating)\r
 173                 {\r
 174                         setSpeed(_speed0);\r
 175                         _regulate = _wasRegulating;\r
 176                 }\r
 177                 _wasRotating = false; // perhaps redundant\r
 178 \r
 179                 if(_mode>2) // stop or float\r
 180                 {\r
 181                         _port.controlMotor(0, _mode);\r
 182                         return;\r
 183                 }\r
 184                  _port.controlMotor(_power, _mode);\r
 185         \r
 186                 if(_regulate)\r
 187                 {\r
 188                   regulator.reset();\r
 189                   _rampUp = true;\r
 190                 }\r
 191                  _direction = 3 - 2*_mode;\r
 192         }\r
 193   }\r
 194 \r
 195   /**\r
 196    * @return true iff the motor is currently in motion.\r
 197    */\r
 198   public final boolean isMoving()\r
 199   {\r
 200     return (_mode == 1 || _mode == 2 || _rotating);       \r
 201   }\r
 202   \r
 203   \r
 204   /**\r
 205    * causes motor to rotate through angle. <br>\r
 206    * @param  angle through which the motor will rotate\r
 207    */\r
 208   public void rotate(int angle)\r
 209   {\r
 210         rotateTo(getTachoCount()+angle);\r
 211   }\r
 212 \r
 213   /**\r
 214    * causes motor to rotate through angle; <br>\r
 215    * iff immediateReturn is true, method returns immediately and the motor stops by itself <br>\r
 216    * When the angle is reached, the method isRotating() returns false;\r
 217    * @param  angle through which the motor will rotate\r
 218    * @param immediateReturn iff true, method returns immediately, thus allowing monitoring of sensors in the calling thread. \r
 219    */\r
 220    public void rotate(int angle, boolean immediateReturn)\r
 221    {\r
 222                 int t = getTachoCount();\r
 223                 rotateTo(t+angle,immediateReturn);\r
 224         }\r
 225 \r
 226   /**\r
 227    * causes motor to rotate to limitAngle;  <br>\r
 228    * Then getTachoCount should be within +- 2 degrees of the limit angle when the method returns\r
 229    * @param  limitAngle to which the motor will rotate\r
 230    */\r
 231   public void rotateTo(int limitAngle)\r
 232   {\r
 233         rotateTo(limitAngle,false);\r
 234   }\r
 235 \r
 236   /**\r
 237    * causes motor to rotate to limitAngle; <br>\r
 238    * if immediateReturn is true, method returns immediately and the motor stops by itself <br>\r
 239    * Then getTachoCount should be within +- 2 degrees if the limit angle\r
 240    * When the angle is reached, the method isRotating() returns false;\r
 241    * @param  limitAngle to which the motor will rotate, and then stop. \r
 242    * @param immediateReturn iff true, method returns immediately, thus allowing monitoring of sensors in the calling thread. \r
 243     */\r
 244   public void rotateTo(int limitAngle,boolean immediateReturn)\r
 245   {\r
 246         synchronized(regulator)\r
 247         {\r
 248                 if(_wasRotating)\r
 249                 {\r
 250                         setSpeed(_speed0);\r
 251                         _regulate = _wasRegulating;\r
 252                         _wasRotating = false;\r
 253                 }\r
 254                 _stopAngle = limitAngle;\r
 255                 if(limitAngle > getTachoCount()) _mode = 1;\r
 256                 else _mode = 2;\r
 257             _port.controlMotor(_power, _mode);\r
 258             _direction = 3 - 2*_mode;\r
 259                 if(_regulate) regulator.reset();\r
 260                 if(!_wasRotating)\r
 261                 {\r
 262                   _stopAngle -= _direction * overshoot();\r
 263                   _limitAngle = limitAngle;\r
 264                 }\r
 265                 _rotating = true; // rotating to a limit\r
 266                 _rampUp = !_noRamp && Math.abs(_stopAngle-getTachoCount())>40 && _speed>200;  //no ramp for small angles\r
 267         if(immediateReturn)return;\r
 268         }\r
 269         while(_rotating) Thread.yield();\r
 270   }\r
 271 \r
 272 /**\r
 273  *inner class to regulate speed; also stop motor at desired rotation angle\r
 274  **/\r
 275  private class Regulator extends Thread\r
 276   {\r
 277         /**\r
 278          *tachoCount when regulating started\r
 279          */\r
 280         int angle0 = 0;\r
 281     /**\r
 282      * set by reset, used  to regulate  motor speed\r
 283      */ \r
 284         float basePower = 0;\r
 285     /**\r
 286      * time regulating started\r
 287      */\r
 288         int time0 = 0;\r
 289     float error = 0;\r
 290     /**\r
 291      * helper method - used by reset and setSpeed()\r
 292      */\r
 293         int calcPower(int speed)\r
 294         {   \r
 295 //              float pwr = 100 - 7.4f*Battery.getVoltage()+0.065f*speed;// no-load motor\r
 296       float pwr = 100 - 7.4f*_voltage+0.065f*speed;\r
 297                 if(pwr<0) return 0;\r
 298                 if(pwr>100)return 100;\r
 299                 else return (int)pwr;\r
 300         }\r
 301  \r
 302     /**\r
 303      * called by forward() backward() and reverseDirection() <br>\r
 304      * resets parameters for speed regulation\r
 305      **/\r
 306         public void reset()\r
 307         {\r
 308                 if(!_regulate)return;\r
 309                 time0 = (int)System.currentTimeMillis();\r
 310                 angle0 = getTachoCount();\r
 311             basePower = calcPower(_speed);\r
 312         setPower((int)basePower);\r
 313         }\r
 314  \r
 315     /**\r
 316      * Monitors time and tachoCount to regulate speed and stop motor rotation at limit angle\r
 317      */\r
 318         public void run()\r
 319         {\r
 320 //              int limit = 0;\r
 321 \r
 322                 float e0 = 0;\r
 323                 float accel =5f;// deg/sec/ms  was 1.5\r
 324         float power =  0;\r
 325                 int ts = 0;//time to reach speed\r
 326                 while(_keepGoing)\r
 327                 { synchronized(this)\r
 328                 {       \r
 329                         if(_regulate && isMoving()) //regulate speed \r
 330                         {\r
 331                                 int elapsed = (int)System.currentTimeMillis()-time0;\r
 332                                 int angle = getTachoCount()-angle0;\r
 333 //                basePower = calcPower(_speed);\r
 334                                 if(_rampUp)\r
 335                                 {   \r
 336 \r
 337                                         ts = (int)(_speed/accel);\r
 338 //                                      ts = 100;\r
 339                     if(elapsed<ts)// not at speed yet\r
 340                                         {\r
 341  \r
 342                                                 // target distance = a * t * t/ 2 - maintain constant acceleration\r
 343                                                 error = accel*elapsed * elapsed/2000 - (float)Math.abs(angle);\r
 344                                         }\r
 345                                         else  // adjust elapsed time for acceleration time - don't try to catch up\r
 346                                         {\r
 347                      error = ((elapsed - ts/2)* _speed)/1000f - (float)Math.abs(angle);\r
 348                                         }\r
 349                                 }\r
 350                                 else    \r
 351                                         error = (elapsed*_speed/1000f)- (float)Math.abs(angle);\r
 352                                 power = basePower + 0.75f * error;// -0.1f * e0;// magic numbers from experiment\r
 353                                 if(power<0) power = 0;\r
 354                                 e0 = error;\r
 355                                 float smooth = 0.0025f;// another magic number from experiment\r
 356                                 basePower = basePower + smooth*(power-basePower); \r
 357                                 setPower((int)power);\r
 358                         }\r
 359           // stop at rotation limit angle\r
 360                         if(_rotating && _direction*(getTachoCount() - _stopAngle)>0)\r
 361                         {\r
 362                                 if(!_wasRotating)_speed0 = _speed;\r
 363                                 _mode = 3; // stop motor\r
 364                                 _port.controlMotor (0, 3);\r
 365                                 int a = angleAtStop();//returns when motor has stopped\r
 366                                 int remaining = _limitAngle - a;\r
 367                                 if(_direction * remaining >3 ) // not yet done; don't call nudge for less than 3 deg\r
 368                                 {\r
 369                                         if(!_wasRotating)// initial call to rotate(); save state variables\r
 370                                         {\r
 371                                                 _wasRegulating = _regulate;\r
 372                                                 _regulate = true;\r
 373                                                 _speed0 = _speed;\r
 374                                                 setSpeed(300);//was 150\r
 375                                                 _wasRotating = true;\r
 376 //                                              limit = _limitAngle;\r
 377                                         }\r
 378                                 nudge(remaining,a); //another try\r
 379                                 }\r
 380                                 else //rotation complete;  restore state variables\r
 381                                 {       \r
 382                                         setSpeed(_speed0);//restore speed setting\r
 383                                         _mode = 3; // stop motor  maybe redundant\r
 384                                         _port.controlMotor (0, _mode);\r
 385                                         _rotating = false;\r
 386                                         _wasRotating = false;\r
 387                                         _regulate = _wasRegulating;\r
 388                                 }\r
 389                         }\r
 390                 }\r
 391                 Thread.yield();\r
 392                 }       \r
 393         }\r
 394   /**\r
 395    *helper method for run  - stop at limit angle branch\r
 396    */ \r
 397         private void nudge(int remaining,int tachoCount)\r
 398         {\r
 399  \r
 400                 if(remaining>0)_mode = 1;\r
 401                 else _mode = 2; \r
 402             _port.controlMotor(_power, _mode);\r
 403             _direction = 3 - 2*_mode;\r
 404                 _stopAngle = tachoCount + remaining/3;\r
 405                 if(remaining < 3 && remaining > -3) _stopAngle += _direction; //nudge at least 1 deg\r
 406             _rotating = true;\r
 407             _rampUp = false;\r
 408             _regulate = true;\r
 409         }  \r
 410     /**\r
 411      *helper method for run\r
 412      **/\r
 413         int angleAtStop()\r
 414         {\r
 415                 int a0 = getTachoCount();\r
 416                 boolean turning = true;\r
 417                 int a = 0;\r
 418                 while(turning)\r
 419                 {\r
 420                         _port.controlMotor(0,3); // looks redundant, but controlMotor(0,3) fails, rarely.\r
 421                         try{Thread.sleep(20);}// was 10\r
 422                         catch(InterruptedException w){}\r
 423                         a = getTachoCount();\r
 424                         turning = Math.abs(a - a0)>0;\r
 425                         a0 = a;\r
 426                 }\r
 427                 return  a;\r
 428         }\r
 429   }\r
 430  \r
 431   /**\r
 432    *causes run() to exit\r
 433    */\r
 434   public void shutdown(){_keepGoing = false;}\r
 435   \r
 436  \r
 437   /** \r
 438    * turns speed regulation on/off; <br>\r
 439    * Cumulative speed error is within about 1 degree after initial acceleration.\r
 440    * @param  yes is true for speed regulation on\r
 441    */\r
 442   public void regulateSpeed(boolean yes) \r
 443   {\r
 444         _regulate = yes;\r
 445   }\r
 446   \r
 447   /**\r
 448    * enables smoother acceleration.  Motor speed increases gently,  and does not <>\r
 449    * overshoot when regulate Speed is used. \r
 450    * \r
 451    */\r
 452   public void smoothAcceleration(boolean yes) \r
 453   {_noRamp = ! yes;}\r
 454  \r
 455   /**\r
 456    * Sets motor speed , in degrees per second; Up to 900 is posssible with 8 volts.\r
 457    * @param speed value in degrees/sec  \r
 458    */\r
 459   public final void setSpeed (int speed)\r
 460   {\r
 461     _speed = Math.abs(speed);\r
 462      setPower((int)regulator.calcPower(_speed));\r
 463      regulator.reset();\r
 464      _rampUp = false;\r
 465   }\r
 466 \r
 467   /**\r
 468    *sets motor power.  This method is used by the Regulator thread to control motor speed.\r
 469    *Warning:  negative power will cause the motor to run in reverse but without updating the _direction \r
 470    *field which is used by the Regulator thread.  If the speed regulation is enabled, the rusults are \r
 471    *unpredictable. \r
 472    */\r
 473   public synchronized void  setPower(int power)\r
 474   {\r
 475           _power = power;\r
 476           _port.controlMotor (_power, _mode);\r
 477   }\r
 478 \r
 479   /**\r
 480    * Returns the current motor speed in degrees per second\r
 481    */\r
 482   public final int getSpeed()\r
 483   {\r
 484     return _speed;        \r
 485   }\r
 486         public int getMode() {return _mode;}\r
 487         public int getPower() { return _power;}\r
 488         \r
 489   private int overshoot()\r
 490   {\r
 491         return   (int)(5+ _speed*0.060f);//60?\r
 492   }\r
 493 \r
 494   /**\r
 495    * Return the angle that a Motor is rotating to.\r
 496    * \r
 497    * @return angle in degrees\r
 498    */\r
 499   public int getLimitAngle()\r
 500   {\r
 501         return _limitAngle;\r
 502   }\r
 503 \r
 504   /**\r
 505    *returns true when motor is rotating towarad a specified angle\r
 506    */ \r
 507   public boolean isRotating()\r
 508   {\r
 509         return  _rotating;\r
 510   }\r
 511   public boolean isRegulating(){return _regulate;}\r
 512   /**\r
 513    * requred by TimerListener interface\r
 514    */\r
 515   public void timedOut()\r
 516   {\r
 517         int angle = getTachoCount();\r
 518         _actualSpeed = 10*(angle - _lastTacho);\r
 519         _lastTacho = angle;\r
 520     _voltage = Battery.getVoltage();\r
 521   }\r
 522         \r
 523   /** \r
 524    *returns actualSpeed degrees per second,  calculated every 100 ms; negative value means motor is rotating backward\r
 525    */\r
 526   public int getActualSpeed() { return _actualSpeed;}   \r
 527   /**\r
 528    * Returns the tachometer count.\r
 529    * \r
 530    * @return tachometer count in degrees\r
 531    */\r
 532   public int getTachoCount()\r
 533   {\r
 534         return _port.getTachoCount();\r
 535   }\r
 536         \r
 537   /**\r
 538    * Resets the tachometer count to zero.\r
 539    */\r
 540   public void resetTachoCount()\r
 541   {\r
 542         _port.resetTachoCount();\r
 543   }\r
 544 \r
 545   public float getError() {return regulator.error;}\r
 546   public float getBasePower() {return regulator.basePower;}\r
 547 }\r
 548 \r
 549 \r
 550 \r
 551 \r
 552 \r
 553 \r
 554 \r