Post Beach Blitz Code

README.md

@@ -14,6 +14,8 @@ The driver uses two large joysticks and the operator uses an xbox controller.
 **Xbox controller (port 2)**:
  - Left bumper: deploy climber
  - Left trigger (only after climber has been deployed): activate climber winch
+ - Right bumper: zero arm encoder
+ - Right trigger: brake flywheels
  - Up d-pad: TBD
  - Down d-pad: Automatically aim
  - Left d-pad: TBD

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

@@ -60,11 +60,15 @@ public final static class Arm {
         public static final double START_CONFIG_ANGLE = 0; // Angle that arm is in when robot is turned on
         public static final double STOWED_ANGLE = 0; // Angle where robot fits under the Wheel of Fortune (WOF)
         public static final double INTAKE_ANGLE = 0; // Angle required for intaking power cells
+        public static final double OUTTAKE_ANGLE = 15;
         public static final double VISION_SEEK_ANGLE = 100; // Angle for finding targets
 
         /******** Arm Motion Constraints ********/
         public static final double MAX_VELOCITY_DEGREES_PER_SEC = 700;
         public static final double MAX_ACCLERATION_DEGREES_PER_SEC_SQUARED = 400;
+
+        public static final double EXIT_STARTING_CONFIG_POWER = -.2;
+        public static final double EXIT_STARTInG_CONFIG_ANGLE = 50; // Arm stops moving out of starting config at this angle
     }
 
     /**
@@ -80,7 +84,7 @@ public final static class Drive {
 
         /******** PID Constants ********/
         public static final PIDParameters WHEEL_PID_CONSTANTS = new PIDParameters(0.3, 0.0001, .6, "Drive wheel PID");
-        public static final PIDController ROTATION_PID_CONTROLLER = new PIDController(0.02, 0, 0.0005);
+        public static final PIDController ROTATION_PID_CONTROLLER = new PIDController(0.023, 0.0001, 0.003);
 
         /******** Physical Constants ********/
         public static final double GEAR_RATIO = 10.7;
@@ -114,7 +118,7 @@ public final static class Serializer {
         public static final int ACCLERATOR_MOTOR_ID = 15;
 
         /******** Tuning Constants ********/
-        public static final double BRUSH_POWER = .75;
+        public static final double BRUSH_POWER = .80;
         public static final double ACCLERATOR_POWER = 1.0;
         public static final double ROLLER_POWER = 1.0;
         public static final double BRUSH_REVERSE_POWER = .75;
@@ -132,25 +136,26 @@ public final static class Shooter {
         public static final int OUTER_FLYWHEEL_SLAVE_ID = 17;
 
         /******** PID Constants ********/
-        public static final PIDParameters MAIN_FLYWHEEL_PID = new PIDParameters(1.3, 0.00005, 5, "Main flywheel PID");
+        public static final PIDParameters MAIN_FLYWHEEL_PID = new PIDParameters(1.4, 0.00005, 5, "Main flywheel PID");
         public static final PIDParameters OUTER_FLYWHEEL_PID = new PIDParameters(0.5, 0.00015, 6, "Outer flywheel PID");
 
         /******** PID Constants ********/
         public static final double MAIN_FLYWHEEL_GEAR_RATIO = 6.0 / 5.0;
         public static final double OUTER_FLYWHEEL_GEAR_RATIO = 1.0 / 1.0;
 
         /******** Tuning Constants ********/
-        public static final int MAIN_FLYWHEEL_SPEED_TOLERANCE = 250; // Only shoot powercells if flywheels are within this range of their target speed
+        public static final int MAIN_FLYWHEEL_SPEED_TOLERANCE = 500; // Only shoot powercells if flywheels are within this range of their target speed
         public static final double MAIN_FLYWHEEL_ROC_TOLERANCE = 5000; // Only shoot powercells if the flywheels' speeds' rate of change is below this value
-        public static final int OUTER_FLYWHEEL_SPEED_TOLERANCE = 250;
+        public static final int OUTER_FLYWHEEL_SPEED_TOLERANCE = 500;
         public static final double OUTER_FLYWHEEL_ROC_TOLERANCE = 5000;
 
         public static final double MAIN_FLYWHEEL_SHOOTING_RPM = 5500;
         public static final double OUTER_FLYWHEEL_SHOOTING_RPM = 3000;
 
         /******** Firing soulutions ********/
         public static final FiringSolution LOB_INTO_GOAL = new FiringSolution(5500, 3000, 20);
-        public static final FiringSolution SHOOT_FROM_STARTING_LINE = new FiringSolution(100);
+        public static final FiringSolution SHOOT_FROM_STARTING_LINE = new FiringSolution(107);
+        public static final FiringSolution SHOOT_FROM_TRENCH = new FiringSolution(125);
 
         /**
          * A 2D array for tuning the shooter.

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

@@ -10,14 +10,15 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.AimAndShoot;
+import frc.robot.commands.BrakeFlywheels;
 import frc.robot.commands.Climb;
 import frc.robot.commands.DriveTeleop;
 import frc.robot.commands.Intake;
 import frc.robot.commands.LineUpWithTarget;
+import frc.robot.commands.MoveArmUpForDefense;
 import frc.robot.commands.Outtake;
 import frc.robot.commands.ReverseSerializerRoller;
 import frc.robot.commands.autos.ScorePreloads;
-import frc.robot.commands.autos.TestAuto;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
@@ -44,13 +45,15 @@ public class RobotContainer {
     private final ShooterSubsystem shooter = new ShooterSubsystem();
 
     private SendableChooser<Command> autoChooser = new SendableChooser<Command>();
-    private final ScorePreloads preloadAuto = new ScorePreloads(arm, drive, serializer, shooter);
+    private final ScorePreloads scorePreloadsTowardsDriverStation = new ScorePreloads(-.25, arm, drive, serializer, shooter);
+    private final ScorePreloads scorePreloadsAwayFromDriverStation = new ScorePreloads(.25, arm, drive, serializer, shooter);
 
     /** 
     * The container for the robot. Contains subsystems, OI devices, and commands. 
     */
     public RobotContainer() {
-        autoChooser.setDefaultOption("Preload auto", preloadAuto);
+        autoChooser.setDefaultOption("Move towards driver station", scorePreloadsTowardsDriverStation);
+        autoChooser.addOption("Move away from driver station", scorePreloadsAwayFromDriverStation);
         SmartDashboard.putData(autoChooser);
 
         configureDefaultCommands();
@@ -73,45 +76,78 @@ private void configureButtonBindings() {
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getBumper(Hand.kLeft))
     .whenActive(() -> climber.deploy(), climber);
 
-    /** When the left trigger is pressed, activate the winch motor */
+    /** While the left trigger is pressed, activate the winch motor */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getTriggerAxis(Hand.kLeft) >= .1)
     .whileActiveContinuous(new Climb(climber));
 
+    /** While the A button is pressed, intake powercells */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getAButton())
     .whileActiveContinuous(new Intake(intake, serializer, arm));
 
+    /** While the X button is pressed, reverse the intake and serializer */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getXButton())
     .whileActiveContinuous(new Outtake(intake, serializer, arm));
 
