diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3d57018..760e9fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,11 @@ package frc.robot; -import frc.robot.constants.ArmConstants; +import frc.robot.constants.ManipulatorPivotConstants; +import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.OIConstants; -import frc.robot.subsystems.Arm; +import frc.robot.subsystems.ManipulatorPivot; import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; @@ -27,8 +28,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { - private Arm arm; - private ClimberPivot climberPivot; private ClimberRollers climberRollers; @@ -39,14 +38,14 @@ public class RobotContainer { private Manipulator manipulator; + private ManipulatorPivot manipulatorPivot; + private CommandXboxController driver; private CommandXboxController operator; private SendableChooser autoChooser; public RobotContainer() { - arm = new Arm(); - climberPivot = new ClimberPivot(); climberRollers = new ClimberRollers(); @@ -57,6 +56,8 @@ public class RobotContainer { manipulator = new Manipulator(); + manipulatorPivot = new ManipulatorPivot(); + driver = new CommandXboxController(OIConstants.kDriverControllerPort); operator = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -71,10 +72,6 @@ public class RobotContainer { private void configureButtonBindings() { //Default commands - arm.setDefaultCommand( - arm.goToSetpoint(0, 1) - ); - climberPivot.setDefaultCommand( climberPivot.runPivot(0) ); @@ -97,7 +94,11 @@ public class RobotContainer { ); manipulator.setDefaultCommand( - manipulator.runManipulator(() -> 0, true) + manipulator.defaultCommand() + ); + + manipulatorPivot.setDefaultCommand( + manipulatorPivot.runAssistedArm(operator::getRightY) ); //Driver inputs @@ -109,32 +110,36 @@ public class RobotContainer { manipulator.runManipulator(() -> 1, true) ); + driver.start().and(driver.back()).onTrue( + startingConfig() + ); + //Operator inputs operator.povUp().onTrue( moveManipulator( - ElevatorConstants.kElevatorL4Position, - ArmConstants.kArmL4Position + ElevatorConstants.kL4Position, + ManipulatorPivotConstants.kL4Position ) ); operator.povRight().onTrue( moveManipulator( - ElevatorConstants.kElevatorL3Position, - ArmConstants.kArmL3Position + ElevatorConstants.kL3Position, + ManipulatorPivotConstants.kL3Position ) ); operator.povLeft().onTrue( moveManipulator( - ElevatorConstants.kElevatorL2Position, - ArmConstants.kArmL2Position + ElevatorConstants.kL2Position, + ManipulatorPivotConstants.kL2Position ) ); operator.povDown().onTrue( moveManipulator( - ElevatorConstants.kElevatorL1Position, - ArmConstants.kArmL1Position + ElevatorConstants.kL1Position, + ManipulatorPivotConstants.kL1Position ) ); @@ -172,7 +177,7 @@ public class RobotContainer { .withPosition(0, 0) .withWidget(BuiltInWidgets.kTextView); - sensorTab.addDouble("ArmPosition", arm::getEncoderPosition) + sensorTab.addDouble("ArmPosition", manipulatorPivot::getEncoderPosition) .withSize(2, 1) .withPosition(2, 0) .withWidget(BuiltInWidgets.kTextView); @@ -188,8 +193,8 @@ public class RobotContainer { */ private Command coralIntakeRoutine() { return moveManipulator( - ElevatorConstants.kElevatorCoralIntakePosition, - ArmConstants.kArmCoralIntakePosition + ElevatorConstants.kCoralIntakePosition, + ManipulatorPivotConstants.kCoralIntakePosition ) .andThen(manipulator.runUntilCollected(1, true)); } @@ -202,8 +207,8 @@ public class RobotContainer { */ private Command algaeIntakeRoutine(boolean l2) { return moveManipulator( - l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, - l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition + l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, + l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition ) .andThen(manipulator.runUntilCollected(1, false)); } @@ -218,23 +223,23 @@ public class RobotContainer { private Command moveManipulator(double elevatorPosition, double armPosition) { // If the elevator current and target positions are above the brace, or the arm current and target position is in // front of the brace, move together - if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (arm.isMotionSafe() && arm.isMotionSafe(armPosition))) { + if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { return moveManipulatorUtil(elevatorPosition, armPosition, false, false); // If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first, // then the elevator, then the arm again - } else if (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) { - return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true) - .andThen(arm.goToSetpoint(armPosition, 2)); + } else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { + return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) + .andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); // If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm - } else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) { + } else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) { return moveManipulatorUtil(elevatorPosition, armPosition, true, true); // If the arm is behind the brace, move the arm first, then the elevator - } else if (!arm.isMotionSafe()) { + } else if (!manipulatorPivot.isMotionSafe()) { return moveManipulatorUtil(elevatorPosition, armPosition, false, true); // Catch all command that's safe regardless of arm and elevator positions } else { - return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true) - .andThen(arm.goToSetpoint(armPosition, 2)); + return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) + .andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); } } @@ -248,23 +253,23 @@ public class RobotContainer { * @return Moves the elevator and arm to the setpoints */ private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) { - if (elevatorPosition <= ElevatorConstants.kElevatorBracePosition || elevatorPosition == 0) { + if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) { armPosition = MathUtil.clamp( armPosition, 0, - ArmConstants.kArmRotationLimit + ManipulatorPivotConstants.kRotationLimit ); } return Commands.either( Commands.either( - elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)), - elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)), + elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)), + elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)), () -> sequential ), Commands.either( - arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), - arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), + manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), + manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), () -> sequential ), () -> elevatorFirst @@ -280,7 +285,12 @@ public class RobotContainer { */ @SuppressWarnings("unused") private Command safeMoveManipulator(double elevatorPosition, double armPosition) { - return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true) - .andThen(arm.goToSetpoint(armPosition, 2)); + return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true) + .andThen(manipulatorPivot.goToSetpoint(armPosition, 2)); + } + + private Command startingConfig() { + return moveManipulatorUtil(0, 0, false, true) + .alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ClimberPivotConstants.java b/src/main/java/frc/robot/constants/ClimberPivotConstants.java index a0d59f4..357b6f7 100644 --- a/src/main/java/frc/robot/constants/ClimberPivotConstants.java +++ b/src/main/java/frc/robot/constants/ClimberPivotConstants.java @@ -14,6 +14,7 @@ public class ClimberPivotConstants { public static final double kPIDControllerD = 0; public static final double kClimberClimbPosition = 0; + public static final double kClimberStartingPosition = 0; public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index a70e003..6982271 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -33,18 +33,18 @@ public class ElevatorConstants { public static final double kFeedForwardG = 0; public static final double kFeedForwardV = 0; - public static final double kElevatorMaxVelocity = 0; + public static final double kMaxVelocity = 0; - public static final double kElevatorCoralIntakePosition = 0; - public static final double kElevatorL1Position = 0; - public static final double kElevatorL2Position = 0; - public static final double kElevatorL3Position = 0; - public static final double kElevatorL4Position = 0; - public static final double kElevatorL2AlgaePosition = 0; - public static final double kElevatorL3AlgaePosition = 0; + public static final double kCoralIntakePosition = 0; + public static final double kL1Position = 0; + public static final double kL2Position = 0; + public static final double kL3Position = 0; + public static final double kL4Position = 0; + public static final double kL2AlgaePosition = 0; + public static final double kL3AlgaePosition = 0; /**The position of the top of the elevator brace */ - public static final double kElevatorBracePosition = 0; - public static final double kElevatorMaxHeight = 0; + public static final double kBracePosition = 0; + public static final double kMaxHeight = 0; // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = 1; diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java similarity index 84% rename from src/main/java/frc/robot/constants/ArmConstants.java rename to src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index f4cf608..5d8196f 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -13,13 +13,13 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -public class ArmConstants { +public class ManipulatorPivotConstants { public static final int kArmMotorID = 0; public static final int kCANcoderID = 0; public static final int kMotorAmpsMax = 0; - public static final double kArmMaxVelocity = 0; + public static final double kPivotMaxVelocity = 0; public static final double kPositionalP = 0; public static final double kPositionalI = 0; @@ -32,17 +32,17 @@ public class ArmConstants { // TODO Is this reasonable? public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60; - public static final double kArmCoralIntakePosition = 0; - public static final double kArmL1Position = 0; - public static final double kArmL2Position = 0; - public static final double kArmL3Position = 0; - public static final double kArmL4Position = 0; - public static final double kArmL2AlgaePosition = 0; - public static final double kArmL3AlgaePosition = 0; + public static final double kCoralIntakePosition = 0; + public static final double kL1Position = 0; + public static final double kL2Position = 0; + public static final double kL3Position = 0; + public static final double kL4Position = 0; + public static final double kL2AlgaePosition = 0; + public static final double kL3AlgaePosition = 0; /**The closest position to the elevator brace without hitting it */ public static final double kArmSafeStowPosition = 0; /**The forward rotation limit of the arm */ - public static final double kArmRotationLimit = 0; + public static final double kRotationLimit = 0; public static final double kMagnetOffset = 0.0; public static final double kAbsoluteSensorDiscontinuityPoint = 0.0; diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java index 636f18a..a8ee3b4 100644 --- a/src/main/java/frc/robot/subsystems/ClimberPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -42,15 +42,16 @@ public class ClimberPivot extends SubsystemBase { } /** - * Runs the climber until it is at a defined setpoint + * Runs the climber until it is at setpoint * * @param speed The speed at which the pivot runs - * @return Sets the motor speed until at the climbed position + * @param setpoint The target position of the climber + * @return Sets the motor speed until at the target position */ - public Command climb(double speed) { + public Command climb(double setpoint, double speed) { return run(() -> { pivotMotor.set(speed); - }).until(() -> neoEncoder.getPosition() >= ClimberPivotConstants.kClimberClimbPosition); + }).until(() -> neoEncoder.getPosition() >= setpoint); } /** diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 71319ba..b7c647e 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; @@ -102,7 +103,7 @@ public class Elevator extends SubsystemBase { * @return Is the motion safe */ public boolean isMotionSafe(double motionTarget) { - return motionTarget > ElevatorConstants.kElevatorBracePosition; + return motionTarget > ElevatorConstants.kBracePosition; } /** @@ -125,7 +126,7 @@ public class Elevator extends SubsystemBase { */ public Command runAssistedElevator(DoubleSupplier speed) { return run(() -> { - double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity; + double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity; double voltsOut = velocityController.calculate( encoder.getVelocity(), @@ -134,11 +135,12 @@ public class Elevator extends SubsystemBase { elevatorMotor1.setVoltage(voltsOut); }).until( - () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight); + () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight); } /** - * Moves the elevator to a target destination (setpoint) + * Moves the elevator to a target destination (setpoint). + * If the setpoint is 0, the elevator will creep down to hit the limit switch * * @param setpoint Target destination of the subsystem * @param timeout Time to achieve the setpoint before quitting @@ -148,19 +150,37 @@ public class Elevator extends SubsystemBase { double clampedSetpoint = MathUtil.clamp( setpoint, 0, - ElevatorConstants.kElevatorMaxHeight + ElevatorConstants.kMaxHeight ); - return run(() -> { - double voltsOut = positionController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(0); - - elevatorMotor1.setVoltage(voltsOut); - }).until( - () -> positionController.atSetpoint() || bottomLimitSwitch.get() - ).withTimeout(timeout); + if (clampedSetpoint == 0) { + return run(() -> { + double voltsOut = positionController.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(0); + + elevatorMotor1.setVoltage(voltsOut); + }).until( + () -> positionController.atSetpoint() || bottomLimitSwitch.get() + ).withTimeout(timeout) + .andThen(Commands.either( + runAssistedElevator(() -> 0), + runAssistedElevator(() -> -.2), + bottomLimitSwitch::get + )).withTimeout(timeout); + } else { + return run(() -> { + double voltsOut = positionController.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(0); + + elevatorMotor1.setVoltage(voltsOut); + }).until( + () -> positionController.atSetpoint() || bottomLimitSwitch.get() + ).withTimeout(timeout); + } } /** diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index 9217965..90e431b 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -16,7 +16,7 @@ public class Manipulator extends SubsystemBase { private SparkMax manipulatorMotor; private DigitalInput coralBeamBreak; - private DigitalInput algaeBeamBreak; + private DigitalInput algaeBeamBreak; public Manipulator() { manipulatorMotor = new SparkMax( @@ -34,16 +34,32 @@ public class Manipulator extends SubsystemBase { algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID); } + /** + * The default command for the manipulator that either stops the manipulator or slowly + * runs the manipulator to retain the algae + * + * @return Returns a command that sets the speed of the motor + */ + public Command defaultCommand() { + return run(() -> { + manipulatorMotor.set( + algaeBeamBreak.get() ? 0.1 : 0 + ); + }); + } + /** * Runs the manipulator at a set speed with the direction based on the coral parameter * * @param speed The speed at which the manipulator runs * @param coral Is the manipulator manipulating a coral? (True = Coral, False = Algae) - * @return Runs manipulator at speed + * @return Returns a command that sets the speed of the motor */ public Command runManipulator(DoubleSupplier speed, boolean coral) { return run(() -> { - manipulatorMotor.set(coral ? speed.getAsDouble() : speed.getAsDouble() * -1); + manipulatorMotor.set( + coral ? speed.getAsDouble() : speed.getAsDouble() * -1 + ); }); } @@ -52,11 +68,13 @@ public class Manipulator extends SubsystemBase { * * @param speed The speed at which the manipulator is run * @param coral Is the object a coral? (True = Coral, False = Algae) - * @return Runs the manipulator until an object is present + * @return Returns a command that sets the speed of the motor */ public Command runUntilCollected(double speed, boolean coral) { return run(() -> { - manipulatorMotor.set(coral ? speed : speed * -1); + manipulatorMotor.set( + coral ? speed : speed * -1 + ); }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get()); } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java similarity index 78% rename from src/main/java/frc/robot/subsystems/Arm.java rename to src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 76b275d..214383b 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -14,9 +14,9 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.ArmConstants; +import frc.robot.constants.ManipulatorPivotConstants; -public class Arm extends SubsystemBase { +public class ManipulatorPivot extends SubsystemBase { protected SparkMax armMotor; private CANcoder canCoder; @@ -26,38 +26,38 @@ public class Arm extends SubsystemBase { private ArmFeedforward feedForward; - public Arm() { + public ManipulatorPivot() { armMotor = new SparkMax( - ArmConstants.kArmMotorID, + ManipulatorPivotConstants.kArmMotorID, MotorType.kBrushless ); armMotor.configure( - ArmConstants.motorConfig, + ManipulatorPivotConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); positionController = new PIDController( - ArmConstants.kPositionalP, - ArmConstants.kPositionalI, - ArmConstants.kPositionalD + ManipulatorPivotConstants.kPositionalP, + ManipulatorPivotConstants.kPositionalI, + ManipulatorPivotConstants.kPositionalD ); // TODO: Generate constants for continuous input range based on CANcoder configuration? positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179)); - positionController.setTolerance(ArmConstants.kPositionalTolerance); + positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance); velocityController = new PIDController( - ArmConstants.kVelocityP, - ArmConstants.kVelocityI, - ArmConstants.kVelocityD + ManipulatorPivotConstants.kVelocityP, + ManipulatorPivotConstants.kVelocityI, + ManipulatorPivotConstants.kVelocityD ); - velocityController.setTolerance(ArmConstants.kVelocityTolerance); + velocityController.setTolerance(ManipulatorPivotConstants.kVelocityTolerance); - canCoder = new CANcoder(ArmConstants.kCANcoderID); - canCoder.getConfigurator().apply(ArmConstants.canCoderConfig); + canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID); + canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig); } /** @@ -78,7 +78,7 @@ public class Arm extends SubsystemBase { * @return Is the motion safe */ public boolean isMotionSafe(double motionTarget) { - return motionTarget > ArmConstants.kArmSafeStowPosition; + return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition; } /** @@ -95,7 +95,7 @@ public class Arm extends SubsystemBase { ); return run(() -> { - double realSpeedTarget = clampedSpeed * ArmConstants.kArmMaxVelocity; + double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity; double voltsOut = velocityController.calculate( getEncoderVelocity(), @@ -120,7 +120,7 @@ public class Arm extends SubsystemBase { double clampedSetpoint = MathUtil.clamp( setpoint, 0, - ArmConstants.kArmRotationLimit + ManipulatorPivotConstants.kRotationLimit ); return run(() -> { diff --git a/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java b/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java index 08bafa7..b8b9218 100644 --- a/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java +++ b/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java @@ -10,10 +10,10 @@ import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.constants.ArmConstants; -import frc.robot.subsystems.Arm; +import frc.robot.constants.ManipulatorPivotConstants; +import frc.robot.subsystems.ManipulatorPivot; -public class ArmSysID extends Arm { +public class ArmSysID extends ManipulatorPivot { private MutVoltage appliedVoltage; private MutAngle armPosition; @@ -32,7 +32,7 @@ public class ArmSysID extends Arm { armVelocity = RadiansPerSecond.mutable(0); routine = new SysIdRoutine( - ArmConstants.kSysIDConfig, + ManipulatorPivotConstants.kSysIDConfig, new SysIdRoutine.Mechanism( armMotor::setVoltage, (log) -> {