Small Removed comments, adjusted scoring command, removed pickup, get…

…ting ready for cleaning up shuffleboard

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

@@ -22,10 +22,9 @@ public final class Constants {
   public static final double DRIVE_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 0.2;
 
   public static int CURRENT_LIMIT = 30;
-
-  public enum ChassisConfiguration {
-    MAIN,
-    PRACTICE
+
+  public static boolean isPracticeBot() {
+    return System.getenv("PRACTICE_ROBOT") != null;
   }
 
   public final static class ControllerConstants {
@@ -36,12 +35,7 @@ public final static class ControllerConstants {
     public static final int POV_ANGLE_LEFT = 270;
     public static final int POV_ANGLE_RIGHT = 90;
   }
-
-  public static ChassisConfiguration getChassisConfiguration() {
-    return System.getenv("PRACTICE_ROBOT") != null ? ChassisConfiguration.PRACTICE : ChassisConfiguration.MAIN;
-  }
-
-  public static final class DriveConstants {
+    public static final class DriveConstants {
     public static final double DRIVETRAIN_TRACKWIDTH_METERS = Units.inchesToMeters(19.5);
     public static final double DRIVETRAIN_WHEELBASE_METERS = Units.inchesToMeters(21.5);
 
@@ -66,21 +60,16 @@ public static final class DriveConstants {
     public static double BACK_RIGHT_ENCODER_OFFSET;
 
     public static final int PIGEON_ID = 19;
-
+  
     public static final void setOffsets() { 
-      if (Constants.getChassisConfiguration() == ChassisConfiguration.MAIN) {
+      if (!isPracticeBot()) {
         FRONT_LEFT_ENCODER_OFFSET = -314;
         FRONT_RIGHT_ENCODER_OFFSET = -100;
         BACK_LEFT_ENCODER_OFFSET = -164;
         BACK_RIGHT_ENCODER_OFFSET = -53;
-      } else {
-        FRONT_LEFT_ENCODER_OFFSET = -Math.toRadians(359);
-        FRONT_RIGHT_ENCODER_OFFSET = -Math.toRadians(57);
-        BACK_LEFT_ENCODER_OFFSET = -Math.toRadians(221);
-        BACK_RIGHT_ENCODER_OFFSET = -Math.toRadians(46);
-      }
-
-      SmartDashboard.putString("Robot Configuration", (Constants.getChassisConfiguration() == ChassisConfiguration.MAIN) ? "Main" : "Practice");
+      } else {}
+
+      SmartDashboard.putString("Robot Configuration", (isPracticeBot()) ? "Practice" : "Main");
     }
   }
 
@@ -92,13 +81,10 @@ public static final class ArmConstants {
     public static double ARM_LIMIT;
 
     public static void setOffsets() {
-      if(getChassisConfiguration() == ChassisConfiguration.MAIN) {
+      if(!isPracticeBot()) {
         ARM_OFFSET = 0;
         ARM_LIMIT = 0.4;
-      } else {
-        ARM_OFFSET = 0.176;
-        ARM_LIMIT = 0.78;
-      }
+      } else {}
     }
   }
 
@@ -126,7 +112,6 @@ public static final class AutoConstants {
     public static final double kPYController = 2;
     public static final double kPThetaController = 2.5;
 
-    // Constraint for the motion profilied robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(1,1);
   }
 
@@ -157,9 +142,9 @@ public static class Paths {
   public static class PhotonConstants {
     public static final int REFLECTIVE_TAPE_PIPELINE_INDEX = 0;
     public static final int APRILTAG_PIPELINE_INDEX = 1;
-    public static final double CAM_HEIGHT = 0.1524; //replace with actual height of camera in meters 
-    public static final double CAM_ANGLE = Units.degreesToRadians(20); //replace with actual angle of the camera
-    public static final int SERVO_PORT = 6; //change to actual port
+    public static final double CAM_HEIGHT = 0.1524; 
+    public static final double CAM_ANGLE = Units.degreesToRadians(20);
+    public static final int SERVO_PORT = 6;
     public static final int FORWARD_ANGLE =  0;
     public static final int BACKWARD_ANGLE = 135;
   }
@@ -187,7 +172,7 @@ public static class GameConstants {
   }
 
   public static class AimbotConstants {
-    public static final double kP = 0.25; //try the kp ki kd values above
+    public static final double kP = 0.25;
     public static final double kI = 0.0;
     public static final double kD = 0.0;
     public static final double speed = 1;

src/main/java/frc/robot/Robot.java

@@ -32,6 +32,7 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
+    m_robotContainer.io.driveSubsystem.syncEncoders();
   }
 
   /**
@@ -67,7 +68,6 @@ public void autonomousInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
-    m_robotContainer.syncEncodersDisabled();
   }
 
   /** This function is called periodically during autonomous. */
@@ -76,14 +76,7 @@ public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
-    m_robotContainer.syncEncodersDisabled();
+    if (m_autonomousCommand != null) m_autonomousCommand.cancel();
     m_robotContainer.io.configTeleop();
   }
 
@@ -93,6 +86,7 @@ public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
+    // LiveWindow.disableAllTelemetry(); // NOTE: Heard this being active disables the CommandScheduler
     CommandScheduler.getInstance().enable(); // NOTE: Heard that the Command Scheduler isn't enabled during TestMode
     CommandScheduler.getInstance().cancelAll();
     m_robotContainer.io.configTesting();

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

@@ -7,19 +7,17 @@
 import frc.robot.Constants.ClawConstants;
 import frc.robot.Constants.Paths;
 import frc.robot.commands.AutonBalance;
-import frc.robot.commands.ScoreHigh;
-import frc.robot.commands.ScoreMid;
+import frc.robot.commands.Score;
 import frc.robot.commands.SetClawPosition;
 import frc.robot.commands.SetTelescopePosition;
+import frc.robot.utilities.General;
 import frc.robot.utilities.IO;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 public class RobotContainer {
 
@@ -34,66 +32,62 @@ public RobotContainer() {
 
     SmartDashboard.putData("Auto Selector", autonomousSelector);
     CameraServer.startAutomaticCapture();
-    // SmartDashboard.putData("Kill LEDs", new InstantCommand(ledSubsystem::killLeds, ledSubsystem));
-    // PathPlannerServer.startServer(5811); // DEBUGGING
   }
 
   private void configureAutonomous() {
     autonomousSelector.setDefaultOption("Drive to side + Park", new SequentialCommandGroup(
         io.driveSubsystem.followPath(Paths.park),
         new AutonBalance(io.driveSubsystem, false)));
-    autonomousSelector.addOption("Only Park", new SequentialCommandGroup(new InstantCommand(() -> io.armCommand.setAngle(75)), new AutonBalance(io.driveSubsystem, false)));
+    autonomousSelector.addOption("Only Park", new SequentialCommandGroup(General.Instant(() -> io.armCommand.setAngle(75)), new AutonBalance(io.driveSubsystem, false)));
     autonomousSelector.addOption("Only Park Reversed", new AutonBalance(io.driveSubsystem, true));
     autonomousSelector.addOption("Leave Community", io.driveSubsystem.followPath(Paths.leaveCommunity));
     autonomousSelector.addOption("Score Mid + Leave Community", new SequentialCommandGroup(
-        new ScoreMid(io),
-        new WaitCommand(1),
+        new Score(io, false),
         new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION),
         io.driveSubsystem.followPath(Paths.leaveCommunity)));
-    autonomousSelector.addOption("frfr", new ScoreMid(io));
+    autonomousSelector.addOption("frfr", new Score(io, false));
     autonomousSelector.addOption("Door Dash",
         io.driveSubsystem.followPathGroupWithEvents(Paths.picking).andThen(new AutonBalance(io.driveSubsystem, true)));
     autonomousSelector.addOption("Leave Community + Park", new SequentialCommandGroup(
-        new ScoreMid(io),
-        new InstantCommand(io.claw::closeClaw),
-        new ScoreHigh(io),
+      new Score(io, false),
+        General.Instant(io.claw::closeClaw),
+        new Score(io, true),
         new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION),
         io.driveSubsystem.followPath(Paths.leaveCommunityPark),
         new AutonBalance(io.driveSubsystem, true)));
     autonomousSelector.addOption("Score Mid + Park", new SequentialCommandGroup(
-      new ScoreMid(io),
+      new Score(io, false),
       new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION),
       new AutonBalance(io.driveSubsystem, true)));
     autonomousSelector.addOption("Score Mid", new SequentialCommandGroup(
-        new ScoreMid(io),
-        new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION).withTimeout(0.5)));
+      new Score(io, false),
+      new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION).withTimeout(0.5)));
     autonomousSelector.addOption("Score Mider", new SequentialCommandGroup(
-      new ScoreMid(io),
+      new Score(io, false),
       new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION).withTimeout(0.5),
-      new InstantCommand(io.claw::openClaw),
+      General.Instant(io.claw::openClaw),
       io.driveSubsystem.followPathGroupWithEvents(Paths.cooking)
     ));
     autonomousSelector.addOption("We BALL auto", new SequentialCommandGroup(
-        new ScoreMid(io),
-        io.driveSubsystem.followPathGroupWithEvents(Paths.weBall)));
+      new Score(io, false),
+      io.driveSubsystem.followPathGroupWithEvents(Paths.weBall)));
     autonomousSelector.addOption("Loop", io.driveSubsystem.followPath(Paths.loop));
   }
 