+    /** While the Y button is pressed, reverse the serializer motor */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getYButton())
     .whileActiveContinuous(new ReverseSerializerRoller(serializer));
 
+    /** While d-pad down is pressed, automatically aim and shoot */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 180)
     .whileActiveContinuous(
         new AimAndShoot(() -> OperatorInterface.OperatorController.getBButton(), 
         arm, serializer, shooter));
 
-    // Lobbing into target
+    /** While d-pad up is pressed, shoot from against driverstation wall */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 0)
     .whileActiveContinuous(
         new AimAndShoot(
             Constants.Shooter.LOB_INTO_GOAL,
             () -> OperatorInterface.OperatorController.getBButton(), 
             arm, serializer, shooter));
 
-    // Shooting from starting line
+    /** While d-pad right is pressed, shoot from initiation line */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 90)
     .whileActiveContinuous(
         new AimAndShoot(
             Constants.Shooter.SHOOT_FROM_STARTING_LINE,
             () -> OperatorInterface.OperatorController.getBButton(), 
             arm, serializer, shooter));
 
-    new LambdaTrigger(() -> OperatorInterface.DriverLeft.getTrigger() && OperatorInterface.DriverRight.getTrigger())
+    /** While d-pad left is pressed, shoot from the trench */
+    new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 270)
+    .whileActiveContinuous(
+        new AimAndShoot(
+            Constants.Shooter.SHOOT_FROM_TRENCH,
+            () -> OperatorInterface.OperatorController.getBButton(), 
+            arm, serializer, shooter));
+
+    /** While driver joystick trigger(s) are pressed, line up the robot with the target */
+    new LambdaTrigger(() -> OperatorInterface.DriverLeft.getTrigger() || OperatorInterface.DriverRight.getTrigger())
     .whileActiveContinuous(new LineUpWithTarget(drive));
 
+    /** 
+     * When right bumper is pressed, zero the arm encoder.
+     * 
+     * This is in case the arm gearbox slips, which is mostly resolved, but
+     * still a good safety net
+     */
     new LambdaTrigger(() -> OperatorInterface.OperatorController.getBumper(Hand.kRight))
     .whileActiveContinuous(() -> arm.zeroArmPosition(), arm);
