Duluth changes

src/main/java/frc/robot/RobotContainer.java

@@ -115,6 +115,8 @@ public class RobotContainer {
   BooleanSupplier ManualShootSup;
   BooleanSupplier ForceVisionSup;
   BooleanSupplier GroundIntakeSup;
+  BooleanSupplier FarStageAngleSup;
+  BooleanSupplier OperatorPreRevSup;
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
 
@@ -156,6 +158,8 @@ public RobotContainer() {
     ManualShootSup = driverController::getL1Button;
     ForceVisionSup = driverController::getOptionsButton;
     GroundIntakeSup = operatorController::getTouchpad;
+    FarStageAngleSup = driverController::getTouchpad;
+    OperatorPreRevSup = operatorController::getL2Button;
 
     // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists);
     limelightInit();
@@ -205,7 +209,7 @@ private void shooterInst() {
   }
   private void angleShooterInst(){
     angleShooterSubsystem = new AngleShooterSubsystem();
-    defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup);
+    defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup, FarStageAngleSup);
     angleShooterSubsystem.setDefaultCommand(defaultShooterAngleCommand);
   }
   private void intakeInst() {
@@ -218,7 +222,7 @@ private void indexInit() {
     indexer = new IndexerSubsystem();
   }
   private void indexCommandInst() {
-    defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup);
+    defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorPreRevSup);
     indexer.setDefaultCommand(defaulNoteHandlingCommand);
   }
 
@@ -257,8 +261,10 @@ private void configureBindings() {
       () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL),
       () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
       driverController::getL2Button,
-      driverController::getCrossButton,
-      driverController::getTriangleButton));
+      StageAngleSup,
+      FarStageAngleSup,
+      SubwooferAngleSup
+      ));
 
     if(Preferences.getBoolean("Detector Limelight", false)) {
       new Trigger(operatorController::getR1Button).onTrue(new SequentialCommandGroup(

src/main/java/frc/robot/commands/AimRobotMoving.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.settings.Constants.DriveConstants;
+import frc.robot.settings.Constants.Field;
 import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD;
@@ -27,13 +28,14 @@ public class AimRobotMoving extends Command {
     DoubleSupplier translationXSupplier;
     DoubleSupplier translationYSupplier;
     BooleanSupplier run;
-    BooleanSupplier cancel1;
-    BooleanSupplier cancel2;
+    BooleanSupplier PodiumAngleSup;
+    BooleanSupplier FarStageAngleSup;
     DoubleSupplier rotationSupplier;
     double rotationSpeed;
     double allianceOffset;
+    BooleanSupplier SubwooferAngleSup;
 
-  public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier cancel1, BooleanSupplier cancel2){
+  public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier PodiumAngleSup, BooleanSupplier FarStageAngleSup, BooleanSupplier SubwooferAngleSup){
         m_drivetrain = drivetrain;
         speedController = new PIDController(
           AUTO_AIM_ROBOT_kP, 
@@ -43,9 +45,10 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSup
           speedController.enableContinuousInput(-180, 180);
           this.translationXSupplier = translationXSupplier;
           this.translationYSupplier = translationYSupplier;
+          this.SubwooferAngleSup = SubwooferAngleSup;
           this.rotationSupplier = rotationSupplier;
-          this.cancel1 = cancel1;
-          this.cancel2 = cancel2;
+          this.FarStageAngleSup = FarStageAngleSup;
+          this.PodiumAngleSup = PodiumAngleSup;
           this.run = run;
           addRequirements(drivetrain);
         }
@@ -61,26 +64,41 @@ public void initialize() {
         @Override
         public void execute() {
           desiredRobotAngle = m_drivetrain.calculateSpeakerAngleMoving();
-          speedController.setSetpoint(desiredRobotAngle);
-        //move robot to desired angle
-        this.currentHeading = m_drivetrain.getPose().getRotation().getDegrees();
-        if(Math.abs(rotationSupplier.getAsDouble()) > 0.3) {
-          rotationSpeed = rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND;
-        } else {
-          rotationSpeed = speedController.calculate(currentHeading);
-        }
-        if(DriverStation.getAlliance().get() == Alliance.Red) {
-          allianceOffset = Math.PI;
-        } else {
-          allianceOffset = 0;
-        }
-        if(!cancel1.getAsBoolean()) {
-          m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
-            translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
-            translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
-            rotationSpeed,
-            new Rotation2d(m_drivetrain.getPose().getRotation().getRadians()+allianceOffset)));
-        }
+          double podiumRobotAngle;
+          double farStageRobotAngle;
+          if(DriverStation.getAlliance().get() == Alliance.Red) {
+            podiumRobotAngle = Field.RED_PODIUM_ROBOT_ANGLE;
+            farStageRobotAngle = Field.RED_FAR_STAGE_ROBOT_ANGLE;
+          } else {
+            podiumRobotAngle = Field.BLUE_PODIUM_ROBOT_ANGLE;
+            farStageRobotAngle = Field.BLUE_FAR_STAGE_ROBOT_ANGLE;
+          }
+          if(PodiumAngleSup.getAsBoolean()) {
+            speedController.setSetpoint(podiumRobotAngle);
+          } else if(FarStageAngleSup.getAsBoolean()) {
+            speedController.setSetpoint(farStageRobotAngle);
+          } else {
+            speedController.setSetpoint(desiredRobotAngle);
+          }
+          //move robot to desired angle
+          this.currentHeading = m_drivetrain.getPose().getRotation().getDegrees();
+          if(Math.abs(rotationSupplier.getAsDouble()) > 0.3) {
+            rotationSpeed = rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND;
+          } else {
+            rotationSpeed = speedController.calculate(currentHeading);
+          }
+          if(DriverStation.getAlliance().get() == Alliance.Red) {
+            allianceOffset = Math.PI;
+          } else {
+            allianceOffset = 0;
+          }
+          if(!SubwooferAngleSup.getAsBoolean()){
+              m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
+              translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
+              translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
+              rotationSpeed,
+              new Rotation2d(m_drivetrain.getPose().getRotation().getRadians()+allianceOffset)));
+          }
         // m_drivetrain.drive(new ChassisSpeeds(
         //   translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
         //   translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
@@ -102,6 +120,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (!run.getAsBoolean() || cancel1.getAsBoolean() || cancel2.getAsBoolean());
+    return (!run.getAsBoolean());
   }
 }

