Merge branch 'mechanism_testing' of https://github.com/Team-4795/2025…

…-Reefscape-Beastbots into mechanism_testing
Team-4795 · Feb 17, 2025 · 2c37d5a · 2c37d5a
2 parents 98b9b9b + 15280d3
commit 2c37d5a
Show file tree

Hide file tree

Showing 6 changed files with 40 additions and 45 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -36,7 +36,6 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOSpark;
-import frc.robot.subsystems.goProServo.goProServo;
 import frc.robot.subsystems.pivot.Pivot;
 import frc.robot.subsystems.pivot.PivotIOReal;
 import frc.robot.subsystems.pivot.PivotIOSim;
@@ -144,28 +143,20 @@ private void configureButtonBindings() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
             drive,
-            () -> -driverController.getLeftY(),
-            () -> -driverController.getLeftX(),
-            () -> -driverController.getRightX()));
+            () -> driverController.getLeftY() * 0.8,
+            () -> driverController.getLeftX() * 0.8,
+            () -> driverController.getRightX() * -0.75));
+
+    driverController.y().onTrue(Commands.runOnce(drive::zeroHeading, drive));
 
     operatorController
-        .povUp()
+        .rightBumper()
         .whileTrue(
             Commands.startEnd(() -> climb.setVoltage(ClimbConstants.ForwardV), climb::stop, climb));
     operatorController
-        .povDown()
+        .leftBumper()
         .whileTrue(
             Commands.startEnd(() -> climb.setVoltage(ClimbConstants.ReverseV), climb::stop, climb));
-    operatorController
-        .x()
-        .onTrue(
-            Commands.sequence(
-                Commands.startEnd(
-                        () -> climb.setVoltage(ClimbConstants.ReverseV), climb::stop, climb)
-                    .withTimeout(4.0),
-                Commands.startEnd(
-                        () -> climb.setVoltage(ClimbConstants.ForwardV), climb::stop, climb)
-                    .withTimeout(4.0)));
 
     // Lock to 0° when A button is held
     driverController
@@ -194,36 +185,28 @@ private void configureButtonBindings() {
     operatorController
         .b()
         .whileTrue(
-            Commands.startEnd(() -> intake.setIntakeVoltage(6), () -> intake.setIntakeVoltage(0)));
+            Commands.startEnd(() -> intake.setIntakeVoltage(3), () -> intake.setIntakeVoltage(0)));
 
     operatorController
         .a()
         .whileTrue(
-            Commands.startEnd(() -> intake.setIntakeVoltage(-6), () -> intake.setIntakeVoltage(0)));
+            Commands.startEnd(() -> intake.setIntakeVoltage(-3), () -> intake.setIntakeVoltage(0)));
 
     operatorController
         .rightTrigger()
         .whileTrue(
-            Commands.run(
-                () -> pivot.setVoltage(operatorController.getRightTriggerAxis() * 4), pivot));
+            Commands.startEnd(
+                () -> pivot.setVoltage(operatorController.getRightTriggerAxis() * 2),
+                () -> pivot.setVoltage(0),
+                pivot));
 
     operatorController
         .leftTrigger()
         .whileTrue(
-            Commands.run(
-                () -> pivot.setVoltage(operatorController.getLeftTriggerAxis() * 4), pivot));
-
-    driverController
-        .y()
-        .onTrue(
-            Commands.runOnce(
-                () -> goProServo.setServoPosition(180)));
-
-    driverController
-        .x()
-        .onTrue(
-            Commands.run(
-                () -> goProServo.setServoPosition(0)));
+            Commands.startEnd(
+                () -> pivot.setVoltage(operatorController.getLeftTriggerAxis() * -2),
+                () -> pivot.setVoltage(0),
+                pivot));
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java
@@ -1,14 +1,13 @@
 package frc.robot.subsystems.climb;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
 public class Climb extends SubsystemBase {
   private ClimbIO io;
   private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged();
 
-  private double velocity = 0.0;
+  private double appliedVolts = 0.0;
   private static Climb instance;
 
   private Climb(ClimbIO io) {
@@ -18,10 +17,12 @@ private Climb(ClimbIO io) {
 
   public void setVoltage(double volts) {
     io.setClimbVoltage(volts);
+    appliedVolts = volts;
   }
 
   public void stop() {
     io.setClimbVoltage(0);
+    appliedVolts = 0;
   }
 
   public static Climb getInstance() {
@@ -35,9 +36,9 @@ public static Climb init(ClimbIO io) {
     return instance;
   }
 
-  public void setVelocity(double v) {
-    this.velocity = v;
-  }
+  // public void setVelocity(double v) {
+  //   this.velocity = v;
+  // }
 
   public double getVelocity() {
     return inputs.velocityRPM;
@@ -55,6 +56,6 @@ public double getVoltage() {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("climb", inputs);
-    io.setClimbVoltage(MathUtil.clamp(velocity * 12, -12, 12));
+    io.setClimbVoltage(appliedVolts);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java
@@ -5,10 +5,10 @@ public final class ClimbConstants {
   public static final double kP = 1;
   public static final double kI = 1;
   public static final double kD = 1;
-  public static final int currentLimit = 40;
+  public static final int currentLimit = 50;
   public static final double gearing = 10;
   public static final double moi = 10;
 
-  public static final double ForwardV = 6.0;
-  public static final double ReverseV = -6.0;
+  public static final double ForwardV = 12.0;
+  public static final double ReverseV = -12.0;
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -218,6 +218,10 @@ public void stop() {
     runVelocity(new ChassisSpeeds());
   }
 
+  public void zeroHeading() {
+    gyroIO.zeroHeading();
+  }
+
   /**
    * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
    * return to their normal orientations the next time a nonzero velocity is requested.

diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -26,5 +26,7 @@ public static class GyroIOInputs {
     public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
   }
 
+  public default void zeroHeading() {}
+
   public default void updateInputs(GyroIOInputs inputs) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIORedux.java b/src/main/java/frc/robot/subsystems/drive/GyroIORedux.java
@@ -30,17 +30,22 @@ public GyroIORedux() {
     yawPositionQueue = SparkOdometryThread.getInstance().registerSignal(navX::getYaw);
   }
 
+  @Override
+  public void zeroHeading() {
+    navX.setYaw(0);
+  }
+
   @Override
   public void updateInputs(GyroIOInputs inputs) {
     inputs.connected = navX.isConnected();
-    inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw());
+    inputs.yawPosition = Rotation2d.fromRadians(-navX.getYaw() * 2 * Math.PI);
     inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getAngularVelocityYaw());
 
     inputs.odometryYawTimestamps =
         yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
     inputs.odometryYawPositions =
         yawPositionQueue.stream()
-            .map((Double value) -> Rotation2d.fromDegrees(-value))
+            .map((Double value) -> Rotation2d.fromRadians(value * 2 * Math.PI))
             .toArray(Rotation2d[]::new);
     yawTimestampQueue.clear();
     yawPositionQueue.clear();