diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 2cf370c..f11c6e3 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -53,5 +53,5 @@ public enum Subsystem { Subsystem.SHOOTER, Subsystem.SWERVE); - public static final Set REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ODOMETRY, Subsystem.SWERVE); + public static final Set REAL_SUBSYSTEMS = ALL_SUBSYSTEMS; } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index 6e88117..22c62e6 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -17,9 +17,9 @@ public record ArmState(State shoulder, State wrist) { public static final ArmState STOW = new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE); - public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(18)); + public static final ArmState SHOOT = STOW.withWrist(WristMotorConstants.MAXIMUM_ANGLE.plus(Rotation2d.fromDegrees(-65))); - public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(4)); + public static final ArmState INTAKE = STOW.withWrist(WristMotorConstants.MAXIMUM_ANGLE.plus(Rotation2d.fromDegrees(-79))); public static final ArmState AMP = new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java index e7e51c2..ee501ce 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java @@ -26,7 +26,7 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO { /** Creates a new shoulder motor using a Spark Max. */ public ShoulderMotorIOSparkMax() { - sparkMax = new CANSparkMax(ShoulderMotorConstants.CAN.id(), MotorType.kBrushless); + sparkMax = new CANSparkMax(1, MotorType.kBrushless); feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0); diff --git a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java index 54edb6b..0e58a90 100644 --- a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java @@ -23,8 +23,7 @@ public class WristMotorIOSparkMax implements WristMotorIO { private final AccelerationCalculator accelerationCalculator; public WristMotorIOSparkMax() { - // TODO test, use dedicated wrist SparkMAX - sparkMax = new CANSparkMax(1, MotorType.kBrushless); + sparkMax = new CANSparkMax(2, MotorType.kBrushless); feedback = new PIDController(WristMotorConstants.KP, 0, 0); diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index c8c0057..90810e4 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -140,7 +140,7 @@ public Command readyIntake() { } public Command intakeNote() { - return readyIntake().andThen(Commands.parallel(intake.intake(), shooter.intake())); + return readyIntake().andThen(intake.intake()); } public Command stow() { diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index e3d2870..a3b92a0 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -34,7 +34,7 @@ public static class PivotMotorConstants { public static final double DISTANCE = Units.inchesToMeters(10.275); /** Pivot motor's minimum angle. */ - public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-48); + public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-40); /** Pivot motor's maximum angle. */ public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(86);