motor.java

专业汽车级嵌入式操作系统OSEK的源代码

package lejos.nxt;
import lejos.nxt.*;
import lejos.util.*;




/**
 * Abstraction for a NXT motor. Three instances of <code>Motor</code>
 * are available: <code>Motor.A</code>, <code>Motor.B</code>
 * and <code>Motor.C</code>. To control each motor use
 * methods <code>forward, backward, reverseDirection, stop</code>
 * and <code>flt</code>. To set each motor's speed, use
 * <code>setSpeed.  Speed is in degrees per second. </code>.\
 * Methods that use the tachometer:  regulateSpeed, rotate, rotateTo <br>
 * Motor has 2 modes : speedRegulation and smoothAcceleration. These are initially enabled. <>
 * They can be switched off/on by the methods regulateSpeed() and smoothAcceleration().
 * The actual maximum speed of the motor depends on battery voltage and load.. 
 * Speed regulation fails if the target speed exceeds the capability of the motor.
 * 
 * <p>
 * Example:<p>
 * <code><pre>
 *   Motor.A.setSpeed(720);// 2 RPM
 *   Motor.C.setSpeed(7200);
 *   Motor.A.forward();
 *   Motor.C.forward();
 *   Thread.sleep (1000);
 *   Motor.A.stop();
 *   Motor.C.stop();
 *   Motor.A.regulateSpeed(true);
 *   Motor.A.rotateTo( 360);
 *   Motor.A.rotate(-720,true);
 *   while(Motor.A.isRotating();
 *   int angle = Motor.A.getTachoCount(); // should be -360
 * </pre></code>
 * @author Roger Glassey revised 22 March 2007
 */
public class Motor extends BasicMotor implements TimerListener
{
  private TachoMotorPort _port;
  private int _speed = 360;
  private int _speed0 = 360;
  // used for speed regulation
  private boolean _keepGoing = true;// for regulator
  /**
   * Initially true; changed only by regulateSpeed(),<br>
   * used by Regulator, updteState, reset*
   */
  private boolean _regulate = true;
  
  public Regulator regulator = new Regulator();
  private Timer timer = new Timer(100,this);
  // used for control of angle of rotation
  private int _direction = 1; // +1 is forward ; used by rotate();
  private int _limitAngle;
  private int _stopAngle;
  private boolean _rotating = false;
  private boolean _wasRotating = false;
  private boolean _rampUp = true;
  private int _lastTacho = 0;
  private int _actualSpeed;


   /** initialized to be false(ramping enabled); changed only by smoothAcceleration
   */
  private boolean _noRamp = false;
 
  /**
   * Motor A.
   */
  public static final Motor A = new Motor (MotorPort.A);
  
  /**
   * Motor B.
   */
  public static final Motor B = new Motor (MotorPort.B);
  
  /**
   * Motor C.
   */
  public static final Motor C = new Motor (MotorPort.C);
  
  public Motor (MotorPort port)
  {
    _port = port;
    regulator.start();
    regulator.setDaemon(true);
    timer.start();
  }
   public int getStopAngle() { return (int)_stopAngle;}
   
  /** 
   *calls controlMotor, startRegating;  updates _direction, _rotating, _wasRotating
   */
  void updateState()
  {
  	synchronized(regulator)
  	{
  		if(_wasRotating)setSpeed(_speed0);
	  	_rotating = false;
		_wasRotating = false;

	  	if(_mode>2) // stop or float
	  	{
	  		_port.controlMotor(0, _mode);
	  		return;
	  	}
		 _port.controlMotor(_power, _mode);
	
	   	if(_regulate)
	   	{
	   	  regulator.reset();
	   	  _rampUp = true;
	   	}
	   	 _direction = 3 - 2*_mode;
  	}
  }

  /**
   * @return true iff the motor is currently in motion.
   */
  public final boolean isMoving()
  {
    return (_mode == 1 || _mode == 2 || _rotating);	  
  }
  
  
  /**
   * causes motor to rotate through angle. <br>
   * @param  angle through which the motor will rotate
   */
  public void rotate(int angle)
  {
	rotateTo(getTachoCount()+angle);
  }

  /**
   * causes motor to rotate through angle; <br>
   * iff immediateReturn is true, method returns immediately and the motor stops by itself <br>
   * When the angle is reached, the method isRotating() returns false;
   * @param  angle through which the motor will rotate
   * @param immediateReturn iff true, method returns immediately, thus allowing monitoring of sensors in the calling thread. 
   */
   public void rotate(int angle, boolean immediateReturn)
   {
		int t = getTachoCount();
		rotateTo(t+angle,immediateReturn);
	}

