Package trclib.subsystem

Class TrcDifferentialServoWrist

java.lang.Object

trclib.subsystem.TrcDifferentialServoWrist

All Implemented Interfaces:

TrcExclusiveSubsystem

public class TrcDifferentialServoWrist
extends Object
implements TrcExclusiveSubsystem

This class implements a platform independent Differential Servo Wrist Subsystem. A Differential Servo Wrist consists of two servos controlling two degrees of freedom. The wrist can tilt as well as rotate. When the two servos turn in the same direction on the mounted axis, the wrist tilts up and down. When the two servos turn in opposite directions, the wrist rotates.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
private static class	TrcDifferentialServoWrist.ActionParams	This class encapsulates all the parameters required to perform the intake action.
private static enum	TrcDifferentialServoWrist.Operation	Specifies the operation types.
static class	TrcDifferentialServoWrist.Params	This class contains all the parameters of the Differential Servo Wrist.

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

TrcExclusiveSubsystem.OwnershipParams

Field Summary

Fields

Modifier and Type	Field	Description
private TrcDifferentialServoWrist.ActionParams	actionParams
private final String	instanceName
private final TrcPresets	rotatePosPresets
private double	rotatePower
private final TrcPresets	tiltPosPresets
private double	tiltPower
private final TrcTimer	timer
final TrcDbgTrace	tracer
private final TrcDifferentialServoWrist.Params	wristParams

Constructor Summary

Constructors

Constructor	Description
TrcDifferentialServoWrist(String instanceName, TrcDifferentialServoWrist.Params params)	Constructor: Create an instance of the object.

Method Summary

Modifier and Type	Method	Description
private void	actionTimedOut(Object context)	This method is called when the action has timed out.
void	cancel()	This method cancels previous wrist operation if applicable.
void	cancel(String owner)	This method cancels previous wrist operation if applicable.
private void	finish(boolean completed)	This method is called to finish the operation and to clean up.
double	getRotatePosition()	This method returns the physical rotate position value of the wrist.
private double	getRotatePosition(double servo1Pos, double servo2Pos)
double	getRotatePower()	This method returns the last set rotate power value.
private double	getServo1Position(double tiltPos, double rotatePos)
private double	getServo2Position(double tiltPos, double rotatePos)
double	getTiltPosition()	This method returns the physical tilt position value of the wrist.
private double	getTiltPosition(double servo1Pos, double servo2Pos)
double	getTiltPower()	This method returns the last set tilt power value.
private void	performAction(Object context)	This method performs the action.
void	rotatePresetPositionDown(String owner)	This method sets the wrist to the next rotate preset position down from the current position.
void	rotatePresetPositionUp(String owner)	This method sets the wrist to the next rotate preset position up from the current position.
private void	setNextRotatePresetPosition(String owner, boolean presetUp)	This method sets the wrist to the next rotate preset position up or down from the current position.
private void	setNextTiltPresetPosition(String owner, boolean presetUp)	This method sets the wrist to the next tilt preset position up or down from the current position.
void	setPosition(double tiltPos, double rotatePos)	This method sets the tilt position of the wrist.
void	setPosition(double delay, double tiltPos, double rotatePos, TrcEvent completionEvent, double timeout)	This method sets the tilt position of the wrist.
void	setPosition(double tiltPos, double rotatePos, TrcEvent completionEvent, double timeout)	This method sets the tilt position of the wrist.
void	setPosition(String owner, double delay, double tiltPos, double rotatePos, TrcEvent completionEvent, double timeout)	This method sets the tilt position of the wrist.
void	setPower(double tiltPower, double rotatePower)	This method sets the wrist tilting power.
void	setPower(double delay, double tiltPower, double rotatePower)	This method sets the wrist tilting power.
void	setPower(String owner, double delay, double tiltPower, double rotatePower)	This method sets the wrist tilting power.
void	setRotatePresetPosition(String owner, double delay, int presetIndex, TrcEvent event, double timeout)	This method sets the wrist to the specified rotate preset position.
void	setTiltPresetPosition(String owner, double delay, int presetIndex, TrcEvent event, double timeout)	This method sets the wrist to the specified tilt preset position.
void	tiltPresetPositionDown(String owner)	This method sets the wrist to the next tilt preset position down from the current position.
void	tiltPresetPositionUp(String owner)	This method sets the wrist to the next tilt preset position up from the current position.
String	toString()	This method returns the instance name.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait

