Merge branch 'main' into practiceFieldTest

2491-NoMythic · Feb 27, 2024 · 8a8e39f · 8a8e39f
2 parents 62496ba + bf1fe48
commit 8a8e39f
Show file tree

Hide file tree

Showing 27 changed files with 26 additions and 164 deletions.
diff --git a/src/main/deploy/pathplanner/autos/FleeAmp.auto b/src/main/deploy/pathplanner/autos/FleeAmp.auto
@@ -20,7 +20,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 1.0
+            "waitTime": 3.0
           }
         },
         {

diff --git a/src/main/deploy/pathplanner/autos/FleeMid.auto b/src/main/deploy/pathplanner/autos/FleeMid.auto
@@ -20,7 +20,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 1.25
+            "waitTime": 3.0
           }
         },
         {

diff --git a/src/main/deploy/pathplanner/autos/FleeSource.auto b/src/main/deploy/pathplanner/autos/FleeSource.auto
@@ -20,7 +20,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 1.25
+            "waitTime": 3.0
           }
         },
         {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,9 +4,7 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,58 +5,34 @@
 package frc.robot;
 
 import static frc.robot.settings.Constants.PS4Driver.*;
-import static frc.robot.settings.Constants.PS4Operator.*;
 import static frc.robot.settings.Constants.ShooterConstants.LONG_SHOOTING_RPS;
-import static frc.robot.settings.Constants.ShooterConstants.SHORT_SHOOTING_RPS;
-
-import java.nio.file.Path;
-import java.time.Instant;
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
-import org.littletonrobotics.urcl.URCL;
-
 import static frc.robot.settings.Constants.DriveConstants.*;
 
-import com.ctre.phoenix6.SignalLogger;
-import com.ctre.phoenix6.hardware.Pigeon2;
-import com.fasterxml.jackson.core.sym.Name;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
-import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import frc.robot.commands.AimShooter;
 import frc.robot.commands.AimRobotMoving;
-import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.CollectNote;
 import frc.robot.commands.Drive;
 import frc.robot.commands.DriveTimeCommand;
 import frc.robot.commands.ConditionalIndexer;
-import frc.robot.settings.Constants.Field;
 import frc.robot.settings.Constants.IndexerConstants;
 import frc.robot.commands.IndexCommand;
 import frc.robot.commands.IndicatorLights;
 import frc.robot.settings.Constants;
 import frc.robot.settings.Constants.ClimberConstants;
-import frc.robot.settings.Constants.DriveConstants;
 import frc.robot.settings.Constants.IntakeConstants;
-import frc.robot.settings.Constants.PathConstants;
 import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.AngleShooterSubsystem;
 import frc.robot.subsystems.Climber;
 import frc.robot.commands.ManualShoot;
-import frc.robot.commands.RotateRobot;
-import frc.robot.commands.autoAimParallel;
 import frc.robot.commands.NamedCommands.initialShot;
 import frc.robot.commands.NamedCommands.shootNote;
 import frc.robot.commands.goToPose.GoToAmp;
-import frc.robot.commands.goToPose.GoToClimbSpot;
-import frc.robot.commands.climber_commands.AutoClimb;
-import frc.robot.commands.climber_commands.ClimberPullDown;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.Lights;
 import frc.robot.subsystems.IndexerSubsystem;
@@ -68,28 +44,19 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import frc.robot.commands.Drive;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.units.Angle;
-import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PS4Controller;
 import edu.wpi.first.wpilibj.PowerDistribution;
-import frc.robot.commands.ManualShoot;
 import frc.robot.commands.AngleShooter;
-import frc.robot.commands.IntakeCommand;
-import frc.robot.settings.IntakeDirection;
 
 
 /**
@@ -108,7 +75,7 @@ public class RobotContainer {
   private final boolean climberExists = Preferences.getBoolean("Climber", true);
   private final boolean lightsExist = Preferences.getBoolean("Lights", true);
   private final boolean indexerExists = Preferences.getBoolean("Indexer", true);
-  private final boolean autosExist = Preferences.getBoolean("Autos", true);
+  //private final boolean autosExist = Preferences.getBoolean("Autos", true);
   private final boolean useDetectorLimelight = Preferences.getBoolean("Detector Limelight", true);
 
   private DrivetrainSubsystem driveTrain;
@@ -119,16 +86,13 @@ public class RobotContainer {
   private Climber climber;
   private Lights lights;
   private PS4Controller driverController;
-  private PS4Controller operatorController;
+  //private PS4Controller operatorController;
   private Limelight limelight;
-  private IntakeDirection iDirection;
-  private Pigeon2 pigeon;
   private IndexCommand defaulNoteHandlingCommand;
   private IndexerSubsystem indexer;
   private AimShooter defaultShooterAngleCommand;
   private SendableChooser<String> climbSpotChooser;
   private SendableChooser<Command> autoChooser;
-  private DoubleSupplier angleSup;
   private PowerDistribution PDP;
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
@@ -137,6 +101,7 @@ public class RobotContainer {
   public RobotContainer() {
     //preferences are initialized IF they don't already exist on the Rio
     Preferences.initBoolean("Brushes", false);
+    Preferences.setBoolean("Brushes", false);
     Preferences.initBoolean("Intake", false);
     Preferences.initBoolean("Climber", false);
     Preferences.initBoolean("Shooter", false);
@@ -153,8 +118,7 @@ public RobotContainer() {
     // SignalLogger.setPath("/media/sda1/ctre-logs/");
     // SignalLogger.start();
     driverController = new PS4Controller(DRIVE_CONTROLLER_ID);
-    operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
-    pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID);
+    //operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
     PDP = new PowerDistribution(1, ModuleType.kRev);
 
     // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists);
@@ -370,9 +334,6 @@ private double modifyAxis(double value, double deadband) {
     return value;
   }
 
-  private double getAmpAngle() {
-    return Constants.Field.AMPLIFIER_ANGLE;
-  }
   private void configureDriveTrain() {
     AutoBuilder.configureHolonomic(
                 driveTrain::getPose, // Pose2d supplier
@@ -410,7 +371,7 @@ private void registerNamedCommands() {
       NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));
     }
     if(indexerExists&&shooterExists) {
-      NamedCommands.registerCommand("initialShot", new initialShot(shooter, indexer, 0.75, 0.5));
+      NamedCommands.registerCommand("initialShot", new initialShot(shooter, indexer, 2.0, 2.25));
       NamedCommands.registerCommand("shootNote", new shootNote(indexer, 1));
       NamedCommands.registerCommand("shootNote", new shootNote(indexer, 1));
       NamedCommands.registerCommand("setFeedTrue", new InstantCommand(()->SmartDashboard.putBoolean("feedMotor", true)));

diff --git a/src/main/java/frc/robot/commands/AimRobotMoving.java b/src/main/java/frc/robot/commands/AimRobotMoving.java
@@ -1,23 +1,18 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 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.DriveConstants.*;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kI;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kP;
-import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_SHOOTER_kP;
-
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 

diff --git a/src/main/java/frc/robot/commands/AngleShooter.java b/src/main/java/frc/robot/commands/AngleShooter.java
@@ -6,9 +6,7 @@
 
 import java.util.function.DoubleSupplier;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.AngleShooterSubsystem;
 
 public class AngleShooter extends Command {

diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java
@@ -9,15 +9,9 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.settings.Constants.DriveConstants;
 import frc.robot.settings.Constants.Vision;
 import frc.robot.settings.LimelightDetectorData;
 import frc.robot.subsystems.DrivetrainSubsystem;
-import static frc.robot.settings.Constants.DriveConstants.*;
-import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.Limelight;
 
 public class CollectNote extends Command {

diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java
@@ -10,14 +10,11 @@
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.PS4Controller;
 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.IndexerConstants;
 import frc.robot.settings.Constants.ShooterConstants;
-import frc.robot.settings.Constants.IntakeConstants;
 import frc.robot.subsystems.AngleShooterSubsystem;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
@@ -41,9 +38,6 @@ public class IndexCommand extends Command {
   AngleShooterSubsystem angleShooterSubsytem;
   boolean auto;
   double runsEmpty = 0;
-  BooleanSupplier SubwooferSupplier1;
-  BooleanSupplier SubwooferSupplier2;
-  BooleanSupplier SubwooferSupplier3;
 
   /** Creates a new IndexCommand. */
   public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, DoubleSupplier ampSupplier) {
@@ -56,9 +50,6 @@ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIf
     this.ampSupplier = ampSupplier;
     this.angleShooterSubsytem = angleShooterSubsystem;
     this.humanPlayerSupplier = humanPlaySupplier;//R1
-    this.SubwooferSupplier1 = SubwooferSupplier1;
-    this.SubwooferSupplier2 = SubwooferSupplier2;
-    this.SubwooferSupplier3 = SubwooferSupplier3;
     SmartDashboard.putNumber("amp RPS", AMP_RPS);
     SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED);
     SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE);