  /**
   * causes motor to rotate to limitAngle;  <br>
   * Then getTachoCount should be within +- 2 degrees of the limit angle when the method returns
   * @param  limitAngle to which the motor will rotate
   */
  public void rotateTo(int limitAngle)
  {
  	rotateTo(limitAngle,false);
  }

  /**
   * causes motor to rotate to limitAngle; <br>
   * if immediateReturn is true, method returns immediately and the motor stops by itself <br>
   * Then getTachoCount should be within +- 2 degrees if the limit angle
   * When the angle is reached, the method isRotating() returns false;
   * @param  limitAngle to which the motor will rotate, and then stop. 
   * @param immediateReturn iff true, method returns immediately, thus allowing monitoring of sensors in the calling thread. 
    */
  public void rotateTo(int limitAngle,boolean immediateReturn)
  {
	_stopAngle = limitAngle;
	if(limitAngle > getTachoCount()) _mode = 1;
	else _mode = 2;
    _port.controlMotor(_power, _mode);
    _direction = 3 - 2*_mode;
   	if(_regulate) regulator.reset();
	if(!_wasRotating)
	{
	  _stopAngle -= _direction * overshoot();
	  _limitAngle = limitAngle;
	}
	_rotating = true; // rotating to a limit
	_rampUp = !_noRamp && Math.abs(_stopAngle-getTachoCount())>40 && _speed>200;  //no ramp for small angles

	if(immediateReturn)return;
	while(isMoving()) Thread.yield();
  }

/**
 *inner class to regulate speed; also stop motor at desired rotation angle
 **/
 private class Regulator extends Thread
  {
  	/**
  	 *tachoCount when regulating started
  	 */
  	int angle0 = 0;
    /**
     * set by reset, used  to regulate  motor speed
     */ 
  	float basePower = 0;
    /**
     * time regulating started
     */
  	int time0 = 0;

    /**
     * helper method - used by reset and setSpeed()
     */
 	int calcPower(int speed)
 	{   
		float pwr = 100 - 7.4f*Battery.getVoltage()+0.065f*speed;// no-load motor
 		if(pwr<0) return 0;
 		if(pwr>100)return 100;
 		else return (int)pwr;
 	}
 
    /**
     * called by forward() backward() and reverseDirection() <br>
     * resets parameters for speed regulation
     **/
 	public void reset()
	{
		if(!_regulate)return;
 		time0 = (int)System.currentTimeMillis();
 		angle0 = getTachoCount();
 	    basePower = calcPower(_speed);
    	setPower((int)basePower);
 	}
 
    /**
     * Monitors time and tachoCount to regulate speed and stop motor rotation at limit angle
     */
  	public void run()
  	{
  		int limit = 0;
  		float error = 0;
	  	float e0 = 0;
	  	float accel = 1.5f;// deg/sec/ms  was 1.5
	  	int td = 100;
	  	float ts = 0;  //time to stabilize
	  	boolean wasRegulating = true;
	  	while(_keepGoing)
	  	{ synchronized(this)
	  	{	
	  		if(_regulate && isMoving()) //regulate speed 
	  		{
	  			int elapsed = (int)System.currentTimeMillis()-time0;
	  			int angle = getTachoCount()-angle0;
	  			if(_rampUp)
	  			{   
	  				ts = _speed/accel;
					if (elapsed +td<ts)// not yet up to speed
		  			{
		  				elapsed +=td;
		  				// target distance = a * t * t/ 2 - maintain constant acceleration
		 				error = accel*elapsed * elapsed/2000 - (float)Math.abs(angle);
		 				basePower = calcPower((int)Math.max(elapsed*accel,400));
		  			}
		  			else  // adjust elapsed time for acceleration time - don't try to catch up
		  			{
		  				error = ((elapsed + td-ts/2)* _speed)/1000f - (float)Math.abs(angle);
		  			}
	  			}
		  		else 	
					error = (elapsed*_speed/1000f)- (float)Math.abs(angle);
	  			float power = basePower + 2 * error -1 * e0;// magic numbers from experiment
	  			if(power<0) power = 0;
	  			e0 = error;
	  			float smooth = 0.0015f;// another magic number from experiment
	  			basePower = basePower + smooth*(power-basePower); 
	  			setPower((int)power);
	  		}
	  // stop at rotation limit angle
//	  		int tc = getTachoCount();
			if(_rotating && _direction*(getTachoCount() - _stopAngle)>-1)
			{
				_mode = 3; // stop motor
				_port.controlMotor (0, 3);
				int a = angleAtStop();//returns when motor has stopped
				int remaining = _limitAngle - a;
				if(_direction * remaining >0 ) // not yet done
				{
					if(!_wasRotating)// initial call to rotate(); save state variables
					{
						_speed0 = _speed;
						setSpeed(150);
						_wasRotating = true;
						wasRegulating = _regulate;
						_regulate = true;
						limit = _limitAngle;
					}
				 	rotateTo(limit - remaining/3,true); //another try
				}
				else //rotation complete;  restore state variables
				{
					setSpeed(_speed0);//restore speed setting
					_mode = 3; // stop motor  maybe redundant
					_port.controlMotor (0, _mode);
					_rotating = false;
					_wasRotating = false;
					_regulate = wasRegulating;
				}
	  		}
	  	}
	  	Thread.yield();
	  	}	
  	}
  
