Merge remote-tracking branch 'origin/reveal-night' into shikhar-op-controls

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -217,9 +217,9 @@ public class Shooter extends SubsystemBase {
 
                 io.setShooterCurrentLimitSpeed(
                     state, 
-                    ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get(), 
+                    ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get()
-                    Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), 
+                    // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), 
-                    RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
+                    // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
                 );
                 io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
                 m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -23,8 +23,8 @@ public class    ShooterConstants {
     // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
     
     public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3);
-    public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
+    // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
-    public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
+    // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
 
     public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
     public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);

src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java

@@ -40,9 +40,9 @@ public interface ShooterIO {
     public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
     public default void setShooterCurrentLimitSpeed(
         ShooterState state, 
-        double percentOutput,
+        double percentOutput
-        Current currentLimit,
+        // Current currentLimit,
-        AngularVelocity targetVelocity
+        // AngularVelocity targetVelocity
     ) {}
     // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
     public default void setIndexerOutput(ShooterState state, double percentOutput) {}

src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java

@@ -61,26 +61,26 @@ public class ShooterReal implements ShooterIO {
     @Override
     public void setShooterCurrentLimitSpeed(
         ShooterState state, 
-        double percentOutput,
+        double percentOutput
-        Current currentLimit,
+        // Current currentLimit,
-        AngularVelocity targetVelocity
+        // AngularVelocity targetVelocity
     ) {
-        state.motor1TargetVelocity = targetVelocity;
+        // state.motor1TargetVelocity = targetVelocity;
-        state.motor2TargetVelocity = targetVelocity;
+        // state.motor2TargetVelocity = targetVelocity;
 
-        double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps));
+        // double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps));
-        double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2;
+        // double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2;
 
-        if(
+        // if(
-            Math.abs(currentLimit.in(Amps)) > current &&
+        //     Math.abs(currentLimit.in(Amps)) > current &&
-            Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
+        //     Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
-        ) {
+        // ) {
             m_shooter1Motor.set(percentOutput);
             m_shooter2Motor.set(percentOutput);
-        } else {
+        // } else {
-            m_shooter1Motor.set(0);
+        //     m_shooter1Motor.set(0);
-            m_shooter2Motor.set(0);
+        //     m_shooter2Motor.set(0);
-        }
+        // }
     }
 
     @Override

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems.swerve;
 
+import static edu.wpi.first.units.Units.Rotations;
+
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -245,6 +247,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
+        if(!TimesNegativeOne.isRed) {
+            leftStick.rotateBy(new Rotation2d(Math.PI/2.));
+        }
+
         io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)

src/main/java/frc4388/utility/compute/FieldPositions.java

@@ -6,7 +6,7 @@ public class FieldPositions {
     // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0);
     // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0);
     public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3);
-    public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3);
+    public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534);// + 0.3);
 
 
     // We set the default position to one just in case it doesn't update