Package trclib.motor

Class TrcServo

java.lang.Object
trclib.motor.TrcServo
All Implemented Interfaces:
TrcExclusiveSubsystem
Direct Known Subclasses:
FrcServo

public abstract class TrcServo extends Object implements TrcExclusiveSubsystem
This class implements a platform independent servo. Typically, this class is to be extended by a platform dependent servo class and must provide a set of abstract methods. This makes sure the rest of the TrcLib classes can access the servo without any knowledge of platform dependent implementations.
  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    static class 
     

    Nested classes/interfaces inherited from interface trclib.robotcore.TrcExclusiveSubsystem

    TrcExclusiveSubsystem.OwnershipParams
  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    protected static TrcElapsedTimer
     
     
  • Constructor Summary

    Constructors
    Constructor
    Description
    TrcServo(String instanceName)
    Constructor: Creates an instance of the object.
    TrcServo(String instanceName, TrcServo.Params params)
    Constructor: Creates an instance of the object.
  • Method Summary

    Modifier and Type
    Method
    Description
    void
    This method cancels previous servo operation if applicable.
    void
    cancel(String owner)
    This method cancels previous servo operation if applicable.
    void
    This method sets this servo to follow the specified servo.
    abstract double
    This method returns the logical position of the servo.
    double
    This method returns the physical position value of the servo.
    double
    This method returns the last set power value.
    abstract boolean
    This method checks if the servo direction is inverted.
    void
    This method sets the actuator to the next preset position down from the current position.
    void
    This method sets the actuator to the next preset position up from the current position.
    static void
    This method prints the elapsed time info.
    static void
    setElapsedTimerEnabled(boolean enabled)
    This method enables/disables the elapsed timers for performance monitoring.
    abstract void
    setInverted(boolean inverted)
    This method inverts the servo direction.
    abstract void
    setLogicalPosition(double position)
    This method sets the logical position of the servo.
    void
    setLogicalPosRange(double logicalMin, double logicalMax)
    This method sets the logical range of the servo motor.
    void
    setMaxStepRate(double maxStepRate)
    This method sets the maximum stepping rate of the servo.
    void
    setPhysicalPosRange(double physicalMin, double physicalMax)
    This method sets the physical range of the servo motor.
    void
    setPosition(double position)
    This method sets the servo position.
    void
    setPosition(double position, double stepRate)
    This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
    void
    setPosition(double delay, double position, double stepRate, TrcEvent completionEvent)
    This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
    void
    setPosition(double position, double stepRate, TrcEvent completionEvent)
    This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
    void
    setPosition(double delay, double position, TrcEvent completionEvent, double timeout)
    This method sets the servo position.
    void
    setPosition(double position, TrcEvent completionEvent, double timeout)
    This method sets the servo position.
    void
    setPosition(String owner, double delay, double position, double stepRate, TrcEvent completionEvent)
    This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
    void
    setPosition(String owner, double delay, double position, TrcEvent completionEvent, double timeout)
    This method sets the servo position.
    void
    setPosPresets(double tolerance, double... presets)
    This method sets an array of preset positions for the motor.
    void
    setPower(double power)
    This method speed controls the servo with stepping.
    void
    setPower(double delay, double power)
    This method speed controls the servo with stepping.
    void
    setPower(double power, double minPos, double maxPos)
    This method speed controls the servo with stepping.
    void
    setPower(double delay, double power, double minPos, double maxPos)
    This method speed controls the servo with stepping.
    void
    setPower(String owner, double delay, double power)
    This method speed controls the servo with stepping.
    void
    setPower(String owner, double delay, double power, double minPos, double maxPos)
    This method speed controls the servo with stepping.
    void
    setPresetPosition(String owner, double delay, int presetIndex, TrcEvent event, double timeout)
    This method sets the servo to the specified preset position.
    double
    toLogicalPosition(double physicalPosition)
    This method is called to convert a physical position to a logical position.
    double
    toPhysicalPosition(double logicalPosition)
    This method is called to convert a logical position to a physical position.
    This method returns the instance name.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait
  • Field Details

  • Constructor Details

    • TrcServo

      public TrcServo(String instanceName, TrcServo.Params params)
      Constructor: Creates an instance of the object.
      Parameters:
      instanceName - specifies the instance name of the servo.
      params - specifies the parameters of the servo.
    • TrcServo

      public TrcServo(String instanceName)
      Constructor: Creates an instance of the object.
      Parameters:
      instanceName - specifies the instance name of the servo.
  • Method Details

    • setInverted

      public abstract void setInverted(boolean inverted)
      This method inverts the servo direction.
      Parameters:
      inverted - specifies true to invert the servo direction, false otherwise.
    • isInverted

      public abstract boolean isInverted()
      This method checks if the servo direction is inverted.
      Returns:
      true if the servo direction is inverted, false otherwise.
    • setLogicalPosition

      public abstract void setLogicalPosition(double position)
      This method sets the logical position of the servo.
      Parameters:
      position - specifies the logical position of the servo in the range of [0.0, 1.0].
    • getLogicalPosition

      public abstract double getLogicalPosition()
      This method returns the logical position of the servo. In general, servo do not provide real time position feedback. Therefore, it will return the position set by the last setLogicalPosition call.
      Returns:
      logical position of the servo in the range of [0.0, 1.0].
    • toString

      public String toString()
      This method returns the instance name.
      Overrides:
      toString in class Object
      Returns:
      instance name.
    • setLogicalPosRange

      public void setLogicalPosRange(double logicalMin, double logicalMax)
      This method sets the logical range of the servo motor. This is typically used to limit the logical range of the servo to less than the 0.0 to 1.0 range. For example, one may limit the logical range to 0.2 to 0.8.
      Parameters:
      logicalMin - specifies the minimum value of the logical range.
      logicalMax - specifies the maximum value of the logical range.
    • setPhysicalPosRange

      public void setPhysicalPosRange(double physicalMin, double physicalMax)
      This method sets the physical range of the servo motor. This is typically used to set a 180-degree servo to have a range of 0.0 to 180.0 instead of the logical range of 0.0 to 1.0. By default physical range is set to the range of 0.0 to 1.0, same as logical range. The physical range is used for map physical position units to logical position unit between 0.0 to 1.0.
      Parameters:
      physicalMin - specifies the minimum value of the physical range.
      physicalMax - specifies the maximum value of the physical range.
    • setMaxStepRate

      public void setMaxStepRate(double maxStepRate)
      This method sets the maximum stepping rate of the servo. This enables setPower to speed control the servo.
      Parameters:
      maxStepRate - specifies the maximum stepping rate (physicalPos/sec).
    • toLogicalPosition

      public double toLogicalPosition(double physicalPosition)
      This method is called to convert a physical position to a logical position. It will make sure the physical position is within the physical range and scale it to the logical range.
      Parameters:
      physicalPosition - specifies the physical position to be converted
      Returns:
      converted logical position.
    • toPhysicalPosition

      public double toPhysicalPosition(double logicalPosition)
      This method is called to convert a logical position to a physical position. It will make sure the logical position is within the logical range and scale it to the physical range. Note: this method is only callable by classes extending this class.
      Parameters:
      logicalPosition - specifies the logical position to be converted.
      Returns:
      converted physical position.
    • follow

      public void follow(TrcServo servo)
      This method sets this servo to follow the specified servo.
      Parameters:
      servo - specifies the other servo to follow.
    • cancel

      public void cancel(String owner)
      This method cancels previous servo operation if applicable.
      Parameters:
      owner - specifies the owner ID to check if the caller has ownership of the subsystem.
    • cancel

      public void cancel()
      This method cancels previous servo operation if applicable.
    • getPosition

      public double getPosition()
      This method returns the physical position value of the servo. Generally, servo do not provide real time position feedback. Therefore, it will only return the position set by the last setPosition call.
      Returns:
      physical position of the servo, could be in degrees if setPhysicalPosRange is called to set the range in degrees.
    • setPosition

      public void setPosition(String owner, double delay, double position, TrcEvent completionEvent, double timeout)
      This method sets the servo position. By default, the servo maps its physical position the same as its logical position [0.0, 1.0]. However, if setPhysicalPosRange was called, it could map a real world physical range (e.g. [0.0, 180.0] degrees) to the logical range of [0.0, 1.0]. If an event is given, it sets event after the given amount of time has expired.

      Servo operates on logical position. On a 180-degree servo, 0.0 is at 0-degree and 1.0 is at 180-degree. For a 90-degree servo, 0->0deg, 1->90deg. If servo direction is inverted, then 0.0 is at 180-degree and 1.0 is at 0-degree.

      Parameters:
      owner - specifies the owner ID to check if the caller has ownership of the subsystem.
      delay - specifies the delay in seconds before setting the position of the servo, can be zero if no delay.
      position - specifies the physical position of the servo motor. This value may be in degrees if setPhysicalPosRange is called with the degree range.
      completionEvent - specifies an event object to signal when the timeout event has expired.
      timeout - specifies a maximum time value the operation should be completed in seconds.
    • setPosition

      public void setPosition(double delay, double position, TrcEvent completionEvent, double timeout)
      This method sets the servo position. By default, the servo maps its physical position the same as its logical position [0.0, 1.0]. However, if setPhysicalPosRange was called, it could map a real world physical range (e.g. [0.0, 180.0] degrees) to the logical range of [0.0, 1.0]. If an event is given, it sets event after the given amount of time has expired.

      Servo operates on logical position. On a 180-degree servo, 0.0 is at 0-degree and 1.0 is at 180-degree. For a 90-degree servo, 0->0deg, 1->90deg. If servo direction is inverted, then 0.0 is at 180-degree and 1.0 is at 0-degree.

      Parameters:
      delay - specifies the delay in seconds before setting the position of the servo, can be zero if no delay.
      position - specifies the physical position of the servo motor. This value may be in degrees if setPhysicalPosRange is called with the degree range.
      completionEvent - specifies an event object to signal when the timeout event has expired.
      timeout - specifies a maximum time value the operation should be completed in seconds.
    • setPosition

      public void setPosition(double position, TrcEvent completionEvent, double timeout)
      This method sets the servo position. By default, the servo maps its physical position the same as its logical position [0.0, 1.0]. However, if setPhysicalPosRange was called, it could map a real world physical range (e.g. [0.0, 180.0] degrees) to the logical range of [0.0, 1.0]. If an event is given, it sets event after the given amount of time has expired.

      Servo operates on logical position. On a 180-degree servo, 0.0 is at 0-degree and 1.0 is at 180-degree. For a 90-degree servo, 0->0deg, 1->90deg. If servo direction is inverted, then 0.0 is at 180-degree and 1.0 is at 0-degree.

      Parameters:
      position - specifies the physical position of the servo motor. This value may be in degrees if setPhysicalPosRange is called with the degree range.
      completionEvent - specifies an event object to signal when the timeout event has expired.
      timeout - specifies a maximum time value the operation should be completed in seconds.
    • setPosition

      public void setPosition(double position)
      This method sets the servo position. By default, the servo maps its physical position the same as its logical position [0.0, 1.0]. However, if setPhysicalPosRange was called, it could map a real world physical range (e.g. [0.0, 180.0] degrees) to the logical range of [0.0, 1.0]. If an event is given, it sets event after the given amount of time has expired.

      Servo operates on logical position. On a 180-degree servo, 0.0 is at 0-degree and 1.0 is at 180-degree. For a 90-degree servo, 0->0deg, 1->90deg. If servo direction is inverted, then 0.0 is at 180-degree and 1.0 is at 0-degree.

      Parameters:
      position - specifies the physical position of the servo motor. This value may be in degrees if setPhysicalPosRange is called with the degree range.
    • setPosition

      public void setPosition(String owner, double delay, double position, double stepRate, TrcEvent completionEvent)
      This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
      Parameters:
      owner - specifies the owner ID to check if the caller has ownership of the subsystem.
      delay - specifies the delay in seconds before setting the position of the servo, can be zero if no delay.
      position - specifies the target position.
      stepRate - specifies the stepping rate to get there (physicalPos/sec).
      completionEvent - specifies an event object to signal when the servo reaches the position..
    • setPosition

      public void setPosition(double delay, double position, double stepRate, TrcEvent completionEvent)
      This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
      Parameters:
      delay - specifies the delay in seconds before setting the position of the servo, can be zero if no delay.
      position - specifies the target position.
      stepRate - specifies the stepping rate to get there (physicalPos/sec).
      completionEvent - specifies an event object to signal when the servo reaches the position..
    • setPosition

      public void setPosition(double position, double stepRate, TrcEvent completionEvent)
      This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
      Parameters:
      position - specifies the target position.
      stepRate - specifies the stepping rate to get there (physicalPos/sec).
      completionEvent - specifies an event object to signal when the servo reaches the position..
    • setPosition

      public void setPosition(double position, double stepRate)
      This method sets the servo to the specifies position but with the specified step rate in effect controlling the speed to get there.
      Parameters:
      position - specifies the target position.
      stepRate - specifies the stepping rate to get there (physicalPos/sec).
    • setPower

      public void setPower(String owner, double delay, double power, double minPos, double maxPos)
      This method speed controls the servo with stepping.
      Parameters:
      owner - specifies the owner ID to check if the caller has ownership of the subsystem.
      delay - specifies the delay in seconds before setting the power of the servo, can be zero if no delay.
      power - specifies how fast the servo will turn.
      minPos - specifies the minimum position the servo can move to.
      maxPos - specifies the maximum position the servo can move to.
    • setPower

      public void setPower(String owner, double delay, double power)
      This method speed controls the servo with stepping.
      Parameters:
      owner - specifies the owner ID to check if the caller has ownership of the subsystem.
      delay - specifies the delay in seconds before setting the power of the servo, can be zero if no delay.
      power - specifies how fast the servo will turn.
    • setPower

      public void setPower(double delay, double power, double minPos, double maxPos)
      This method speed controls the servo with stepping.
      Parameters:
      delay - specifies the delay in seconds before setting the power of the servo, can be zero if no delay.
      power - specifies how fast the servo will turn.
      minPos - specifies the minimum position the servo can move to.
      maxPos - specifies the maximum position the servo can move to.
    • setPower

      public void setPower(double delay, double power)
      This method speed controls the servo with stepping.
      Parameters:
      delay - specifies the delay in seconds before setting the power of the servo, can be zero if no delay.
      power - specifies how fast the servo will turn.
    • setPower

      public void setPower(double power, double minPos, double maxPos)
      This method speed controls the servo with stepping.
      Parameters:
      power - specifies how fast the servo will turn.
      minPos - specifies the minimum position the servo can move to.
      maxPos - specifies the maximum position the servo can move to.
    • setPower

      public void setPower(double power)
      This method speed controls the servo with stepping.
      Parameters:
      power - specifies how fast the servo will turn.
    • getPower

      public double getPower()
      This method returns the last set power value.
      Returns:
      last power set to the motor.
    • setPosPresets

      public void setPosPresets(double tolerance, double... presets)
      This method sets an array of preset positions for the motor.
      Parameters:
      tolerance - specifies the preset tolerance.
      presets - specifies an array of preset positions in scaled unit.
    • setPresetPosition

      public void setPresetPosition(String owner, double delay, int presetIndex, TrcEvent event, double timeout)
      This method sets the servo to the specified preset position.
      Parameters:
      owner - specifies the owner ID to check if the caller has ownership of the subsystem.
      delay - specifies delay time in seconds before setting position, can be zero if no delay.
      presetIndex - specifies the index to the preset position array.
      event - specifies the event to signal when done, can be null if not provided.
      timeout - specifies a timeout value in seconds. If the operation is not completed without the specified timeout, the operation will be canceled and the event will be signaled. If no timeout is specified, it should be set to zero.
    • presetPositionUp

      public void presetPositionUp(String owner)
      This method sets the actuator to the next preset position up from the current position.
      Parameters:
      owner - specifies the owner ID that will acquire ownership before setting the preset position and will automatically release ownership when the actuator movement is completed, can be null if no ownership is required.
    • presetPositionDown

      public void presetPositionDown(String owner)
      This method sets the actuator to the next preset position down from the current position.
      Parameters:
      owner - specifies the owner ID that will acquire ownership before setting the preset position and will automatically release ownership when the actuator movement is coompleted, can be null if no ownership is required.
    • setElapsedTimerEnabled

      public static void setElapsedTimerEnabled(boolean enabled)
      This method enables/disables the elapsed timers for performance monitoring.
      Parameters:
      enabled - specifies true to enable elapsed timers, false to disable.
    • printElapsedTime

      public static void printElapsedTime(TrcDbgTrace msgTracer)
      This method prints the elapsed time info.
      Parameters:
      msgTracer - specifies the tracer to be used to print the info.