Changed robot to proto arm

TitaniumTitans · Jan 27, 2024 · ef54c14 · ef54c14
1 parent bfb7814
commit ef54c14
Show file tree

Hide file tree

Showing 6 changed files with 30 additions and 16 deletions.
diff --git 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
@@ -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
@@ -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){
-    }
 }
diff --git 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
@@ -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
@@ -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();
   }
 }