-  public void initEventMap() {
+  private void initEventMap() {
     Paths.eventMap.put("pickup", new SequentialCommandGroup(
-        new InstantCommand(io.claw::openClaw),
+        General.Instant(io.claw::openClaw),
         new SetClawPosition(io.claw, ClawConstants.INTAKE_POSITION).withTimeout(1),
-        new InstantCommand(io.claw::intake)));
-    Paths.eventMap.put("closeclaw", new InstantCommand(io.claw::closeClaw));
-    Paths.eventMap.put("openclaw", new InstantCommand(io.claw::openClaw));
-    Paths.eventMap.put("stopintake", new InstantCommand(io.claw::stopClaw));
+        General.Instant(io.claw::intake)));
+    Paths.eventMap.put("closeclaw", General.Instant(io.claw::closeClaw));
+    Paths.eventMap.put("openclaw", General.Instant(io.claw::openClaw));
+    Paths.eventMap.put("stopintake", General.Instant(io.claw::stopClaw));
     Paths.eventMap.put("intakeposition",
         new SetClawPosition(io.claw, ClawConstants.INTAKE_POSITION).withTimeout(1));
-    Paths.eventMap.put("print", new PrintCommand("PRINTINTINTINTTINTIN"));
     Paths.eventMap.put("liftarm", new SetClawPosition(io.claw, ClawConstants.CARRY_POSITION).withTimeout(1));