    /**
     *helper method for run
     **/
  	int angleAtStop()
  	{
		int a0 = getTachoCount();
		boolean turning = true;
		int a = 0;
		while(turning)
		{
	  		_port.controlMotor(0,3); // looks redundant, but controlMotor(0,3) fails, rarely.
			try{Thread.sleep(20);}// was 10
			catch(InterruptedException w){}
			a = getTachoCount();
			turning = Math.abs(a - a0)>0;
			a0 = a;
		}
		return	a;
	}
  }
 
  /**
   *causes run() to exit
   */
  public void shutdown(){_keepGoing = false;}
  
 
  /** 
   * turns speed regulation on/off; <br>
   * Cumulative speed error is within about 1 degree after initial acceleration.
   * @param  yes is true for speed regulation on
   */
  public void regulateSpeed(boolean yes) 
  {
	_regulate = yes;
  }
  
  /**
   * enables smoother acceleration.  Motor speed increases gently,  and does not <>
   * overshoot when regulate Speed is used. 
   * 
   */
  public void smoothAcceleration(boolean yes) 
  {_noRamp = ! yes;}
 
  /**
   * Sets motor speed , in degrees per second; Up to 900 is posssible with 8 volts.
   * @param speed value in degrees/sec  
   */
  public final void setSpeed (int speed)
  {
    _speed = Math.abs(speed);
     setPower((int)regulator.calcPower(_speed));
     regulator.reset();
     _rampUp = false;
  }

  /**
   *sets motor power.  This method is used by the Regulator thread to control motor speed.
   *Warning:  negative power will cause the motor to run in reverse but without updating the _direction 
   *field which is used by the Regulator thread.  If the speed regulation is enabled, the rusults are 
   *unpredictable. 
   */
  public synchronized void  setPower(int power)
  {
	  _power = power;
	  _port.controlMotor (_power, _mode);
  }

  /**
   * Returns the current motor speed in degrees per second
   */
  public final int getSpeed()
  {
    return _speed;	  
  }
	public int getMode() {return _mode;}
	public int getPower() { return _power;}
	
  private int overshoot()
  {
	return   (int)( _speed*0.060f);
  }

  /**
   * Return the angle that a Motor is rotating to.
   * 
   * @return angle in degrees
   */
  public int getLimitAngle()
  {
	return _limitAngle;
  }

  /**
   *returns true when motor is rotating towarad a specified angle
   */ 
  public final boolean isRotating()
  {
  	return  _rotating;
  }

  /**
   * requred by TimerListener interface
   */
  public void timedOut()
  {
	int angle = getTachoCount();
	_actualSpeed = 10*(angle - _lastTacho);
	_lastTacho = angle;
  }
	
  /** 
   *returns actualSpeed degrees per second,  calculated every 100 ms; negative value means motor is rotating backward
   */
  public int getActualSpeed() { return _actualSpeed;}
	
  /**
   * Returns the tachometer count.
   * 
   * @return tachometer count in degrees
   */
  public int getTachoCount()
  {
	return _port.getTachoCount();
  }
	
  /**
   * Resets the tachometer count to zero.
   */
  public void resetTachoCount()
  {
	_port.resetTachoCount();
  }
}