diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9fa95252..9e386ac2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,11 +27,12 @@ public final class Constants { private Constants() { throw new IllegalStateException("Constants class should not be constructed"); } - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.PROTO_ARM; public enum Mode { /** Running on a real robot. */ REAL, + PROTO_ARM, /** Running a physics simulator. */ SIM, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 235a2204..ca8a7510 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,6 +74,17 @@ public RobotContainer() { m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); m_armSubsystem = new ArmSubsystem(new ArmIOPrototype()); } + case PROTO_ARM -> { + m_driveSubsystem = new DriveSubsystem( + new GyroIO() { + }, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + m_shooter = new ShooterSubsystem(new ShooterIO() {}); + m_armSubsystem = new ArmSubsystem(new ArmIOPrototype()); + } case SIM -> { // Sim robot, instantiate physics sim IO implementations m_driveSubsystem = @@ -148,13 +159,15 @@ private void configureButtonBindings() { new Pose2d(m_driveSubsystem.getPose().getTranslation(), new Rotation2d())), m_driveSubsystem) .ignoringDisable(true)); - controller.a().whileTrue(m_armSubsystem.setShoulderPowerFactory()) + + controller.a().whileTrue(m_armSubsystem.setShoulderPowerFactory(armPower.get())) .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0)); - controller.y().whileTrue(m_armSubsystem.setShoulderPowerFactory()) - .whileFalse(m_armSubsystem.setWristPowerFactory(0.0)); - controller.leftBumper().whileTrue(m_armSubsystem.setWristPowerFactory()) + controller.y().whileTrue(m_armSubsystem.setShoulderPowerFactory(-armPower.get())) + .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0)); + + controller.leftBumper().whileTrue(m_armSubsystem.setWristPowerFactory(wristPower.get())) .whileFalse(m_armSubsystem.setWristPowerFactory(0.0)); - controller.rightBumper().whileTrue(m_armSubsystem.setWristPowerFactory()) + controller.rightBumper().whileTrue(m_armSubsystem.setWristPowerFactory(-wristPower.get())) .whileFalse(m_armSubsystem.setWristPowerFactory(0.0)); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java index a10773b2..29e80aed 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java @@ -16,16 +16,8 @@ public void setShoulderVoltage(double voltage){ m_shoulder.setVoltage(voltage); } - @Override - public void setShoulderAngle(double degrees){ - } - @Override public void setWristVoltage(double voltage){ m_wrist.setVoltage(voltage); } - - @Override - public void setWristAngle(double degrees){ - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index e3695981..aaa6de9a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -14,7 +14,7 @@ public void setShoulderPower(double power){ m_io.setShoulderVoltage(power * 12.0); } public void setWristPower(double power){ - m_io.setWristVoltage(power * 12); + m_io.setWristVoltage(power * 12.0); } public Command setShoulderPowerFactory(double power) { diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java index 9d766357..d215cf77 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java @@ -19,6 +19,14 @@ public record ModuleConstants( double TURN_KP, double TURN_KI, double TURN_KD) { public static final double DEFAULT_WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0); + public ModuleConstants() { + this (0, new int[] {0, 0, 0}, + new double[] {0.0, 0.0, 0.0}, + new double[] {0.0, 0.0, 0.0}, + new double[] {0.0, 0.0, 0.0}, + 0.0, false, GearRatios.L3); + } + public enum GearRatios { TURN(150.0 / 7.0), L1((14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0)), diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index 6b5bac51..e364670f 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -139,6 +139,6 @@ default void setTurnBrakeMode(boolean enable) {} // Used to pass moduleConstants default ModuleConstants getModuleConstants() { - throw new UnsupportedOperationException("getModuleConstants() not implemented"); + return new ModuleConstants(); } }