-    Paths.eventMap.put("scorehigh", new ScoreHigh(io)
+    Paths.eventMap.put("scorehigh", new Score(io, true)
         .andThen(new SetTelescopePosition(io.telescope, io.arm, 0)));
-    Paths.eventMap.put("scorelow", new ScoreMid(io)
+    Paths.eventMap.put("scorelow", new Score(io, false)
         .andThen(new SetTelescopePosition(io.telescope,io.arm, 0)));
   }
 
@@ -110,12 +104,4 @@ public Command getAutonomousCommand() {
     }, io.driveSubsystem);
     return autonomousSelector.getSelected().andThen(postAutonomous);
   }
-
-  public Command runSubsystemTests(){
-    return io.runSystemsCheck();
-  }
-
-  public void syncEncodersDisabled() {
-    io.driveSubsystem.syncEncoders();
-  }
 }

src/main/java/frc/robot/SwerveModule.java

@@ -35,7 +35,7 @@ public SwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerC
         steerEncoder.configFactoryDefault();
         steerEncoder.configMagnetOffset(offset);
         steerEncoder.configSensorDirection(false);
-        steerEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); // NOTE: Might want to test putting it between [-180,180]
+        steerEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
         steerEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
         steerEncoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 100, 250);
 
@@ -48,7 +48,7 @@ public SwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerC
         driveMotor.getEncoder().setPositionConversionFactor(DRIVE_CONVERSION_FACTOR);
         driveMotor.getEncoder().setVelocityConversionFactor(DRIVE_CONVERSION_FACTOR / 60.0);
 