Methods inherited from interface trclib.robotcore.TrcExclusiveSubsystem

acquireExclusiveAccess, acquireOwnership, cancelExclusiveAccess, getCurrentOwner, hasOwnership, releaseExclusiveAccess, releaseOwnership, validateOwnership

Field Details

tracer

public final TrcDbgTrace tracer

instanceName

private final String instanceName

wristParams

private final TrcDifferentialServoWrist.Params wristParams

timer

private final TrcTimer timer

tiltPosPresets

private final TrcPresets tiltPosPresets

rotatePosPresets

private final TrcPresets rotatePosPresets

actionParams

private TrcDifferentialServoWrist.ActionParams actionParams

tiltPower

private double tiltPower

rotatePower

private double rotatePower

Constructor Details

TrcDifferentialServoWrist

public TrcDifferentialServoWrist(String instanceName,
 TrcDifferentialServoWrist.Params params)

Constructor: Create an instance of the object.

Parameters:

instanceName - specifies the instance name.

params - specifies the wrist parameters.

Method Details

toString

public String toString()

This method returns the instance name.

Overrides:

toString in class Object

Returns:

instance name.

finish

private void finish(boolean completed)

This method is called to finish the operation and to clean up. It can be called either at the end of timeout or when the operation is done and signal the caller for completion. It can also be called if the caller explicitly cancel the operation in which case the event will be set to canceled.

Parameters:

completed - specifies true if the operation is completed, false if canceled.

cancel

public void cancel(String owner)

This method cancels previous wrist 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 wrist operation if applicable.

performAction

private void performAction(Object context)

This method performs the action.

Parameters:

context - specifies the action parameters.

actionTimedOut

private void actionTimedOut(Object context)

This method is called when the action has timed out. Servo doesn't have feedback sensors. Timeout is used as the way for the caller to wait for completion. Therefore, a timeout here means the operation has completed.

Parameters:

context - not used.

setPower

public void setPower(String owner,
 double delay,
 double tiltPower,
 double rotatePower)

This method sets the wrist tilting power.

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 wrist, can be zero if no delay.

tiltPower - specifies how fast the wrist will tilt.

rotatePower - specifies how fast the wrist will rotate.

setPower

public void setPower(double delay,
 double tiltPower,
 double rotatePower)

This method sets the wrist tilting power.

Parameters:

delay - specifies the delay in seconds before setting the power of the wrist, can be zero if no delay.

tiltPower - specifies how fast the wrist will tilt.

rotatePower - specifies how fast the wrist will rotate.

setPower

public void setPower(double tiltPower,
 double rotatePower)

This method sets the wrist tilting power.

Parameters:

tiltPower - specifies how fast the wrist will tilt.

rotatePower - specifies how fast the wrist will rotate.

getTiltPower

public double getTiltPower()

This method returns the last set tilt power value.

Returns:

last tilt power set to the wrist.

getRotatePower

public double getRotatePower()

This method returns the last set rotate power value.

Returns:

last rotate power set to the wrist.

setPosition

public void setPosition(String owner,
 double delay,
 double tiltPos,
 double rotatePos,
 TrcEvent completionEvent,
 double timeout)

This method sets the tilt position of the wrist. 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 tilt position of the wrist, can be zero if no delay.

tiltPos - specifies the physical tilt position of the wrist. This value may be in degrees if setPhysicalPosRange is called with the degree range.