src/main/java/frc/robot/commands/AimShooter.java

@@ -15,16 +15,18 @@ public class AimShooter extends Command {
 	BooleanSupplier humanPlayerSupplier;
 	BooleanSupplier SubwooferSupplier1;
 	BooleanSupplier StageAngleSupplier;
-	BooleanSupplier intakeSupplier;
+	BooleanSupplier groundIntakeSup;
+	BooleanSupplier farStageAngleSup;
 
 	public AimShooter(AngleShooterSubsystem angleShooterSubsystem, DoubleSupplier POVSupplier, BooleanSupplier humanPlayerSupplier,
-					  BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier intakeSup) {
+					  BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier groundIntakeSup, BooleanSupplier farStageAngleSup) {
 		this.angleShooterSubsystem = angleShooterSubsystem;
 		this.POVSupplier = POVSupplier;
 		this.humanPlayerSupplier = humanPlayerSupplier;
 		this.SubwooferSupplier1 = SubwooferSupplier1;
 		this.StageAngleSupplier = StageAngleSupplier;
-		this.intakeSupplier = intakeSup;
+		this.groundIntakeSup = groundIntakeSup;
+		this.farStageAngleSup = farStageAngleSup;
 		addRequirements(angleShooterSubsystem);
 	}
 
@@ -38,12 +40,14 @@ public void execute() {
 		if(SubwooferSupplier1.getAsBoolean()) {
 			angleShooterSubsystem.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE);
 		} else if (StageAngleSupplier.getAsBoolean()) {
-			angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_ANGLE);
+			angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_SHOOTER_ANGLE);
+		}  else if (farStageAngleSup.getAsBoolean()) {
+			angleShooterSubsystem.setDesiredShooterAngle(Field.FAR_STAGE_SHOOTER_ANGLE);
 		} else if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) {
-			angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_ANGLE);
+			angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE);
 		} else if(humanPlayerSupplier.getAsBoolean()) {
 			angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
-		} else if(intakeSupplier.getAsBoolean()) {
+		} else if (groundIntakeSup.getAsBoolean()){
 			angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE);
 		} else {
 			angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle());

src/main/java/frc/robot/commands/GroundIntake.java

@@ -42,6 +42,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return intake.isNoteIn();
   }
 }

src/main/java/frc/robot/commands/IndexCommand.java