diff --git a/src/main/java/frc/robot/commands/ManualShoot.java b/src/main/java/frc/robot/commands/ManualShoot.java
@@ -4,10 +4,8 @@
 
 package frc.robot.commands;
 import frc.robot.settings.Constants.IndexerConstants;
-import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.IndexerSubsystem;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj2.command.Command;

diff --git a/src/main/java/frc/robot/commands/NamedCommands/initialShot.java b/src/main/java/frc/robot/commands/NamedCommands/initialShot.java
@@ -14,13 +14,13 @@ public class initialShot extends Command {
   ShooterSubsystem shooter;
   IndexerSubsystem indexer;
   Timer timer;
-  double revtime;
+  double revTime;
   double shootTime;
   /** Creates a new shootThing. */
   public initialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double revTime, double shootTime) {
     this.indexer = indexer;
     this.shooter = shooter;
-    this.revtime = revtime;
+    this.revTime = revTime;
     this.shootTime = shootTime;
     timer = new Timer();
     addRequirements(shooter, indexer);
@@ -30,14 +30,16 @@ public initialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double re
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    timer.reset();
     timer.start();
+    indexer.off();
     shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(timer.get()>revtime) {
+    if(timer.get()>revTime) {
       indexer.on();
     }
   }

diff --git a/src/main/java/frc/robot/commands/RotateRobot.java b/src/main/java/frc/robot/commands/RotateRobot.java
@@ -1,19 +1,14 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.settings.Constants.Field;
 import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.DrivetrainSubsystem;
-import static frc.robot.settings.Constants.DriveConstants.*;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kI;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kP;
-import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_SHOOTER_kP;
-
 import java.util.function.DoubleSupplier;
 
 public class RotateRobot extends Command {

diff --git a/src/main/java/frc/robot/commands/autoAimParallel.java b/src/main/java/frc/robot/commands/autoAimParallel.java
@@ -4,15 +4,6 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.PS4Controller;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.settings.Constants.Field;
-import frc.robot.subsystems.DrivetrainSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-import static frc.robot.settings.Constants.ShooterConstants.*;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.ShooterSubsystem; 

diff --git a/src/main/java/frc/robot/commands/climber_commands/AutoClimb.java b/src/main/java/frc/robot/commands/climber_commands/AutoClimb.java
@@ -4,10 +4,7 @@
 
 package frc.robot.commands.climber_commands;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.settings.Constants.ClimberConstants;
 import frc.robot.subsystems.Climber;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more

diff --git a/src/main/java/frc/robot/commands/climber_commands/ClimberPullDown.java b/src/main/java/frc/robot/commands/climber_commands/ClimberPullDown.java
@@ -5,7 +5,6 @@
 package frc.robot.commands.climber_commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.settings.Constants.ClimberConstants;
 import frc.robot.subsystems.Climber;
 
 public class ClimberPullDown extends Command {

diff --git a/src/main/java/frc/robot/commands/goToPose/GoToAmp.java b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java
@@ -12,7 +12,6 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.settings.Constants.PathConstants;
 import frc.robot.subsystems.DrivetrainSubsystem;
 
 public class GoToAmp extends Command {

diff --git a/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java b/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java
@@ -6,15 +6,11 @@
 
 import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS;
 
-import java.util.function.BooleanSupplier;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathPlannerPath;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.settings.Constants.PathConstants;
 import frc.robot.subsystems.DrivetrainSubsystem;
 
 public class GoToClimbSpot extends Command {