SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 13:01:24 -07:00
Code Issues Packages Projects Releases Wiki Activity

Recreated SampleCode branch because previous branch was messed up.

Browse Source
This commit is contained in:
Titan Robotics Club
2025-05-02 16:48:02 -07:00
parent 83ba712462
commit 0a62370ac3
11 changed files with 1652 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

242
TeamCode/src/main/java/teamcode/FtcTeleOp.java
View File

@ -29,6 +29,9 @@ import java.util.Locale;
import ftclib.drivebase.FtcSwerveDrive; import ftclib.drivebase.FtcSwerveDrive;
import ftclib.driverio.FtcGamepad; import ftclib.driverio.FtcGamepad;
import ftclib.robotcore.FtcOpMode; import ftclib.robotcore.FtcOpMode;
import teamcode.subsystems.Arm;
import teamcode.subsystems.Elevator;
import teamcode.subsystems.Intake;
import teamcode.subsystems.RumbleIndicator; import teamcode.subsystems.RumbleIndicator;
import trclib.drivebase.TrcDriveBase; import trclib.drivebase.TrcDriveBase;
import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose2D;
@ -56,6 +59,13 @@ public class FtcTeleOp extends FtcOpMode
private TrcPose2D robotFieldPose = null; private TrcPose2D robotFieldPose = null;
private Integer savedLimelightPipeline = 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. // Implements FtcOpMode abstract method.
// //
@ -237,6 +247,83 @@ public class FtcTeleOp extends FtcOpMode
if (RobotParams.Preferences.useSubsystems) if (RobotParams.Preferences.useSubsystems)
{ {
// Analog control of subsystems. // 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. // Display subsystem status.
if (RobotParams.Preferences.updateDashboard || statusUpdateOn) 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"); 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) switch (button)
{ {
case A: 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 B:
case X: case X:
case Y: case Y:
@ -465,9 +603,113 @@ public class FtcTeleOp extends FtcOpMode
case RightBumper: case RightBumper:
case DpadUp: 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: 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: 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: case DpadRight:
if (robot.shooter != null)
{
if (pressed)
{
robot.shooterVelocity.upIncrement();
}
}
else if (robot.diffyServoWrist != null)
{
if (pressed)
{
robot.diffyServoWrist.rotatePresetPositionUp(null);
}
}
break; break;
case Back: case Back:

88
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -599,6 +599,50 @@ public class FtcTest extends FtcTeleOp
swerveDrive.setSteerAngle(0.0, false, true); 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; passToTeleOp = false;
} }
break; break;
@ -616,6 +660,50 @@ public class FtcTest extends FtcTeleOp
swerveDrive.setSteerAngle(180.0, false, true); 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; passToTeleOp = false;
} }
break; break;

58
TeamCode/src/main/java/teamcode/Robot.java
View File

@ -29,10 +29,18 @@ import ftclib.driverio.FtcDashboard;
import ftclib.driverio.FtcMatchInfo; import ftclib.driverio.FtcMatchInfo;
import ftclib.robotcore.FtcOpMode; import ftclib.robotcore.FtcOpMode;
import ftclib.sensor.FtcRobotBattery; 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.LEDIndicator;
import teamcode.subsystems.RobotBase; import teamcode.subsystems.RobotBase;
import teamcode.subsystems.RumbleIndicator; import teamcode.subsystems.RumbleIndicator;
import teamcode.subsystems.ServoWrist;
import teamcode.subsystems.Shooter;
import teamcode.vision.Vision; import teamcode.vision.Vision;
import trclib.dataprocessor.TrcDiscreteValue;
import trclib.motor.TrcMotor; import trclib.motor.TrcMotor;
import trclib.motor.TrcServo; import trclib.motor.TrcServo;
import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose2D;
@ -40,7 +48,11 @@ import trclib.robotcore.TrcDbgTrace;
import trclib.robotcore.TrcEvent; import trclib.robotcore.TrcEvent;
import trclib.robotcore.TrcRobot; import trclib.robotcore.TrcRobot;
import trclib.sensor.TrcDigitalInput; import trclib.sensor.TrcDigitalInput;
import trclib.subsystem.TrcIntake;
import trclib.subsystem.TrcServoClaw;
import trclib.subsystem.TrcShooter;
import trclib.subsystem.TrcSubsystem; 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. * 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 RumbleIndicator operatorRumble;
public FtcRobotBattery battery; public FtcRobotBattery battery;
// Subsystems. // 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. // Autotasks.
/** /**
@ -114,7 +134,45 @@ public class Robot
if (RobotParams.Preferences.useSubsystems) if (RobotParams.Preferences.useSubsystems)
{ {
// Create subsystems. // 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. // 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. // 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) if (runMode == TrcRobot.RunMode.AUTO_MODE || FtcAuto.autoChoices.alliance == null)

11
TeamCode/src/main/java/teamcode/RobotParams.java
View File

@ -75,9 +75,20 @@ public class RobotParams
public static final boolean useSparkfunOTOS = false; public static final boolean useSparkfunOTOS = false;
// Subsystems // Subsystems
public static final boolean useSubsystems = false; 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 // Tuning
public static final boolean tuneColorBlobVision = false; public static final boolean tuneColorBlobVision = false;
public static final boolean tuneDriveBase = 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 } //class Preferences
/** /**

168
TeamCode/src/main/java/teamcode/subsystems/Arm.java Normal file
View File

@ -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

187
TeamCode/src/main/java/teamcode/subsystems/Claw.java Normal file
View File

@ -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

286
TeamCode/src/main/java/teamcode/subsystems/DiffyServoWrist.java Normal file
View File

@ -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

163
TeamCode/src/main/java/teamcode/subsystems/Elevator.java Normal file
View File

@ -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

136
TeamCode/src/main/java/teamcode/subsystems/Intake.java Normal file
View File

@ -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

133
TeamCode/src/main/java/teamcode/subsystems/ServoWrist.java Normal file
View File

@ -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

180
TeamCode/src/main/java/teamcode/subsystems/Shooter.java Normal file
View File

@ -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