diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 4ea2aaa..a580a84 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/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(), - Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), - RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) + ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get() + // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), + // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) ); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 89b3cb7..c504b1d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/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_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); + // 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 INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 4fd3916..c7d3369 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/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, - Current currentLimit, - AngularVelocity targetVelocity + double percentOutput + // Current currentLimit, + // AngularVelocity targetVelocity ) {} // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerOutput(ShooterState state, double percentOutput) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index fdb9a61..6ca3ce4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/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, - Current currentLimit, - AngularVelocity targetVelocity + double percentOutput + // Current currentLimit, + // AngularVelocity targetVelocity ) { - state.motor1TargetVelocity = targetVelocity; - state.motor2TargetVelocity = targetVelocity; + // state.motor1TargetVelocity = targetVelocity; + // state.motor2TargetVelocity = targetVelocity; - 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 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; - if( - Math.abs(currentLimit.in(Amps)) > current && - Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity - ) { + // if( + // Math.abs(currentLimit.in(Amps)) > current && + // Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity + // ) { m_shooter1Motor.set(percentOutput); m_shooter2Motor.set(percentOutput); - } else { - m_shooter1Motor.set(0); - m_shooter2Motor.set(0); - } + // } else { + // m_shooter1Motor.set(0); + // m_shooter2Motor.set(0); + // } } @Override diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 228429e..2ee10d3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/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) diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index d0b438c..f3a1f17 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/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