rotatePos - specifies the physical rotate position of the wrist. 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 tiltPos,
 double rotatePos,
 TrcEvent completionEvent,
 double timeout)

This method sets the tilt position of the wrist. 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 tilt position of the wrist, can be zero if no delay.

tiltPos - specifies the physical tilt position of the wrist. This value may be in degrees if setPhysicalPosRange is called with the degree range.

rotatePos - specifies the physical rotate position of the wrist. 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 tiltPos,
 double rotatePos,
 TrcEvent completionEvent,
 double timeout)

This method sets the tilt position of the wrist. 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:

tiltPos - specifies the physical tilt position of the wrist. This value may be in degrees if setPhysicalPosRange is called with the degree range.

rotatePos - specifies the physical rotate position of the wrist. 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 tiltPos,
 double rotatePos)

This method sets the tilt position of the wrist. 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:

tiltPos - specifies the physical tilt position of the wrist. This value may be in degrees if setPhysicalPosRange is called with the degree range.

rotatePos - specifies the physical rotate position of the wrist. This value may be in degrees if setPhysicalPosRange is called with the degree range.

getTiltPosition

public double getTiltPosition()

This method returns the physical tilt position value of the wrist. Generally, servo do not provide real time position feedback. Therefore, it will only return the position set by the last setPosition call.

Returns:

physical tilt position of the wrist, could be in degrees if setPhysicalPosRange is called to set the range in degrees.

getRotatePosition

public double getRotatePosition()

This method returns the physical rotate position value of the wrist. Generally, servo do not provide real time position feedback. Therefore, it will only return the position set by the last setPosition call.

Returns:

physical rotate position of the wrist, could be in degrees if setPhysicalPosRange is called to set the range in degrees.

getTiltPosition

private double getTiltPosition(double servo1Pos,
 double servo2Pos)

getRotatePosition

private double getRotatePosition(double servo1Pos,
 double servo2Pos)

getServo1Position

private double getServo1Position(double tiltPos,
 double rotatePos)

getServo2Position

private double getServo2Position(double tiltPos,
 double rotatePos)

setTiltPresetPosition

public void setTiltPresetPosition(String owner,
 double delay,
 int presetIndex,
 TrcEvent event,
 double timeout)

This method sets the wrist to the specified tilt 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 target is reached, can be null if not provided.

timeout - specifies a maximum time value the operation should be completed in seconds.

setRotatePresetPosition

public void setRotatePresetPosition(String owner,
 double delay,
 int presetIndex,
 TrcEvent event,
 double timeout)

This method sets the wrist to the specified rotate 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 target is reached, can be null if not provided.

timeout - specifies a maximum time value the operation should be completed in seconds.

setNextTiltPresetPosition

private void setNextTiltPresetPosition(String owner,
 boolean presetUp)

This method sets the wrist to the next tilt preset position up or 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 motor movement is completed, can be null if no ownership is required.

presetUp - specifies true to move to next preset up, false to move to next preset down.

tiltPresetPositionUp

public void tiltPresetPositionUp(String owner)

This method sets the wrist to the next tilt 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 motor movement is completed, can be null if no ownership is required.

tiltPresetPositionDown

public void tiltPresetPositionDown(String owner)

This method sets the wrist to the next tilt 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 motor movement is completed, can be null if no ownership is required.

setNextRotatePresetPosition

private void setNextRotatePresetPosition(String owner,
 boolean presetUp)

This method sets the wrist to the next rotate preset position up or 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 motor movement is completed, can be null if no ownership is required.

presetUp - specifies true to move to next preset up, false to move to next preset down.

rotatePresetPositionUp

public void rotatePresetPositionUp(String owner)

This method sets the wrist to the next rotate 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 motor movement is completed, can be null if no ownership is required.

rotatePresetPositionDown

public void rotatePresetPositionDown(String owner)

This method sets the wrist to the next rotate 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 motor movement is completed, can be null if no ownership is required.