+
+    /** While the right trigger is pressed, slow down the flywheels quickly */
+    new LambdaTrigger(() -> OperatorInterface.OperatorController.getTriggerAxis(Hand.kRight) >= .1)
+    .whileActiveContinuous(new BrakeFlywheels(shooter));
+
+    /** 
+     * While the start button on the xbox controller is pressed, move 
+     * the arm all the way up to block opponents' shots.
+     * 
+     * Used sucessfully in a Beach Blitz 2021 playoff match
+     * TODO: Find out what match that is
+     */
+    new LambdaTrigger(() -> OperatorInterface.OperatorController.getStartButton())
+    .whileActiveContinuous(new MoveArmUpForDefense(arm));
   }
 
   /**
@@ -124,7 +160,6 @@ public Command getAutonomousCommand() {
   }
 
   public Command getTeleopInitCommand() {
-      System.out.println("Teleop init");
       arm.setArmPositionToCurrentPosition();
       return new InstantCommand(arm::setArmPositionToCurrentPosition, arm);
   }

src/main/java/frc/robot/Vision.java

@@ -1,8 +1,5 @@
 package frc.robot;
 
-import org.photonvision.PhotonCamera;
-
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.util.Units;
 import frc.robot.subsystems.ArmSubsystem;
 import friarLib2.vision.PhotonCameraWrapper;
@@ -13,7 +10,7 @@
  * Container for the vision systems
  */
 public class Vision {
-    public static VisionCamera mainCamera = new PhotonCameraWrapper("gloworm");
+    public static VisionCamera mainCamera = new PhotonCameraWrapper("camera");
 
     private static double lastDistance = 0;
 

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

@@ -0,0 +1,33 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class BrakeFlywheels extends CommandBase {
+
+    ShooterSubsystem shooter;
+
+    public BrakeFlywheels (ShooterSubsystem shooter) {
+        this.shooter = shooter;
+
+        addRequirements(shooter);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize () {
+        shooter.brakeFlywheels();
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        shooter.stopFlywheels();
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -4,6 +4,9 @@
 import frc.robot.Constants;
 import frc.robot.subsystems.ArmSubsystem;
 
+/**
+ * Unclip the arm from its starting position
+ */
 public class ExitStartingConfiguration extends CommandBase {
 
     ArmSubsystem arm;
@@ -17,16 +20,18 @@ public ExitStartingConfiguration (ArmSubsystem arm) {
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-        arm.setArmPosition(Constants.Arm.INTAKE_ANGLE);
+        arm.setArmPower(Constants.Arm.EXIT_STARTING_CONFIG_POWER);
     }
 
     // Called once the command ends or is interrupted.
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        arm.stopArm();
+    }
 
     // Returns true when the command should end.
     @Override
     public boolean isFinished() {
-      return arm.isArmAtPosition();
+        return arm.getArmPosition() <= Constants.Arm.EXIT_STARTInG_CONFIG_ANGLE;
     }
 }

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

@@ -0,0 +1,37 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.subsystems.ArmSubsystem;
+
+/**
+ * Move the arm all the way up to block other robots' shots
+ */
+public class MoveArmUpForDefense extends CommandBase {
+
+    ArmSubsystem arm;
+
+    public MoveArmUpForDefense (ArmSubsystem arm) {
+        this.arm = arm;
+
+        addRequirements(arm);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        arm.setArmPosition(180);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        arm.setArmPosition(Constants.Arm.STOWED_ANGLE);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -23,7 +23,7 @@ public Outtake (IntakeSubsystem intake, SerializerSubsystem serializer, ArmSubsy
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-        arm.setArmPosition(Constants.Arm.INTAKE_ANGLE);
+        arm.setArmPosition(Constants.Arm.OUTTAKE_ANGLE);
         intake.outtake();
         serializer.reverseBrushesAndRoller();
     }

src/main/java/frc/robot/commands/autos/ScorePreloads.java

@@ -3,22 +3,25 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants;
 import frc.robot.commands.AimAndShoot;
+import frc.robot.commands.ExitStartingConfiguration;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.SerializerSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import friarLib2.commands.RunForTime;
 
 public class ScorePreloads extends SequentialCommandGroup {
-    public ScorePreloads (ArmSubsystem arm, DriveSubsystem drive, SerializerSubsystem serializer, ShooterSubsystem shooter) {
+    public ScorePreloads (double drivePower, ArmSubsystem arm, DriveSubsystem drive, SerializerSubsystem serializer, ShooterSubsystem shooter) {
         addCommands(
+            new ExitStartingConfiguration(arm),
             new RunForTime(
                 new AimAndShoot(
                     Constants.Shooter.SHOOT_FROM_STARTING_LINE, 
-                    () -> true, 
-                    arm, serializer, shooter), 
-                14),
-            new DriveForward(-.25, .5, drive)
+                    () -> shooter.getMainFlywheelSpeed() >= 2000, 
+                    arm, serializer, shooter),
+                    /*:)*/
+                12),
+            new DriveForward(drivePower, 1.5, drive)
         );
     }
 }