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

Renamed Arm subsystem to DcMotorArm.

Browse Source 
Added CrServoArm subsystem sample.
This commit is contained in:
Titan Robotics Club
2025-05-12 14:19:13 -07:00
parent e63ba1a457
commit f4c8b1fd14
6 changed files with 270 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

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

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

176
TeamCode/src/main/java/teamcode/subsystems/CrServoArm.java Normal file
View File

@ -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 (<a href="https://trc492.github.io">...</a>) 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

18
TeamCode/src/main/java/teamcode/subsystems/Arm.java → TeamCode/src/main/java/teamcode/subsystems/DcMotorArm.java
View File

@ -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 (<a href="https://trc492.github.io">...</a>) 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