diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index f6650c5..3ceff98 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -29,6 +29,9 @@ import java.util.Locale; import ftclib.drivebase.FtcSwerveDrive; import ftclib.driverio.FtcGamepad; import ftclib.robotcore.FtcOpMode; +import teamcode.subsystems.Arm; +import teamcode.subsystems.Elevator; +import teamcode.subsystems.Intake; import teamcode.subsystems.RumbleIndicator; import trclib.drivebase.TrcDriveBase; import trclib.pathdrive.TrcPose2D; @@ -56,6 +59,13 @@ public class FtcTeleOp extends FtcOpMode private TrcPose2D robotFieldPose = null; private Integer savedLimelightPipeline = null; + private double prevElevatorPower = 0.0; + private double prevArmPower = 0.0; + private double prevWristTiltPower = 0.0; + private double prevWristRotatePower = 0.0; + private boolean shooterOn = false; + private double prevShooterVelocity = 0.0; + // // Implements FtcOpMode abstract method. // @@ -237,6 +247,83 @@ public class FtcTeleOp extends FtcOpMode if (RobotParams.Preferences.useSubsystems) { // Analog control of subsystems. + // Note that this sample code assumes only one subsystem is enabled at a time for demo purpose. + // Therefore, the same control may be assigned to multiple subsystems. + if (robot.elevator != null) + { + double elevatorPower = operatorGamepad.getLeftStickY(true); + if (elevatorPower != prevElevatorPower) + { + if (operatorAltFunc) + { + // Manual override. + robot.elevator.setPower(elevatorPower); + } + else + { + robot.elevator.setPidPower( + elevatorPower, Elevator.Params.MIN_POS, Elevator.Params.MAX_POS, true); + } + prevElevatorPower = elevatorPower; + } + } + else if (robot.arm != null) + { + double armPower = operatorGamepad.getLeftStickY(true); + if (armPower != prevArmPower) + { + if (operatorAltFunc) + { + // Manual override. + robot.arm.setPower(armPower); + } + else + { + robot.arm.setPidPower(armPower, Arm.Params.MIN_POS, Arm.Params.MAX_POS, true); + } + prevArmPower = armPower; + } + } + else if (robot.shooter != null) + { + if (shooterOn) + { + double shooterVel = robot.shooterVelocity.getValue(); + if (shooterVel != prevShooterVelocity) + { + robot.shooter.setShooterMotorRPM(shooterVel, shooterVel); + prevShooterVelocity = shooterVel; + } + } + else + { + if (prevShooterVelocity != 0.0) + { + robot.shooter.stopShooter(); + prevShooterVelocity = 0.0; + } + } + } + else if (robot.servoWrist != null) + { + double tiltPower = operatorGamepad.getRightStickY(true); + if (tiltPower != prevWristTiltPower) + { + robot.servoWrist.setPower(tiltPower); + prevWristTiltPower = tiltPower; + } + } + else if (robot.diffyServoWrist != null) + { + double tiltPower = operatorGamepad.getRightStickY(true); + double rotatePower = operatorGamepad.getRightStickX(true); + if (tiltPower != prevWristTiltPower || rotatePower != prevWristRotatePower) + { + robot.diffyServoWrist.diffyWrist.setPower(tiltPower, rotatePower); + prevWristTiltPower = tiltPower; + prevWristRotatePower = rotatePower; + } + } } // Display subsystem status. if (RobotParams.Preferences.updateDashboard || statusUpdateOn) @@ -450,9 +537,60 @@ public class FtcTeleOp extends FtcOpMode { robot.dashboard.displayPrintf(8, "Operator: %s=%s", button, pressed? "Pressed": "Released"); + // Note that this sample code assumes only one subsystem is enabled at a time for demo purpose. + // Therefore, the same control may be assigned to multiple subsystems. switch (button) { case A: + if (robot.shooter != null) + { + if (pressed) + { + shooterOn = !shooterOn; + } + } + else if (robot.intake != null) + { + if (pressed) + { + robot.intake.autoIntakeForward( + Intake.Params.INTAKE_FORWARD_POWER, Intake.Params.RETAIN_POWER, Intake.Params.FINISH_DELAY); + } + else + { + robot.intake.cancel(); + } + } + else if (robot.claw != null) + { + if (driverAltFunc) + { + if (pressed) + { + if (robot.claw.isClosed()) + { + robot.claw.open(); + } + else + { + robot.claw.close(); + } + } + } + else + { + if (pressed) + { + robot.claw.autoGrab(null, 0.0, null, 0.0); + } + else + { + robot.claw.cancel(); + } + } + } + break; + case B: case X: case Y: @@ -465,9 +603,113 @@ public class FtcTeleOp extends FtcOpMode case RightBumper: case DpadUp: + if (robot.elevator != null) + { + if (pressed) + { + robot.elevator.presetPositionUp(null, Elevator.Params.POWER_LIMIT); + } + } + else if (robot.arm != null) + { + if (pressed) + { + robot.arm.presetPositionUp(null, Arm.Params.POWER_LIMIT); + } + } + else if (robot.shooter != null) + { + if (pressed) + { + robot.shooterVelocity.upValue(); + } + } + else if (robot.servoWrist != null) + { + if (pressed) + { + robot.servoWrist.presetPositionUp(null); + } + } + else if (robot.diffyServoWrist != null) + { + if (pressed) + { + robot.diffyServoWrist.tiltPresetPositionUp(null); + } + } + break; + case DpadDown: + if (robot.elevator != null) + { + if (pressed) + { + robot.elevator.presetPositionDown(null, Elevator.Params.POWER_LIMIT); + } + } + else if (robot.arm != null) + { + if (pressed) + { + robot.arm.presetPositionDown(null, Arm.Params.POWER_LIMIT); + } + } + else if (robot.shooter != null) + { + if (pressed) + { + robot.shooterVelocity.downValue(); + } + } + else if (robot.servoWrist != null) + { + if (pressed) + { + robot.servoWrist.presetPositionDown(null); + } + } + else if (robot.diffyServoWrist != null) + { + if (pressed) + { + robot.diffyServoWrist.tiltPresetPositionDown(null); + } + } + break; + case DpadLeft: + if (robot.shooter != null) + { + if (pressed) + { + robot.shooterVelocity.downIncrement(); + } + } + else if (robot.diffyServoWrist != null) + { + if (pressed) + { + robot.diffyServoWrist.rotatePresetPositionDown(null); + } + } + break; + case DpadRight: + if (robot.shooter != null) + { + if (pressed) + { + robot.shooterVelocity.upIncrement(); + } + } + else if (robot.diffyServoWrist != null) + { + if (pressed) + { + robot.diffyServoWrist.rotatePresetPositionUp(null); + } + } break; case Back: diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index 2fefead..eb437b8 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -599,6 +599,50 @@ public class FtcTest extends FtcTeleOp swerveDrive.setSteerAngle(0.0, false, true); } } + else if (RobotParams.Preferences.tuneElevator) + { + if (pressed) + { + robot.elevator.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.elevator.presetPositionUp(null, Dashboard.Subsystem.powerLimit); + } + } + else if (RobotParams.Preferences.tuneArm) + { + if (pressed) + { + robot.arm.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.arm.presetPositionUp(null, Dashboard.Subsystem.powerLimit); + } + } + else if (RobotParams.Preferences.tuneShooterMotor1) + { + if (pressed) + { + robot.shooter.shooterMotor1.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.shooter.shooterMotor1.presetPositionUp(null, Dashboard.Subsystem.powerLimit); + } + } + else if (RobotParams.Preferences.tuneShooterMotor2) + { + if (pressed) + { + robot.shooter.shooterMotor2.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.shooter.shooterMotor2.presetPositionUp(null, Dashboard.Subsystem.powerLimit); + } + } passToTeleOp = false; } break; @@ -616,6 +660,50 @@ public class FtcTest extends FtcTeleOp swerveDrive.setSteerAngle(180.0, false, true); } } + else if (RobotParams.Preferences.tuneElevator) + { + if (pressed) + { + robot.elevator.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.elevator.presetPositionDown(null, Dashboard.Subsystem.powerLimit); + } + } + else if (RobotParams.Preferences.tuneArm) + { + if (pressed) + { + robot.arm.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.arm.presetPositionDown(null, Dashboard.Subsystem.powerLimit); + } + } + else if (RobotParams.Preferences.tuneShooterMotor1) + { + if (pressed) + { + robot.shooter.shooterMotor1.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.shooter.shooterMotor1.presetPositionDown(null, Dashboard.Subsystem.powerLimit); + } + } + else if (RobotParams.Preferences.tuneShooterMotor2) + { + if (pressed) + { + robot.shooter.shooterMotor2.setPositionPidParameters( + Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, + Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, + Dashboard.Subsystem.enableSquid); + robot.shooter.shooterMotor2.presetPositionDown(null, Dashboard.Subsystem.powerLimit); + } + } passToTeleOp = false; } break; diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index a7b7372..b09f599 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -29,10 +29,18 @@ import ftclib.driverio.FtcDashboard; import ftclib.driverio.FtcMatchInfo; import ftclib.robotcore.FtcOpMode; import ftclib.sensor.FtcRobotBattery; +import teamcode.subsystems.Arm; +import teamcode.subsystems.Claw; +import teamcode.subsystems.DiffyServoWrist; +import teamcode.subsystems.Elevator; +import teamcode.subsystems.Intake; import teamcode.subsystems.LEDIndicator; import teamcode.subsystems.RobotBase; import teamcode.subsystems.RumbleIndicator; +import teamcode.subsystems.ServoWrist; +import teamcode.subsystems.Shooter; import teamcode.vision.Vision; +import trclib.dataprocessor.TrcDiscreteValue; import trclib.motor.TrcMotor; import trclib.motor.TrcServo; import trclib.pathdrive.TrcPose2D; @@ -40,7 +48,11 @@ import trclib.robotcore.TrcDbgTrace; import trclib.robotcore.TrcEvent; import trclib.robotcore.TrcRobot; import trclib.sensor.TrcDigitalInput; +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. @@ -66,6 +78,14 @@ public class Robot public RumbleIndicator operatorRumble; public FtcRobotBattery battery; // Subsystems. + public TrcMotor elevator; + public TrcMotor arm; + public TrcShooter shooter; + public TrcDiscreteValue shooterVelocity; + public TrcIntake intake; + public TrcServoClaw claw; + public TrcServo servoWrist; + public DiffyServoWrist diffyServoWrist; // Autotasks. /** @@ -114,7 +134,45 @@ public class Robot if (RobotParams.Preferences.useSubsystems) { // Create subsystems. + if (RobotParams.Preferences.useElevator) + { + elevator = new Elevator().getMotor(); + } + if (RobotParams.Preferences.useArm) + { + arm = new Arm().getMotor(); + } + + if (RobotParams.Preferences.useShooter) + { + shooter = new Shooter().getShooter(); + shooterVelocity = new TrcDiscreteValue( + Shooter.Params.SUBSYSTEM_NAME + ".motorVel", + Shooter.Params.SHOOTER_MIN_VEL, Shooter.Params.SHOOTER_MAX_VEL, + Shooter.Params.SHOOTER_MIN_VEL_INC, Shooter.Params.SHOOTER_MAX_VEL_INC, + Shooter.Params.SHOOTER_DEF_VEL, Shooter.Params.SHOOTER_DEF_VEL_INC); + } + + if (RobotParams.Preferences.useIntake) + { + intake = new Intake().getIntake(); + } + + if (RobotParams.Preferences.useClaw) + { + claw = new Claw().getClaw(); + } + + if (RobotParams.Preferences.useServoWrist) + { + servoWrist = new ServoWrist().getServo(); + } + + if (RobotParams.Preferences.useDiffyServoWrist) + { + diffyServoWrist = new DiffyServoWrist(); + } // 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 83f4f5f..3030428 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -75,9 +75,20 @@ public class RobotParams public static final boolean useSparkfunOTOS = false; // Subsystems public static final boolean useSubsystems = false; + public static final boolean useElevator = false; + public static final boolean useArm = false; + public static final boolean useShooter = false; + public static final boolean useIntake = false; + public static final boolean useClaw = false; + public static final boolean useServoWrist = false; + public static final boolean useDiffyServoWrist = false; // Tuning public static final boolean tuneColorBlobVision = false; public static final boolean tuneDriveBase = false; + public static final boolean tuneElevator = false; + public static final boolean tuneArm = false; + public static final boolean tuneShooterMotor1 = false; + public static final boolean tuneShooterMotor2 = false; } //class Preferences /** diff --git a/TeamCode/src/main/java/teamcode/subsystems/Arm.java b/TeamCode/src/main/java/teamcode/subsystems/Arm.java new file mode 100644 index 0000000..adc6300 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/Arm.java @@ -0,0 +1,168 @@ +/* + * Copyright (c) 2024 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.FtcMotorActuator; +import trclib.controller.TrcPidController; +import trclib.motor.TrcMotor; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcSubsystem; + +/** + * This class implements an Arm Subsystem. The Arm subsystem consists of a motor with built-in encoder. It does not + * have any limit switches, so it is using motor stall detection to zero calibrate the built-in relative encoder. It + * supports gravity compensation by computing the power required to hold the arm at its current angle. + */ +public class Arm extends TrcSubsystem +{ + public static final class Params + { + public static final String SUBSYSTEM_NAME = "Arm"; + public static final boolean NEED_ZERO_CAL = true; + + public static final String MOTOR_NAME = SUBSYSTEM_NAME + ".motor"; + public static final FtcMotorActuator.MotorType MOTOR_TYPE= FtcMotorActuator.MotorType.DcMotor; + public static final boolean MOTOR_INVERTED = true; + + public static final double GOBILDA312_CPR = (((1.0 + (46.0/17.0))) * (1.0 + (46.0/11.0))) * 28.0; + public static final double DEG_PER_COUNT = 360.0 / GOBILDA312_CPR; + public static final double POS_OFFSET = 39.0; + public static final double POWER_LIMIT = 0.5; + public static final double ZERO_CAL_POWER = -0.25; + + public static final double MIN_POS = POS_OFFSET; + public static final double MAX_POS = 270.0; + public static final double TURTLE_POS = MIN_POS; + public static final double TURTLE_DELAY = 0.0; + public static final double[] posPresets = { + MIN_POS, 60.0, 90.0, 120.0, 150.0, 180.0, 210.0, 240.0, 270.0}; + public static final double POS_PRESET_TOLERANCE = 10.0; + + public static final boolean SOFTWARE_PID_ENABLED = true; + public static final boolean SQUID_ENABLED = false; + public static final TrcPidController.PidCoefficients posPidCoeffs = + new TrcPidController.PidCoefficients(0.018, 0.1, 0.001, 0.0, 2.0); + public static final double POS_PID_TOLERANCE = 1.0; + public static final double GRAVITY_COMP_MAX_POWER = 0.158; + // Since we don't have lower limit switch, must enable Stall Protection to do zero calibration by stalling. + public static final double STALL_MIN_POWER = Math.abs(ZERO_CAL_POWER); + public static final double STALL_TOLERANCE = 0.1; + public static final double STALL_TIMEOUT = 0.1; + public static final double STALL_RESET_TIMEOUT = 0.0; + } //class Params + + private final FtcDashboard dashboard; + private final TrcMotor motor; + + /** + * Constructor: Creates an instance of the object. + */ + public Arm() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcMotorActuator.Params motorParams = new FtcMotorActuator.Params() + .setPrimaryMotor(Params.MOTOR_NAME, Params.MOTOR_TYPE, Params.MOTOR_INVERTED) + .setPositionScaleAndOffset(Params.DEG_PER_COUNT, Params.POS_OFFSET) + .setPositionPresets(Params.POS_PRESET_TOLERANCE, Params.posPresets); + motor = new FtcMotorActuator(motorParams).getMotor(); + motor.setPositionPidParameters( + Params.posPidCoeffs, null, Params.POS_PID_TOLERANCE, Params.SOFTWARE_PID_ENABLED, Params.SQUID_ENABLED); + motor.setPositionPidPowerComp(this::getGravityComp); + motor.setStallProtection( + Params.STALL_MIN_POWER, Params.STALL_TOLERANCE, Params.STALL_TIMEOUT, Params.STALL_RESET_TIMEOUT); + } //Arm + + /** + * This method returns the created Arm motor. + * + * @return created arm motor. + */ + public TrcMotor getMotor() + { + return motor; + } //getMotor + + /** + * This method calculates the power required to make the arm gravity neutral. + * + * @param currPower specifies the current applied PID power (not used). + * @return calculated compensation power. + */ + private double getGravityComp(double currPower) + { + return Params.GRAVITY_COMP_MAX_POWER * Math.sin(Math.toRadians(motor.getPosition())); + } //getGravityComp + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + motor.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) + { + motor.zeroCalibrate(owner, Params.ZERO_CAL_POWER, event); + } //zeroCalibrate + + /** + * This method resets the subsystem state. Typically, this is used to retract the subsystem for turtle mode. + */ + @Override + public void resetState() + { + motor.setPosition(Params.TURTLE_DELAY, Params.TURTLE_POS, true, Params.POWER_LIMIT); + } //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: power=%.3f, current=%.3f, pos=%.3f/%.3f", + Params.SUBSYSTEM_NAME, motor.getPower(), motor.getCurrent(), motor.getPosition(), motor.getPidTarget()); + return lineNum; + } //updateStatus + +} //class Arm diff --git a/TeamCode/src/main/java/teamcode/subsystems/Claw.java b/TeamCode/src/main/java/teamcode/subsystems/Claw.java new file mode 100644 index 0000000..84078cf --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/Claw.java @@ -0,0 +1,187 @@ +/* + * Copyright (c) 2024 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 com.qualcomm.hardware.rev.Rev2mDistanceSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import ftclib.driverio.FtcDashboard; +import ftclib.robotcore.FtcOpMode; +import ftclib.subsystem.FtcServoClaw; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcServoClaw; +import trclib.subsystem.TrcSubsystem; + +/** + * This class implements a Claw Subsystem. The Claw subsystem consists of two servos and a sensor to detect the + * presence of an object and can auto grab it. The sensor can be either a digital sensor such as touch sensor or beam + * break sensor) or an analog sensor such as a distance sensor. + */ +public class Claw extends TrcSubsystem +{ + public static final class Params + { + public static final String SUBSYSTEM_NAME = "Claw"; + public static final boolean NEED_ZERO_CAL = false; + + public static final String PRIMARY_SERVO_NAME = SUBSYSTEM_NAME + ".primary"; + public static final boolean PRIMARY_SERVO_INVERTED = false; + + public static final String FOLLOWER_SERVO_NAME = SUBSYSTEM_NAME + ".follower"; + public static final boolean FOLLOWER_SERVO_INVERTED = !PRIMARY_SERVO_INVERTED; + + public static final double OPEN_POS = 0.2; + public static final double OPEN_TIME = 0.5; + public static final double CLOSE_POS = 0.55; + public static final double CLOSE_TIME = 0.5; + + public static final boolean USE_ANALOG_SENSOR = true; + public static final String ANALOG_SENSOR_NAME = SUBSYSTEM_NAME + ".sensor"; + public static final double SENSOR_TRIGGER_THRESHOLD = 2.0; + public static final boolean ANALOG_TRIGGER_INVERTED = true; + + public static final boolean USE_DIGITAL_SENSOR = false; + public static final String DIGITAL_SENSOR_NAME = SUBSYSTEM_NAME + ".sensor"; + public static final boolean DIGITAL_TRIGGER_INVERTED = false; + } //class Params + + private final FtcDashboard dashboard; + private final Rev2mDistanceSensor analogSensor; + private final TrcServoClaw claw; + + /** + * Constructor: Creates an instance of the object. + */ + public Claw() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + if (Params.USE_ANALOG_SENSOR) + { + analogSensor = FtcOpMode.getInstance().hardwareMap.get( + Rev2mDistanceSensor.class, Params.ANALOG_SENSOR_NAME); + } + else + { + analogSensor = null; + } + + FtcServoClaw.Params clawParams = new FtcServoClaw.Params() + .setPrimaryServo(Params.PRIMARY_SERVO_NAME, Params.PRIMARY_SERVO_INVERTED) + .setFollowerServo(Params.FOLLOWER_SERVO_NAME, Params.FOLLOWER_SERVO_INVERTED) + .setOpenCloseParams(Params.OPEN_POS, Params.OPEN_TIME, Params.CLOSE_POS, Params.CLOSE_TIME); + + if (analogSensor != null) + { + clawParams.setAnalogSensorTrigger( + this::getSensorData, Params.ANALOG_TRIGGER_INVERTED, Params.SENSOR_TRIGGER_THRESHOLD); + } + else if (Params.USE_DIGITAL_SENSOR) + { + clawParams.setDigitalInputTrigger(Params.DIGITAL_SENSOR_NAME, Params.DIGITAL_TRIGGER_INVERTED); + } + + claw = new FtcServoClaw(Params.SUBSYSTEM_NAME, clawParams).getClaw(); + claw.open(); + } //Claw + + /** + * This method returns the created Servo Claw object. + * + * @return created claw. + */ + public TrcServoClaw getClaw() + { + return claw; + } //getClaw + + /** + * This method returns the current sensor value if it has one. + * + * @return sensor value if there is a sensor, 0 if there is none. + */ + private double getSensorData() + { + if (analogSensor != null) + { + return analogSensor.getDistance(DistanceUnit.INCH); + } + else + { + return 0.0; + } + } //getSensorData + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + claw.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() + { + claw.open(); + } //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=%s, hasObject=%s, sensor=%s, autoActive=%s", + Params.SUBSYSTEM_NAME, claw.isClosed()? "closed": "open", claw.hasObject(), + analogSensor != null? claw.getSensorValue(): claw.getSensorState(), claw.isAutoActive()); + return lineNum; + } //updateStatus + +} //class Claw diff --git a/TeamCode/src/main/java/teamcode/subsystems/DiffyServoWrist.java b/TeamCode/src/main/java/teamcode/subsystems/DiffyServoWrist.java new file mode 100644 index 0000000..445e77a --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/DiffyServoWrist.java @@ -0,0 +1,286 @@ +/* + * 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.subsystem.FtcDifferentialServoWrist; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcDifferentialServoWrist; +import trclib.subsystem.TrcSubsystem; + +/** + * This class creates the Differential Servo Wrist subsystem. The Differential Servo Wrist subsystem is a 2-DOF + * system that consists of two servos. When the two servos rotate in the same direction, the wrist will tilt up and + * down. When the two servos rotate in opposite direction, the wrist will rotate. Regular servos have a limited + * range of movement. Because of this, the tilt and rotation of the wrist will limit each other's range of motion. + * For example, if the wrist is tilted to one extreme end, the wrist cannot rotate. If the wrist is in the middle + * tilt position, it will have maximum rotation range and vice versa. + */ +public class DiffyServoWrist extends TrcSubsystem +{ + public static class Params + { + public static final String SUBSYSTEM_NAME = "DiffyServoWrist"; + public static final boolean NEED_ZERO_CAL = false; + + public static final String SERVO1_NAME = Params.SUBSYSTEM_NAME + ".servo1"; + public static final boolean SERVO1_INVERTED = false; + public static final String SERVO2_NAME = Params.SUBSYSTEM_NAME + ".servo2"; + public static final boolean SERVO2_INVERTED = !SERVO1_INVERTED; + + public static final double LOGICAL_MIN_POS = 0.15; + public static final double LOGICAL_MAX_POS = 0.85; + public static final double PHYSICAL_POS_RANGE = 230.0; + public static final double TILT_POS_OFFSET = -20.0; + public static final double ROTATE_POS_OFFSET = -1.0; + public static final double MAX_STEP_RATE = 300.0; // deg/sec (max 520) + + public static final double TILT_MIN_POS = -90.0; + public static final double TILT_MAX_POS = 90.0; + public static final double ROTATE_MIN_POS = -90.0; + public static final double ROTATE_MAX_POS = 90.0; + + public static final double POS_PRESET_TOLERANCE = 1.0; + public static final double[] tiltPosPresets = {-110, -90.0, -45.0, 0.0, 45.0, 90.0, 110}; + public static final double[] rotatePosPresets = {-90.0, -45.0, 0.0, 45.0, 90.0}; + } //class Params + + private final FtcDashboard dashboard; + public final TrcDifferentialServoWrist diffyWrist; + + /** + * Constructor: Creates an instance of the object. + */ + public DiffyServoWrist() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcDifferentialServoWrist.Params wristParams = new FtcDifferentialServoWrist.Params() + .setServos(Params.SERVO1_NAME, Params.SERVO1_INVERTED, Params.SERVO2_NAME, Params.SERVO2_INVERTED) + .setPosRange( + Params.LOGICAL_MIN_POS, Params.LOGICAL_MAX_POS, Params.PHYSICAL_POS_RANGE, Params.TILT_POS_OFFSET, + Params.ROTATE_POS_OFFSET) + .setMaxStepRate(Params.MAX_STEP_RATE) + .setPositionLimits(Params.TILT_MIN_POS, Params.TILT_MAX_POS, Params.ROTATE_MIN_POS, Params.ROTATE_MAX_POS) + .setPosPresets(Params.POS_PRESET_TOLERANCE, Params.tiltPosPresets, Params.rotatePosPresets); + + diffyWrist = new FtcDifferentialServoWrist(Params.SUBSYSTEM_NAME, wristParams).getWrist(); + diffyWrist.setPosition(90.0, 0.0); + } //DiffyServoWrist + + /** + * This method returns the wrist tilt position in degrees. + * + * @return wrist tilt position. + */ + public double getTiltPosition() + { + return diffyWrist.getTiltPosition(); + } //getTiltPosition + + /** + * This method returns the wrist rotate position in degrees. + * + * @return wrist rotate position. + */ + public double getRotatePosition() + { + return diffyWrist.getRotatePosition(); + } //getRotatePosition + + /** + * This method sets the wrist 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 tilt position of the wrist, can be zero if no + * delay. + * @param tiltPos specifies the physical tilt position of the wrist in degrees. + * @param rotatePos specifies the physical rotate position of the wrist in degrees, null if not provided for no + * change. + * @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 setPosition( + String owner, double delay, double tiltPos, Double rotatePos, TrcEvent completionEvent, double timeout) + { + if (rotatePos != null) + { + rotatePos %= 180.0; + if (rotatePos > 90.0) + { + rotatePos -= 180.0; + } + else if (rotatePos < -90.0) + { + rotatePos += 180.0; + } + } + else + { + rotatePos = getRotatePosition(); + } + diffyWrist.setPosition(owner, delay, tiltPos, rotatePos, completionEvent, timeout); + } //setPosition + + /** + * This method sets the wrist position. + * + * @param delay specifies the delay in seconds before setting the tilt position of the wrist, can be zero if no + * delay. + * @param tiltPos specifies the physical tilt position of the wrist in degrees. + * @param rotatePos specifies the physical rotate position of the wrist in degrees, null if not provided for no + * change. + * @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 setPosition(double delay, double tiltPos, Double rotatePos, TrcEvent completionEvent, double timeout) + { + setPosition(null, delay, tiltPos, rotatePos, completionEvent, timeout); + } //setPosition + + /** + * This method sets the wrist position. + * + * @param tiltPos specifies the physical tilt position of the wrist in degrees. + * @param rotatePos specifies the physical rotate position of the wrist in degrees, null if not provided for no + * change. + * @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 setPosition(double tiltPos, Double rotatePos, TrcEvent completionEvent, double timeout) + { + setPosition(null, 0.0, tiltPos, rotatePos, completionEvent, timeout); + } //setPosition + + /** + * This method sets the wrist position. + * + * @param tiltPos specifies the physical tilt position of the wrist in degrees. + * @param rotatePos specifies the physical rotate position of the wrist in degrees, null if not provided for no + * change. + */ + public void setPosition(double tiltPos, Double rotatePos) + { + setPosition(null, 0.0, tiltPos, rotatePos, null, 0.0); + } //setPosition + + /** + * This method sets the wrist to the next tilt preset position up from the current position. + * + * @param 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. + */ + public void tiltPresetPositionUp(String owner) + { + diffyWrist.tiltPresetPositionUp(owner); + } //tiltPresetPositionUp + + /** + * This method sets the wrist to the next tilt preset position down from the current position. + * + * @param 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. + */ + public void tiltPresetPositionDown(String owner) + { + diffyWrist.tiltPresetPositionDown(owner); + } //tiltPresetPositionDown + + /** + * This method sets the wrist to the next rotate preset position up from the current position. + * + * @param 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. + */ + public void rotatePresetPositionUp(String owner) + { + diffyWrist.rotatePresetPositionUp(owner); + } //rotatePresetPositionUp + + /** + * This method sets the wrist to the next rotate preset position down from the current position. + * + * @param 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. + */ + public void rotatePresetPositionDown(String owner) + { + diffyWrist.rotatePresetPositionDown(owner); + } //rotatePresetPositionDown + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + diffyWrist.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() + { + setPosition(-90.0, 0.0); + } //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: tilt(pwr/pos)=%.1f/%.1f,rotate(pwr/pos)=%.1f/%.1f", + Params.SUBSYSTEM_NAME, diffyWrist.getTiltPower(), diffyWrist.getTiltPosition(), + diffyWrist.getRotatePower(), diffyWrist.getRotatePosition()); + return lineNum; + } //updateStatus + +} //class DiffyServoWrist diff --git a/TeamCode/src/main/java/teamcode/subsystems/Elevator.java b/TeamCode/src/main/java/teamcode/subsystems/Elevator.java new file mode 100644 index 0000000..5cdde51 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/Elevator.java @@ -0,0 +1,163 @@ +/* + * Copyright (c) 2024 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.FtcMotorActuator; +import trclib.controller.TrcPidController; +import trclib.motor.TrcMotor; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcSubsystem; + +/** + * This class implements an Elevator Subsystem. The Elevator subsystem consists of a motor with built-in encoder and + * a lower limit switch for zero calibrating the relative encoder. It supports gravity compensation. In the case of + * an elevator, gravity compensation power is the constant power required to hold the elevator at any position. + */ +public class Elevator extends TrcSubsystem +{ + public static final class Params + { + public static final String SUBSYSTEM_NAME = "Elevator"; + public static final boolean NEED_ZERO_CAL = true; + + public static final String MOTOR_NAME = SUBSYSTEM_NAME + ".motor"; + public static final FtcMotorActuator.MotorType MOTOR_TYPE= FtcMotorActuator.MotorType.DcMotor; + public static final boolean MOTOR_INVERTED = true; + + public static final String LOWER_LIMITSW_NAME = SUBSYSTEM_NAME + ".lowerLimitSw"; + public static final boolean LOWER_LIMITSW_INVERTED = false; + + public static final double INCHES_PER_COUNT = 18.25/4941.0; + public static final double POS_OFFSET = 10.875; + public static final double POWER_LIMIT = 1.0; + public static final double ZERO_CAL_POWER = -0.25; + + public static final double MIN_POS = POS_OFFSET; + public static final double MAX_POS = 30.25; + public static final double TURTLE_POS = MIN_POS; + public static final double TURTLE_DELAY = 0.0; + public static final double[] posPresets = {MIN_POS, 15.0, 20.0, 25.0, 30.0}; + public static final double POS_PRESET_TOLERANCE = 1.0; + + public static final boolean SOFTWARE_PID_ENABLED = true; + public static final TrcPidController.PidCoefficients posPidCoeffs = + new TrcPidController.PidCoefficients(1.0, 0.0, 0.0, 0.0, 0.0); + public static final double POS_PID_TOLERANCE = 0.1; + public static final double GRAVITY_COMP_POWER = 0.0; + } //class Params + + private final FtcDashboard dashboard; + private final TrcMotor motor; + + /** + * Constructor: Creates an instance of the object. + */ + public Elevator() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcMotorActuator.Params motorParams = new FtcMotorActuator.Params() + .setPrimaryMotor(Params.MOTOR_NAME, Params.MOTOR_TYPE, Params.MOTOR_INVERTED) + .setLowerLimitSwitch(Params.LOWER_LIMITSW_NAME, Params.LOWER_LIMITSW_INVERTED) + .setPositionScaleAndOffset(Params.INCHES_PER_COUNT, Params.POS_OFFSET) + .setPositionPresets(Params.POS_PRESET_TOLERANCE, Params.posPresets); + motor = new FtcMotorActuator(motorParams).getMotor(); + motor.setPositionPidParameters( + Params.posPidCoeffs, Params.POS_PID_TOLERANCE, Params.SOFTWARE_PID_ENABLED); + motor.setPositionPidPowerComp(this::getGravityComp); + } //Elevator + + /** + * This method returns the created Elevator motor. + * + * @return created elevator motor. + */ + public TrcMotor getMotor() + { + return motor; + } //getMotor + + /** + * This method calculates the power required to make the elevator gravity neutral. + * + * @param currPower specifies the current applied PID power (not used). + * @return calculated compensation power. + */ + private double getGravityComp(double currPower) + { + return Params.GRAVITY_COMP_POWER; + } //getGravityComp + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + motor.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) + { + motor.zeroCalibrate(owner, Params.ZERO_CAL_POWER, event); + } //zeroCalibrate + + /** + * This method resets the subsystem state. Typically, this is used to retract the subsystem for turtle mode. + */ + @Override + public void resetState() + { + motor.setPosition(Params.TURTLE_DELAY, Params.TURTLE_POS, true, Params.POWER_LIMIT); + } //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: power=%.3f, current=%.3f, pos=%.3f/%.3f, lowerLimit=%s", + Params.SUBSYSTEM_NAME, motor.getPower(), motor.getCurrent(), motor.getPosition(), motor.getPidTarget(), + motor.isLowerLimitSwitchActive()); + return lineNum; + } //updateStatus + +} //class Elevator diff --git a/TeamCode/src/main/java/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/teamcode/subsystems/Intake.java new file mode 100644 index 0000000..89bc08a --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/Intake.java @@ -0,0 +1,136 @@ +/* + * Copyright (c) 2024 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.FtcMotorActuator; +import ftclib.subsystem.FtcIntake; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcIntake; +import trclib.subsystem.TrcSubsystem; + +/** + * This class implements an Intake Subsystem. + */ +public class Intake extends TrcSubsystem +{ + public static final class Params + { + public static final String SUBSYSTEM_NAME = "Intake"; + public static final boolean NEED_ZERO_CAL = false; + public static final boolean TWO_MOTOR_INTAKE = true; + + public static final String PRIMARY_MOTOR_NAME = SUBSYSTEM_NAME + ".primary"; + public static final FtcMotorActuator.MotorType PRIMARY_MOTOR_TYPE= FtcMotorActuator.MotorType.DcMotor; + public static final boolean PRIMARY_MOTOR_INVERTED = !TWO_MOTOR_INTAKE; + + public static final String FOLLOWER_MOTOR_NAME = SUBSYSTEM_NAME + ".follower"; + public static final FtcMotorActuator.MotorType FOLLOWER_MOTOR_TYPE= FtcMotorActuator.MotorType.DcMotor; + public static final boolean FOLLOWER_MOTOR_INVERTED = PRIMARY_MOTOR_INVERTED; + + public static final String SENSOR_NAME = SUBSYSTEM_NAME + ".sensor"; + public static final boolean SENSOR_INVERTED = false; + + public static final double INTAKE_FORWARD_POWER = 1.0; + public static final double RETAIN_POWER = 0.0; + public static final double FINISH_DELAY = 0.0; + } //class Params + + private final FtcDashboard dashboard; + private final TrcIntake intake; + + /** + * Constructor: Creates an instance of the object. + */ + public Intake() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcIntake.Params intakeParams = new FtcIntake.Params() + .setPrimaryMotor(Params.PRIMARY_MOTOR_NAME, Params.PRIMARY_MOTOR_TYPE, Params.PRIMARY_MOTOR_INVERTED) + .setEntryDigitalInput(Params.SENSOR_NAME, Params.SENSOR_INVERTED, null); + if (Params.TWO_MOTOR_INTAKE) + { + intakeParams.setFollowerMotor( + Params.FOLLOWER_MOTOR_NAME, Params.FOLLOWER_MOTOR_TYPE, Params.FOLLOWER_MOTOR_INVERTED); + } + intake = new FtcIntake(Params.SUBSYSTEM_NAME, intakeParams).getIntake(); + } //Intake + + public TrcIntake getIntake() + { + return intake; + } //getIntake + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + intake.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() + { + // No reset state needed. + } //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: power=%.3f, hasObject=%s, sensorState=%s, active=%s", + Params.SUBSYSTEM_NAME, intake.getPower(), intake.hasObject(), intake.getSensorState(intake.entryTrigger), + intake.isAutoActive()); + return lineNum; + } //updateStatus + +} //class Intake diff --git a/TeamCode/src/main/java/teamcode/subsystems/ServoWrist.java b/TeamCode/src/main/java/teamcode/subsystems/ServoWrist.java new file mode 100644 index 0000000..8720a34 --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/ServoWrist.java @@ -0,0 +1,133 @@ +/* + * 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 Wrist subsystem. The Servo Wrist subsystem is a 1-DOF system that consists of a servo. + * It allows the wrist to tilt up and down. Regular servos have a limited * range of movement. Therefore, it limits + * the tilting range of the wrist. + */ +public class ServoWrist extends TrcSubsystem +{ + public static class Params + { + public static final String SUBSYSTEM_NAME = "ServoWrist"; + 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 double LOGICAL_MIN_POS = 0.1; + public static final double LOGICAL_MAX_POS = 0.8; + public static final double PHYSICAL_MIN_POS = -90.0; // in degrees + public static final double PHYSICAL_MAX_POS = 90.0; // in degrees + + public static final double POS_PRESET_TOLERANCE = 1.0; // in degrees + public static final double[] tiltPosPresets = {-110, -90.0, -45.0, 0.0, 45.0, 90.0, 110}; + public static final double[] rotatePosPresets = {-90.0, -45.0, 0.0, 45.0, 90.0}; + } //class Params + + private final FtcDashboard dashboard; + public final TrcServo servoWrist; + + /** + * Constructor: Creates an instance of the object. + */ + public ServoWrist() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcServoActuator.Params wristParams = new FtcServoActuator.Params() + .setPrimaryServo(Params.PRIMARY_SERVO_NAME, Params.PRIMARY_SERVO_INVERTED) + .setLogicalPosRange(Params.LOGICAL_MIN_POS, Params.LOGICAL_MAX_POS) + .setPhysicalPosRange(Params.PHYSICAL_MIN_POS, Params.PHYSICAL_MAX_POS) + .setPositionPresets(Params.POS_PRESET_TOLERANCE, Params.tiltPosPresets); + + servoWrist = new FtcServoActuator(wristParams).getServo(); + } //ServoWrist + + /** + * This method returns the created ServoWrist servo. + * + * @return created wrist servo. + */ + public TrcServo getServo() + { + return servoWrist; + } //getServo + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + servoWrist.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() + { + servoWrist.setPosition(-90.0); + } //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", Params.SUBSYSTEM_NAME, servoWrist.getPosition()); + return lineNum; + } //updateStatus + +} //class ServoWrist diff --git a/TeamCode/src/main/java/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/teamcode/subsystems/Shooter.java new file mode 100644 index 0000000..4b58d7d --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/Shooter.java @@ -0,0 +1,180 @@ +/* + * Copyright (c) 2024 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.FtcMotorActuator; +import ftclib.subsystem.FtcShooter; +import trclib.controller.TrcPidController; +import trclib.motor.TrcMotor; +import trclib.robotcore.TrcEvent; +import trclib.subsystem.TrcShooter; +import trclib.subsystem.TrcSubsystem; + +/** + * This class implements an Shooter Subsystem. The shooter subsystem consists of one or two shooter motors. For + * two-motor shooter, the two motors can be arranged to spin in the same direction (2-stage shooting) or in opposite + * directions. For opposite spinning motor arrangement, one can spin the motors at different speed to create back spin + * when shooting the object. In the two-motor configuration, because the two motors may not be identical (even if they + * are the same model), the subsystem allows you to tune different PID coefficients for each motor. The shooter + * subsystem also supports optionally mounting on a pan and tilt platform. This allows for aiming the shooter at + * the shooting target. + */ +public class Shooter extends TrcSubsystem +{ + public static final class Params + { + public static final String SUBSYSTEM_NAME = "Shooter"; + public static final boolean NEED_ZERO_CAL = false; + + public static final String MOTOR1_NAME = SUBSYSTEM_NAME + ".motor1"; + public static final FtcMotorActuator.MotorType MOTOR1_TYPE= FtcMotorActuator.MotorType.DcMotor; + public static final boolean MOTOR1_INVERTED = false; + + public static final boolean HAS_TWO_SHOOTER_MOTORS = true; + public static final String MOTOR2_NAME = SUBSYSTEM_NAME + ".motor2"; + public static final FtcMotorActuator.MotorType MOTOR2_TYPE= FtcMotorActuator.MotorType.DcMotor; + public static final boolean MOTOR2_INVERTED = true; + + public static final double GOBILDA1620_RPC = 1.0 / ((1.0 + (46.0/17.0)) * 28.0); + public static final boolean SOFTWARE_PID_ENABLED = true; + public static final TrcPidController.PidCoefficients shooter1PidCoeffs = + new TrcPidController.PidCoefficients(0.025, 0.0, 0.0, 0.039, 0.0); + public static final TrcPidController.PidCoefficients shooter2PidCoeffs = + new TrcPidController.PidCoefficients(0.025, 0.0, 0.0, 0.041, 0.0); + public static final double SHOOTER_PID_TOLERANCE = 10.0; + + public static final double SHOOTER_MIN_VEL = 10.0; // in RPM + public static final double SHOOTER_MAX_VEL = 1620.0; // in RPM + public static final double SHOOTER_MIN_VEL_INC = 1.0; // in RPM + public static final double SHOOTER_MAX_VEL_INC = 100.0; // in RPM + public static final double SHOOTER_DEF_VEL = 1000.0; // in RPM + public static final double SHOOTER_DEF_VEL_INC = 10.0; // in RPM + } //class Params + + private final FtcDashboard dashboard; + private final TrcShooter shooter; + + /** + * Constructor: Creates an instance of the object. + */ + public Shooter() + { + super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL); + + dashboard = FtcDashboard.getInstance(); + FtcShooter.Params shooterParams = new FtcShooter.Params() + .setShooterMotor1(Params.MOTOR1_NAME, Params.MOTOR1_TYPE, Params.MOTOR1_INVERTED); + if (Params.HAS_TWO_SHOOTER_MOTORS) + { + shooterParams.setShooterMotor2(Params.MOTOR2_NAME, Params.MOTOR2_TYPE, Params.MOTOR2_INVERTED); + } + shooter = new FtcShooter(Params.SUBSYSTEM_NAME, shooterParams).getShooter(); + configShooterMotor(shooter.getShooterMotor1(), Params.shooter1PidCoeffs); + TrcMotor shooterMotor2 = shooter.getShooterMotor2(); + if (shooterMotor2 != null) + { + configShooterMotor(shooterMotor2, Params.shooter2PidCoeffs); + } + } //Shooter + + /** + * This method returns the created Shooter. + * + * @return created shooter. + */ + public TrcShooter getShooter() + { + return shooter; + } //getShooter + + /** + * This method configures the shooter motor. + * + * @param motor specifies the shooter motor. + * @param pidCoeffs specifies the PID coefficients for velocity control. + */ + private void configShooterMotor(TrcMotor motor, TrcPidController.PidCoefficients pidCoeffs) + { + motor.setPositionSensorScaleAndOffset(Params.GOBILDA1620_RPC, 0.0); + motor.setVelocityPidParameters(pidCoeffs, Params.SHOOTER_PID_TOLERANCE, Params.SOFTWARE_PID_ENABLED); + } //configShooterMotor + + // + // Implements TrcSubsystem abstract methods. + // + + /** + * This method cancels any pending operations. + */ + @Override + public void cancel() + { + shooter.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() + { + // No reset state needed. + } //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++, "%s1: power=%.3f, current=%.3f, vel=%.3f, target=%.3f", + Params.SUBSYSTEM_NAME, shooter.shooterMotor1.getPower(), shooter.shooterMotor1.getCurrent(), + shooter.getShooterMotor1RPM(), shooter.getShooterMotor1TargetRPM()); + if (Params.HAS_TWO_SHOOTER_MOTORS) + { + dashboard.displayPrintf( + lineNum++, "%s2: power=%.3f, current=%.3f, vel=%.3f, target=%.3f", + Params.SUBSYSTEM_NAME, shooter.shooterMotor2.getPower(), shooter.shooterMotor2.getCurrent(), + shooter.getShooterMotor2RPM(), shooter.getShooterMotor2TargetRPM()); + } + return lineNum; + } //updateStatus + +} //class Shooter