@@ -28,7 +28,7 @@ public class IndexCommand extends Command {
   BooleanSupplier revUpSupplier;
   Boolean revUp;
   BooleanSupplier shootIfReadySupplier;
-  DoubleSupplier ampSupplier;;
+  DoubleSupplier ampSupplier;
   BooleanSupplier groundIntakeSup;
   Boolean shootIfReady;
   // DoubleSupplier POVSupplier;
@@ -40,11 +40,13 @@ public class IndexCommand extends Command {
   AngleShooterSubsystem angleShooterSubsytem;
   BooleanSupplier stageAngleSup;
   BooleanSupplier subwooferAngleSup;
+  BooleanSupplier farStageAngleSup;
+  BooleanSupplier operatorRevSup;
   boolean auto;
   double runsEmpty = 0;
 
   /** Creates a new IndexCommand. */
-  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, BooleanSupplier stageAngleSup, BooleanSupplier SubwooferSup, BooleanSupplier groundIntakeSup) {
+  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, BooleanSupplier stageAngleSup, BooleanSupplier SubwooferSup, BooleanSupplier groundIntakeSup, BooleanSupplier farStageAngleSup, BooleanSupplier operatorRevSup) {
     this.m_Indexer = m_IndexerSubsystem;
     this.shootIfReadySupplier = shootIfReadySupplier;//R2
     this.revUpSupplier = revUpSupplier;//L2
@@ -55,10 +57,12 @@ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIf
     this.humanPlayerSupplier = humanPlaySupplier;//R1
     this.subwooferAngleSup = SubwooferSup;
     this.stageAngleSup = stageAngleSup;
+    this.farStageAngleSup = farStageAngleSup;
     this.groundIntakeSup = groundIntakeSup;
+    this.operatorRevSup = operatorRevSup;
     SmartDashboard.putNumber("amp RPS", AMP_RPS);
     SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED);
-    SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE);
+    SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_SHOOTER_ANGLE);
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(m_IndexerSubsystem, shooter, intake);
   }
@@ -83,7 +87,7 @@ public void execute() {
       if(runsEmpty>20) {
         if(humanPlayerSupplier.getAsBoolean()) {
           m_Indexer.set(IndexerConstants.HUMAN_PLAYER_INDEXER_SPEED);
-          shooter.shootRPS(ShooterConstants.HUMAN_PLAYER_RPS);
+          shooter.shootSameRPS(ShooterConstants.HUMAN_PLAYER_RPS);
           intake.intakeOff();
         }
         else {
@@ -99,38 +103,41 @@ public void execute() {
     } else {
       runsEmpty = 0;
       intake.intakeOff();
-      if(shootIfReadySupplier.getAsBoolean()&&revUpSupplier.getAsBoolean()&&humanPlayerSupplier.getAsBoolean()) {
-        shooter.shootRPS(ShooterConstants.SUBWOOFER_RPS);
-      } else {
-        if(revUpSupplier.getAsBoolean()||stageAngleSup.getAsBoolean()||subwooferAngleSup.getAsBoolean()) {
-          if(angleShooterSubsytem.shortSpeakerDist()) {
-            shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS);
-          } else {
-            shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS);
-          }
+      if(revUpSupplier.getAsBoolean()||stageAngleSup.getAsBoolean()||subwooferAngleSup.getAsBoolean()) {
+        if(angleShooterSubsytem.shortSpeakerDist()) {
+          shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS);
         } else {
-          shooter.turnOff();
+          shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS);
         }
-        boolean indexer = false;
-        if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot()) {
-          RobotState.getInstance().ShooterReady = true;
-          if (shootIfReadySupplier.getAsBoolean()) {
-            indexer = true;
-          }
+      } else {
+        if (operatorRevSup.getAsBoolean()){ 
+          shooter.shootRPSWithCurrent(100, 10, 20);
         } else {
-          RobotState.getInstance().ShooterReady = false;
+          shooter.turnOff();
         }
-        if(SmartDashboard.getBoolean("feedMotor", false)) {
+      }
+      boolean indexer = false;
+      if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot() && shooter.isReving()) {
+        RobotState.getInstance().ShooterReady = true;
+        if (shootIfReadySupplier.getAsBoolean()) {
           indexer = true;
         }
-        if (indexer) {
-            m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
-         } else {
-            m_Indexer.off();
-         }
+      } else {
+        RobotState.getInstance().ShooterReady = false;
+      }
+      if(SmartDashboard.getBoolean("feedMotor", false)) {
+        indexer = true;
+      }
+      if((stageAngleSup.getAsBoolean()||subwooferAngleSup.getAsBoolean()||farStageAngleSup.getAsBoolean())&&revUpSupplier.getAsBoolean()&& shooter.validShot()) {
+        indexer = true;
+      }
+      if (indexer) {
+          m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+       } else {
+          m_Indexer.off();
+       }
     }
   }