-        steerMotor.getEncoder().setPositionConversionFactor(2 * Math.PI * STEER_REDUCTION); // NOTE: May need to multiply by 2
+        steerMotor.getEncoder().setPositionConversionFactor(2 * Math.PI * STEER_REDUCTION);
         steerMotor.getEncoder().setVelocityConversionFactor(2 * Math.PI * STEER_REDUCTION / 60);
         steerMotor.getEncoder().setPosition(steerEncoder.getAbsolutePosition());
 
@@ -65,7 +65,7 @@ public SwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerC
         steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20);
         steerMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 20);
 
-        tab.addDouble("Absolute Angle", steerEncoder::getAbsolutePosition);
+        tab.addDouble("Absolute Angle", steerEncoder::getAbsolutePosition); 
         tab.addDouble("Current Angle", () -> Math.toDegrees(steerMotor.getEncoder().getPosition()));
         tab.addDouble("Target Angle", () -> desiredAngle);
         tab.addDouble("Velocity", steerMotor.getEncoder()::getVelocity);

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

@@ -38,6 +38,6 @@ public boolean isFinished() {
 
   @Override
   public void end(boolean interrupted) {
-    io.driveSubsystem.drive(new ChassisSpeeds(0, 0, 0));
+    io.driveSubsystem.stop();
   }
 }

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

@@ -47,7 +47,7 @@ public void execute() {
     public void end(boolean interrupted) {
       super.end(interrupted);
       System.out.println("Robot Balanced!");
-      driveSubsystem.drive(new ChassisSpeeds(0, 0, 0));
+      driveSubsystem.stop();
     }
   }
 

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

@@ -46,6 +46,6 @@ public void execute() {
 
     @Override
     public void end(boolean interrupted) {
-        driveSubsystem.drive(new ChassisSpeeds());
+        driveSubsystem.stop();
     }
 }

src/main/java/frc/robot/commands/ScoreMid.java → src/main/java/frc/robot/commands/Score.java

@@ -4,32 +4,31 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants.TelescopeConstants;
 import frc.robot.utilities.IO;
+import frc.robot.utilities.General;
 
-public class ScoreMid extends SequentialCommandGroup {
-  public ScoreMid(IO io) {
+
+public class Score extends SequentialCommandGroup {
+  public Score(IO io, boolean high) {
     SetArmProfiled armCommand = new SetArmProfiled(75,io.arm, io.telescope, io.photon::rotateMount, false);
     addCommands(
       new ParallelCommandGroup(
         armCommand,
-        new PrintCommand("Starting Low..."),
         new SequentialCommandGroup(
           new WaitCommand(0.5),
           new SetTelescopePosition(io.telescope, io.arm, TelescopeConstants.LOW_POSITION),
           new SetClawPosition(io.claw, -50).withTimeout(0.5),
-          new InstantCommand(io.claw::openClaw),
-          new InstantCommand(() -> io.claw.setIntakeSpeed(-0.5)),
+          General.Instant(io.claw::openClaw),
+          General.Instant(() -> {if (high) io.claw.setIntakeSpeed(-0.5);}),
           new SetTelescopePosition(io.telescope, io.arm, 0)
         )
       ).withTimeout(3),
-      new InstantCommand(io.claw::stopClaw),
-      new InstantCommand(() -> io.claw.setWristVoltage(0))
+      General.Instant(io.claw::stopClaw),
+      General.Instant(() -> io.claw.setWristVoltage(0))
     );
   }
 }