diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java
index 90c76be..8b20ddf 100644
--- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java
+++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java
@@ -29,7 +29,8 @@ import java.util.Locale;
import ftclib.drivebase.FtcSwerveDrive;
import ftclib.driverio.FtcGamepad;
import ftclib.robotcore.FtcOpMode;
-import teamcode.subsystems.Arm;
+import teamcode.subsystems.CrServoArm;
+import teamcode.subsystems.DcMotorArm;
import teamcode.subsystems.Elevator;
import teamcode.subsystems.Intake;
import teamcode.subsystems.RumbleIndicator;
@@ -268,7 +269,7 @@ public class FtcTeleOp extends FtcOpMode
prevElevatorPower = elevatorPower;
}
}
- else if (robot.arm != null)
+ else if (robot.dcMotorArm != null)
{
double armPower = operatorGamepad.getLeftStickY(true);
if (armPower != prevArmPower)
@@ -276,11 +277,30 @@ public class FtcTeleOp extends FtcOpMode
if (operatorAltFunc)
{
// Manual override.
- robot.arm.setPower(armPower);
+ robot.dcMotorArm.setPower(armPower);
}
else
{
- robot.arm.setPidPower(armPower, Arm.Params.MIN_POS, Arm.Params.MAX_POS, true);
+ robot.dcMotorArm.setPidPower(
+ armPower, DcMotorArm.Params.MIN_POS, DcMotorArm.Params.MAX_POS, true);
+ }
+ prevArmPower = armPower;
+ }
+ }
+ else if (robot.crServoArm != null)
+ {
+ double armPower = operatorGamepad.getLeftStickY(true);
+ if (armPower != prevArmPower)
+ {
+ if (operatorAltFunc)
+ {
+ // Manual override.
+ robot.crServoArm.setPower(armPower);
+ }
+ else
+ {
+ robot.crServoArm.setPidPower(
+ armPower, CrServoArm.Params.MIN_POS, CrServoArm.Params.MAX_POS, true);
}
prevArmPower = armPower;
}
@@ -626,11 +646,18 @@ public class FtcTeleOp extends FtcOpMode
robot.elevator.presetPositionUp(null, Elevator.Params.POWER_LIMIT);
}
}
- else if (robot.arm != null)
+ else if (robot.dcMotorArm != null)
{
if (pressed)
{
- robot.arm.presetPositionUp(null, Arm.Params.POWER_LIMIT);
+ robot.dcMotorArm.presetPositionUp(null, DcMotorArm.Params.POWER_LIMIT);
+ }
+ }
+ else if (robot.crServoArm != null)
+ {
+ if (pressed)
+ {
+ robot.crServoArm.presetPositionUp(null, CrServoArm.Params.POWER_LIMIT);
}
}
else if (robot.shooter != null)
@@ -664,11 +691,18 @@ public class FtcTeleOp extends FtcOpMode
robot.elevator.presetPositionDown(null, Elevator.Params.POWER_LIMIT);
}
}
- else if (robot.arm != null)
+ else if (robot.dcMotorArm != null)
{
if (pressed)
{
- robot.arm.presetPositionDown(null, Arm.Params.POWER_LIMIT);
+ robot.dcMotorArm.presetPositionDown(null, DcMotorArm.Params.POWER_LIMIT);
+ }
+ }
+ else if (robot.crServoArm != null)
+ {
+ if (pressed)
+ {
+ robot.crServoArm.presetPositionDown(null, CrServoArm.Params.POWER_LIMIT);
}
}
else if (robot.shooter != null)
diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java
index 3118427..d83d828 100644
--- a/TeamCode/src/main/java/teamcode/FtcTest.java
+++ b/TeamCode/src/main/java/teamcode/FtcTest.java
@@ -610,15 +610,26 @@ public class FtcTest extends FtcTeleOp
robot.elevator.presetPositionUp(null, Dashboard.Subsystem.powerLimit);
}
}
- else if (RobotParams.Preferences.tuneArm)
+ else if (RobotParams.Preferences.tuneDcMotorArm)
{
if (pressed)
{
- robot.arm.setPositionPidParameters(
+ robot.dcMotorArm.setPositionPidParameters(
Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs,
Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid,
Dashboard.Subsystem.enableSquid);
- robot.arm.presetPositionUp(null, Dashboard.Subsystem.powerLimit);
+ robot.dcMotorArm.presetPositionUp(null, Dashboard.Subsystem.powerLimit);
+ }
+ }
+ else if (RobotParams.Preferences.tuneCrServoArm)
+ {
+ if (pressed)
+ {
+ robot.crServoArm.setPositionPidParameters(
+ Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs,
+ Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid,
+ Dashboard.Subsystem.enableSquid);
+ robot.crServoArm.presetPositionUp(null, Dashboard.Subsystem.powerLimit);
}
}
else if (RobotParams.Preferences.tuneTurret)
@@ -682,15 +693,26 @@ public class FtcTest extends FtcTeleOp
robot.elevator.presetPositionDown(null, Dashboard.Subsystem.powerLimit);
}
}
- else if (RobotParams.Preferences.tuneArm)
+ else if (RobotParams.Preferences.tuneDcMotorArm)
{
if (pressed)
{
- robot.arm.setPositionPidParameters(
+ robot.dcMotorArm.setPositionPidParameters(
Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs,
Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid,
Dashboard.Subsystem.enableSquid);
- robot.arm.presetPositionDown(null, Dashboard.Subsystem.powerLimit);
+ robot.dcMotorArm.presetPositionDown(null, Dashboard.Subsystem.powerLimit);
+ }
+ }
+ else if (RobotParams.Preferences.tuneCrServoArm)
+ {
+ if (pressed)
+ {
+ robot.crServoArm.setPositionPidParameters(
+ Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs,
+ Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid,
+ Dashboard.Subsystem.enableSquid);
+ robot.crServoArm.presetPositionDown(null, Dashboard.Subsystem.powerLimit);
}
}
else if (RobotParams.Preferences.tuneTurret)
diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java
index 35ef66f..ff3b784 100644
--- a/TeamCode/src/main/java/teamcode/Robot.java
+++ b/TeamCode/src/main/java/teamcode/Robot.java
@@ -29,7 +29,8 @@ import ftclib.driverio.FtcDashboard;
import ftclib.driverio.FtcMatchInfo;
import ftclib.robotcore.FtcOpMode;
import ftclib.sensor.FtcRobotBattery;
-import teamcode.subsystems.Arm;
+import teamcode.subsystems.CrServoArm;
+import teamcode.subsystems.DcMotorArm;
import teamcode.subsystems.Claw;
import teamcode.subsystems.DiffyServoWrist;
import teamcode.subsystems.Elevator;
@@ -80,7 +81,8 @@ public class Robot
public FtcRobotBattery battery;
// Subsystems.
public TrcMotor elevator;
- public TrcMotor arm;
+ public TrcMotor dcMotorArm;
+ public TrcMotor crServoArm;
public TrcMotor turret;
public TrcShooter shooter;
public TrcDiscreteValue shooterVelocity;
@@ -142,9 +144,14 @@ public class Robot
elevator = new Elevator().getMotor();
}
- if (RobotParams.Preferences.useArm)
+ if (RobotParams.Preferences.useDcMotorArm)
{
- arm = new Arm().getMotor();
+ dcMotorArm = new DcMotorArm().getMotor();
+ }
+
+ if (RobotParams.Preferences.useCrServoArm)
+ {
+ crServoArm = new CrServoArm().getMotor();
}
if (RobotParams.Preferences.useTurret)
diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java
index 269fe69..b44bbb4 100644
--- a/TeamCode/src/main/java/teamcode/RobotParams.java
+++ b/TeamCode/src/main/java/teamcode/RobotParams.java
@@ -76,7 +76,8 @@ public class RobotParams
// Subsystems
public static final boolean useSubsystems = false;
public static final boolean useElevator = false;
- public static final boolean useArm = false;
+ public static final boolean useDcMotorArm = false;
+ public static final boolean useCrServoArm = false;
public static final boolean useTurret = false;
public static final boolean useShooter = false;
public static final boolean useIntake = false;
@@ -88,7 +89,8 @@ public class RobotParams
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 tuneDcMotorArm = false;
+ public static final boolean tuneCrServoArm = false;
public static final boolean tuneTurret = false;
public static final boolean tuneShooterMotor1 = false;
public static final boolean tuneShooterMotor2 = false;
diff --git a/TeamCode/src/main/java/teamcode/subsystems/CrServoArm.java b/TeamCode/src/main/java/teamcode/subsystems/CrServoArm.java
new file mode 100644
index 0000000..e299aa2
--- /dev/null
+++ b/TeamCode/src/main/java/teamcode/subsystems/CrServoArm.java
@@ -0,0 +1,176 @@
+/*
+ * 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.FtcMotorActuator;
+import trclib.controller.TrcPidController;
+import trclib.motor.TrcMotor;
+import trclib.robotcore.TrcEvent;
+import trclib.subsystem.TrcSubsystem;
+
+/**
+ * This class implements a CrServoArm Subsystem. This implementation consists of two Axon servos running in Continuous
+ * Rotation mode with an analog absolute encoder. It does not require zero calibration. Therefore, limit switches are
+ * optional. If using limit switches, they are for movement range protection. If not using limit switches, software
+ * limit must be set. It supports gravity compensation by computing the power required to hold the arm at its current
+ * angle.
+ * There are many possible implementations by setting different parameters.
+ * Please refer to the TrcLib documentation (...) for details.
+ */
+public class CrServoArm extends TrcSubsystem
+{
+ public static final class Params
+ {
+ public static final String SUBSYSTEM_NAME = "CrServoArm";
+ public static final boolean NEED_ZERO_CAL = false;
+
+ public static final String PRIMARY_MOTOR_NAME = SUBSYSTEM_NAME + ".primary";
+ public static final FtcMotorActuator.MotorType PRIMARY_MOTOR_TYPE =
+ FtcMotorActuator.MotorType.CRServo;
+ public static final boolean PRIMARY_MOTOR_INVERTED = false;
+
+ public static final String FOLLOWER_MOTOR_NAME = SUBSYSTEM_NAME + ".follower";
+ public static final FtcMotorActuator.MotorType FOLLOWER_MOTOR_TYPE =
+ FtcMotorActuator.MotorType.CRServo;
+ public static final boolean FOLLOWER_MOTOR_INVERTED = true;
+
+ public static final double POWER_LIMIT = 0.25;
+
+ public static final String ABSENC_NAME = SUBSYSTEM_NAME + ".enc";
+ public static final boolean ABSENC_INVERTED = true;
+ public static final double ABSENC_ZERO_OFFSET = 0.949697;
+
+ public static final double POS_OFFSET = 27.0;
+ public static final double POS_DEG_SCALE = 360.0;
+ public static final double MIN_POS = 27.3;
+ public static final double MAX_POS = 300.0;
+
+ public static final double TURTLE_POS = MIN_POS;
+ public static final double TURTLE_DELAY = 0.0;
+ public static final double[] posPresets = {
+ 30.0, 60.0, 90.0, 120.0, 150.0, 180.0, 210.0, 240.0, 270.0};
+ public static final double POS_PRESET_TOLERANCE = 5.0;
+
+ public static final boolean SOFTWARE_PID_ENABLED = true;
+ public static final TrcPidController.PidCoefficients posPidCoeffs =
+ new TrcPidController.PidCoefficients(0.0162, 0.0, 0.0, 0.0, 2.0);
+ public static final double POS_PID_TOLERANCE = 1.0;
+ public static final double GRAVITY_COMP_MAX_POWER = 0.1675;
+ } //class Params
+
+ private final FtcDashboard dashboard;
+ private final TrcMotor motor;
+
+ /**
+ * Constructor: Creates an instance of the object.
+ */
+ public CrServoArm()
+ {
+ super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL);
+
+ dashboard = FtcDashboard.getInstance();
+ FtcMotorActuator.Params motorParams = new FtcMotorActuator.Params()
+ .setPrimaryMotor(Params.PRIMARY_MOTOR_NAME, Params.PRIMARY_MOTOR_TYPE, Params.PRIMARY_MOTOR_INVERTED)
+ .setFollowerMotor(Params.FOLLOWER_MOTOR_NAME, Params.FOLLOWER_MOTOR_TYPE, Params.FOLLOWER_MOTOR_INVERTED)
+ .setExternalEncoder(Params.ABSENC_NAME, Params.ABSENC_INVERTED)
+ .setPositionScaleAndOffset(Params.POS_DEG_SCALE, Params.POS_OFFSET, Params.ABSENC_ZERO_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);
+ motor.setSoftPositionLimits(Params.MIN_POS, Params.MAX_POS, false);
+ } //CrServoArm
+
+ /**
+ * This method returns the created CrServoArm 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)
+ {
+ // 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()
+ {
+ 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 CrServoArm
diff --git a/TeamCode/src/main/java/teamcode/subsystems/Arm.java b/TeamCode/src/main/java/teamcode/subsystems/DcMotorArm.java
similarity index 92%
rename from TeamCode/src/main/java/teamcode/subsystems/Arm.java
rename to TeamCode/src/main/java/teamcode/subsystems/DcMotorArm.java
index 06da604..46745c6 100644
--- a/TeamCode/src/main/java/teamcode/subsystems/Arm.java
+++ b/TeamCode/src/main/java/teamcode/subsystems/DcMotorArm.java
@@ -30,17 +30,17 @@ import trclib.robotcore.TrcEvent;
import trclib.subsystem.TrcSubsystem;
/**
- * This class implements an Arm Subsystem. This implementation 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.
+ * This class implements a DcMotorArm Subsystem. This implementation 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.
* There are many possible implementations by setting different parameters.
* Please refer to the TrcLib documentation (...) for details.
*/
-public class Arm extends TrcSubsystem
+public class DcMotorArm extends TrcSubsystem
{
public static final class Params
{
- public static final String SUBSYSTEM_NAME = "Arm";
+ public static final String SUBSYSTEM_NAME = "DcMotorArm";
public static final boolean NEED_ZERO_CAL = true;
public static final String MOTOR_NAME = SUBSYSTEM_NAME + ".motor";
@@ -79,7 +79,7 @@ public class Arm extends TrcSubsystem
/**
* Constructor: Creates an instance of the object.
*/
- public Arm()
+ public DcMotorArm()
{
super(Params.SUBSYSTEM_NAME, Params.NEED_ZERO_CAL);
@@ -95,10 +95,10 @@ public class Arm extends TrcSubsystem
motor.setStallProtection(
Params.STALL_MIN_POWER, Params.STALL_TOLERANCE, Params.STALL_TIMEOUT, Params.STALL_RESET_TIMEOUT);
motor.setSoftPositionLimits(Params.MIN_POS, Params.MAX_POS, false);
- } //Arm
+ } //DcMotorArm
/**
- * This method returns the created Arm motor.
+ * This method returns the created DcMotorArm motor.
*
* @return created arm motor.
*/
@@ -167,4 +167,4 @@ public class Arm extends TrcSubsystem
return lineNum;
} //updateStatus
-} //class Arm
+} //class DcMotorArm