-  }
 
   // Called once the command ends or is interrupted.
   @Override

src/main/java/frc/robot/settings/Constants.java

@@ -85,7 +85,7 @@ public double getValue() {
      * Whether the drive motor should be counterclockwise or clockwise positive. 
      * If there is an odd number of gear reductions this is typically clockwise-positive.
      */
-    public static final InvertedValue DRIVETRAIN_DRIVE_INVERTED = InvertedValue.CounterClockwise_Positive;
+    public static final InvertedValue DRIVETRAIN_DRIVE_INVERTED = InvertedValue.Clockwise_Positive;
 
     /**
      * The overall steer reduction of the module. Multiplying motor rotations by
@@ -263,7 +263,7 @@ public static final class ShooterConstants{
   public static final double COMP_MAXIMUM_SHOOTER_ANGLE = 108;
   public static final double PRAC_MAXIMUM_SHOOTER_ANGLE = 101;
   public static final double HUMAN_PLAYER_ANGLE = 97;
-  public static final double HUMAN_PLAYER_RPS = -10;
+  public static final double HUMAN_PLAYER_RPS = -15;
   public static final double SAFE_SHOOTER_ANGLE = 15;
   public static final double GROUND_INTAKE_SHOOTER_ANGLE = 90;
 
@@ -299,7 +299,7 @@ public static final class ShooterConstants{
 public static final class ClimberConstants{
   public static final int CLIMBER_MOTOR_RIGHT = 23;
   public static final int CLIMBER_MOTOR_LEFT = 22;
-  public static final double CLIMBER_SPEED_DOWN = 0.8;
+  public static final double CLIMBER_SPEED_DOWN = 1;
   public static final double CLIMBER_SPEED_UP = -1;
   public static final double MAX_MOTOR_ROTATIONS = 235;
 
@@ -324,7 +324,7 @@ public static final class IntakeConstants{
   public static final int BRUSH_1_MOTOR = 2491;
   public static final int BRUSH_2_MOTOR = 2491;
   public static final int BRUSH_3_MOTOR = 2491;
-  public static final double INTAKE_SPEED = 0.8;
+  public static final double INTAKE_SPEED = 1;
 }
 public static final class CTREConfigs {
   public TalonFXConfiguration driveMotorConfig;
@@ -367,6 +367,7 @@ public CTREConfigs() {
       driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
       driveMotorConfig.CurrentLimits.SupplyCurrentLimit = DriveConstants.DRIVE_CURRENT_LIMIT;
       driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+      driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = false;
 
       //  Steer encoder.
       steerEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
@@ -403,14 +404,21 @@ public final class PS4Operator{
   public final class Field{
     public static final double BLUE_SPEAKER_X = 0.23;
     public static final double RED_SPEAKER_X = 16.87;
-    public static final double SPEAKER_Y = 5.3;//16.412;
+    public static final double SPEAKER_Y = 5.6;
     public static final double SPEAKER_Z = 2.08; //height of opening
     public static final double MAX_SHOOTING_DISTANCE = 9;
     public static final double SHORT_RANGE_SHOOTING_DIST = 3;
 
-    public static final double AMPLIFIER_ANGLE = 108;
+    public static final double AMPLIFIER_SHOOTER_ANGLE = 108;
     public static final double SUBWOOFER_ANGLE = 60;
-    public static final double PODIUM_ANGLE = 37.5;
+    public static final double PODIUM_SHOOTER_ANGLE = 36.3;
+    public static final double FAR_STAGE_SHOOTER_ANGLE = 24.5;
+
+    public static final double BLUE_PODIUM_ROBOT_ANGLE = 149;
+    public static final double RED_PODIUM_ROBOT_ANGLE = 31;
+    public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 184;
+    public static final double RED_FAR_STAGE_ROBOT_ANGLE = -4;
+
     //angle at 60 for bounce techinque, didn't work
   }