From 7c20f1d1f3e7c406a47cf7218d51919b57defbbc Mon Sep 17 00:00:00 2001 From: Titan Robotics Club Date: Mon, 5 May 2025 00:19:30 -0700 Subject: [PATCH] Added ServoExtender sample. --- .../src/main/java/teamcode/FtcTeleOp.java | 16 ++ TeamCode/src/main/java/teamcode/Robot.java | 8 +- .../src/main/java/teamcode/RobotParams.java | 1 + .../teamcode/subsystems/ServoExtender.java | 217 ++++++++++++++++++ 4 files changed, 241 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/teamcode/subsystems/ServoExtender.java diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index 3ceff98..90c76be 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -65,6 +65,7 @@ public class FtcTeleOp extends FtcOpMode private double prevWristRotatePower = 0.0; private boolean shooterOn = false; private double prevShooterVelocity = 0.0; + private boolean extenderExtended = false; // // Implements FtcOpMode abstract method. @@ -589,6 +590,21 @@ public class FtcTeleOp extends FtcOpMode } } } + else if (robot.servoExtender != null) + { + if (pressed) + { + extenderExtended = !extenderExtended; + if (extenderExtended) + { + robot.servoExtender.extend(); + } + else + { + robot.servoExtender.retract(); + } + } + } break; case B: diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 5b1bda6..35ef66f 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -37,6 +37,7 @@ import teamcode.subsystems.Intake; import teamcode.subsystems.LEDIndicator; import teamcode.subsystems.RobotBase; import teamcode.subsystems.RumbleIndicator; +import teamcode.subsystems.ServoExtender; import teamcode.subsystems.ServoWrist; import teamcode.subsystems.Shooter; import teamcode.subsystems.Turret; @@ -53,7 +54,6 @@ import trclib.subsystem.TrcIntake; import trclib.subsystem.TrcServoClaw; import trclib.subsystem.TrcShooter; import trclib.subsystem.TrcSubsystem; -import trclib.timer.TrcTimer; /** * This class creates the robot object that consists of sensors, indicators, drive base and all the subsystems. @@ -88,6 +88,7 @@ public class Robot public TrcServoClaw claw; public TrcServo servoWrist; public DiffyServoWrist diffyServoWrist; + public ServoExtender servoExtender; // Autotasks. /** @@ -180,6 +181,11 @@ public class Robot { diffyServoWrist = new DiffyServoWrist(); } + + if (RobotParams.Preferences.useServoExtender) + { + servoExtender = new ServoExtender(); + } // Zero calibrate all subsystems only in Auto or if TeleOp is run standalone without prior Auto. // There is no reason to zero calibrate again if Auto was run right before TeleOp. if (runMode == TrcRobot.RunMode.AUTO_MODE || FtcAuto.autoChoices.alliance == null) diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 3d84ea9..269fe69 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -83,6 +83,7 @@ public class RobotParams public static final boolean useClaw = false; public static final boolean useServoWrist = false; public static final boolean useDiffyServoWrist = false; + public static final boolean useServoExtender = false; // Tuning public static final boolean tuneColorBlobVision = false; public static final boolean tuneDriveBase = false; diff --git a/TeamCode/src/main/java/teamcode/subsystems/ServoExtender.java b/TeamCode/src/main/java/teamcode/subsystems/ServoExtender.java new file mode 100644 index 0000000..bd392b0 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/ServoExtender.java @@ -0,0 +1,217 @@ +/* + * Copyright (c) 2025 Titan Robotics Club (http://www.titanrobotics.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package teamcode.subsystems; + +import ftclib.driverio.FtcDashboard; +import ftclib.motor.FtcServoActuator; +import trclib.motor.TrcServo; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcSubsystem; + +/** + * This class creates the Servo Extender subsystem. This implementation is a linear extender driven by two servos + * to either extend or retract the extender. + * There are many possible implementations by setting different parameters. + * Please refer to the TrcLib documentation (...) for details. + */ +public class ServoExtender extends TrcSubsystem +{ + public static class Params + { + public static final String SUBSYSTEM_NAME = "ServoExtender"; + public static final boolean NEED_ZERO_CAL = false; + + public static final String PRIMARY_SERVO_NAME = Params.SUBSYSTEM_NAME + ".primary"; + public static final boolean PRIMARY_SERVO_INVERTED = false; + public static final String FOLLOWER_SERVO_NAME = Params.SUBSYSTEM_NAME + ".follower"; + public static final boolean FOLLOWER_SERVO_INVERTED = false; + + public static final double POS_RETRACT = 0.1; + public static final double POS_EXTEND = 0.8; + } //class Params + + private final FtcDashboard dashboard; + public final TrcServo servoExtender; + + /** + * Constructor: Creates an instance of the object. + */ + public ServoExtender() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcServoActuator.Params extenderParams = new FtcServoActuator.Params() + .setPrimaryServo(Params.PRIMARY_SERVO_NAME, Params.PRIMARY_SERVO_INVERTED) + .setFollowerServo(Params.FOLLOWER_SERVO_NAME, Params.FOLLOWER_SERVO_INVERTED); + + servoExtender = new FtcServoActuator(extenderParams).getServo(); + } //ServoExtender + + /** + * This method checks if the extender is extended. + * + * @return true if extended, false otherwise. + */ + public boolean isExtended() + { + return servoExtender.getPosition() == Params.POS_EXTEND; + } //isExtended + + /** + * This method sets the extender to extended position. + * + * @param owner specifies the owner ID to check if the caller has ownership of the subsystem. + * @param delay specifies the delay in seconds before setting the position of the servo, can be zero if no delay. + * @param completionEvent specifies an event object to signal when the timeout event has expired. + * @param timeout specifies a maximum time value the operation should be completed in seconds. + */ + public void extend(String owner, double delay, TrcEvent completionEvent, double timeout) + { + servoExtender.setPosition(owner, delay, Params.POS_EXTEND, completionEvent, timeout); + } //extend + + /** + * This method sets the extender to extended position. + * + * @param delay specifies the delay in seconds before setting the position of the servo, can be zero if no delay. + * @param completionEvent specifies an event object to signal when the timeout event has expired. + * @param timeout specifies a maximum time value the operation should be completed in seconds. + */ + public void extend(double delay, TrcEvent completionEvent, double timeout) + { + servoExtender.setPosition(null, delay, Params.POS_EXTEND, completionEvent, timeout); + } //extend + + /** + * This method sets the extender to extended position. + * + * @param completionEvent specifies an event object to signal when the timeout event has expired. + * @param timeout specifies a maximum time value the operation should be completed in seconds. + */ + public void extend(TrcEvent completionEvent, double timeout) + { + servoExtender.setPosition(null, 0.0, Params.POS_EXTEND, completionEvent, timeout); + } //extend + + /** + * This method sets the extender to extended position. + */ + public void extend() + { + servoExtender.setPosition(null, 0.0, Params.POS_EXTEND, null, 0.0); + } //extend + + /** + * This method sets the extender to retracted position. + * + * @param owner specifies the owner ID to check if the caller has ownership of the subsystem. + * @param delay specifies the delay in seconds before setting the position of the servo, can be zero if no delay. + * @param completionEvent specifies an event object to signal when the timeout event has expired. + * @param timeout specifies a maximum time value the operation should be completed in seconds. + */ + public void retract(String owner, double delay, TrcEvent completionEvent, double timeout) + { + servoExtender.setPosition(owner, delay, Params.POS_RETRACT, completionEvent, timeout); + } //retract + + /** + * This method sets the extender to retracted position. + * + * @param delay specifies the delay in seconds before setting the position of the servo, can be zero if no delay. + * @param completionEvent specifies an event object to signal when the timeout event has expired. + * @param timeout specifies a maximum time value the operation should be completed in seconds. + */ + public void retract(double delay, TrcEvent completionEvent, double timeout) + { + servoExtender.setPosition(null, delay, Params.POS_RETRACT, completionEvent, timeout); + } //retract + + /** + * This method sets the extender to retracted position. + * + * @param completionEvent specifies an event object to signal when the timeout event has expired. + * @param timeout specifies a maximum time value the operation should be completed in seconds. + */ + public void retract(TrcEvent completionEvent, double timeout) + { + servoExtender.setPosition(null, 0.0, Params.POS_RETRACT, completionEvent, timeout); + } //retract + + /** + * This method sets the extender to retracted position. + */ + public void retract() + { + servoExtender.setPosition(null, 0.0, Params.POS_RETRACT, null, 0.0); + } //retract + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + servoExtender.cancel(); + } //cancel + + /** + * This method starts zero calibrate of the subsystem. + * + * @param owner specifies the owner ID to to claim subsystem ownership, can be null if ownership not required. + * @param event specifies an event to signal when zero calibration is done, can be null if not provided. + */ + @Override + public void zeroCalibrate(String owner, TrcEvent event) + { + // No zero calibration needed. + } //zeroCalibrate + + /** + * This method resets the subsystem state. Typically, this is used to retract the subsystem for turtle mode. + */ + @Override + public void resetState() + { + servoExtender.setPosition(Params.POS_RETRACT); + } //resetState + + /** + * This method update the dashboard with the subsystem status. + * + * @param lineNum specifies the starting line number to print the subsystem status. + * @return updated line number for the next subsystem to print. + */ + @Override + public int updateStatus(int lineNum) + { + dashboard.displayPrintf( + lineNum++, "%s: pos=%.3f, extended=%s", Params.SUBSYSTEM_NAME, servoExtender.getPosition(), isExtended()); + return lineNum; + } //updateStatus + +} //class ServoExtender