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