Merge branch 'main' into Upstream_Changes

.vscode/extensions.json

@@ -8,6 +8,7 @@
     "wpilibsuite.vscode-wpilib",
     "vscjava.vscode-gradle",
     "vscjava.vscode-java-debug",
-    "redhat.java"
+    "redhat.java",
+    "yzhang.markdown-all-in-one"
   ]
 }

.wpilib/wpilib_preferences.json

@@ -2,5 +2,5 @@
   "enableCppIntellisense": false,
   "currentLanguage": "java",
   "projectYear": "2024",
-  "teamNumber": 9985
+  "teamNumber": 5829
 }

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

@@ -85,6 +85,7 @@ public static final class FlywheelConstants {
     public static final double ks = 0;
     public static final double kv = 0.00025;
     public static final double shootingVelocity = 4500; // revolutions per second
+    public static final double slowShootingVelocity = 10; // ! guessed value
   }
 
   public static final class IntakeConstants {
@@ -105,7 +106,7 @@ public static final class IntakeConstants {
     // Intake Feedforward characterization constants
     public static final double ks = 0.1;
     public static final double kv = 5;
-    public static double percentPower = 0.45 * 1.05;
+    public static double percentPower = 1;
 
     public static final int conveyorSensor = 0;
     public static final int shooterSensor = 1;
@@ -122,10 +123,10 @@ public static final class ArmConstants {
     public static final double kMaxOutput = 0.33;
 
     // // Arm PID constants
-    public static final double kP = 0.7;
+    public static final double kP = 0.65;
     public static final double kI = 0.0;
-    public static final double kD = 0.025;
-    public static final double kWeightBasedFF = 0.025;
+    public static final double kD = 0.035;
+    public static final double kWeightBasedFF = 0.025 * 1.3 * 1.15;
 
     // // Arm PID constants
     // public static final double kP = 0;
@@ -160,16 +161,16 @@ public static final class ClimberConstants {
     public static final int kCurrentLimit = 30;
 
     public static final double initialPosition = 0.7;
-    public static final double minPosition = 0.0;
-    public static final double maxPosition = 0.55;
+    public static final double minPosition = -2; // 0
+    public static final double maxPosition = 2; // 0.55
 
     public static final double climberConversion = 1;
 
     // in meters, i.e. that many meters per rotation
     public static final double gearCircumfrence = 0.134032531982;
 
     // Climber PID constants
-    public static final double kP = 0.1;
+    public static final double kP = 1; // 0.1
     public static final double kI = 0.0;
     public static final double kD = 0.0;
 

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

@@ -178,16 +178,14 @@ public RobotContainer() {
     // NamedCommands.registerCommand("ShootFarPosition", new
     // ShootFarCommand(sArm).withTimeout(2.0));
 
-    NamedCommands.registerCommand(
-        "PreRunShooter", new PreRunShooter(sIntake, sFlywheel).withTimeout(4.0));
+    NamedCommands.registerCommand("PreRunShooter", new PreRunShooter(sFlywheel).withTimeout(4.0));
 
     // Groups of the above
     NamedCommands.registerCommand(
         "StartGroup",
         new SequentialCommandGroup(
             new ParallelDeadlineGroup(
-                ShootClose.run(sArm).withTimeout(2),
-                new PreRunShooter(sIntake, sFlywheel).withTimeout(4.0)),
+                ShootClose.run(sArm).withTimeout(2), new PreRunShooter(sFlywheel).withTimeout(4.0)),
             new ShootNote(sIntake, sFlywheel, sArm).withTimeout(3.0)));
 
     NamedCommands.registerCommand( // The name is inaccurate
@@ -344,54 +342,16 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+
+    // # Driver controller configuration
+
     sDrive.setDefaultCommand(
         DriveCommands.joystickDrive(
             sDrive,
             () -> -driverController.getLeftY(),
             () -> -driverController.getLeftX(),
             () -> -driverController.getRightX()));
 
-    // driverController
-    //     .a()
-    //     .whileTrue(
-    //         Commands.startEnd(
-    //             () ->
-    //                 sFlywheel.runVelocity(
-    //                     -flywheelSpeedInput
-    //                         .get()), // ! Is the smartdashboard thing permanent? surely not?
-    //             sFlywheel::stop,
-    //             sFlywheel));
-    // driverController
-    //     .a()
-    //     .whileFalse(Commands.startEnd(() -> sFlywheel.runVelocity(0), sFlywheel::stop,
-    // sFlywheel));
-
-    // driverController // TODO Reverse intake needed, also it stops randomly after a bit, get rid
-    // of
-    //     .b()
-    //     .whileTrue(
-    //         Commands.startEnd(
-    //             () -> sIntake.runVelocity(Constants.IntakeConstants.velocity),
-    //             sIntake::stop,
-    //             sIntake));
-
-    // driverController
-    //     .b()
-    //     .whileFalse(Commands.startEnd(() -> sIntake.runVelocity(0), sIntake::stop, sIntake));
-
-    // driverController // TODO Reverse intake needed, also it stops randomly after a bit, get rid
-    // of
-    //     .x()
-    //     .whileTrue(
-    //         Commands.startEnd(
-    //             () -> sIntake.runVelocity(-Constants.IntakeConstants.velocity),
-    //             sIntake::stop,
-    //             sIntake));
-
-    // driverController
-    //     .b()
-    //     .whileFalse(Commands.startEnd(() -> sIntake.runVelocity(0), sIntake::stop, sIntake));
-
     driverController
         .start()
         .whileTrue(Commands.startEnd(() -> sDrive.resetRotation(), sDrive::stop, sDrive));
@@ -404,23 +364,26 @@ private void configureButtonBindings() {
         .rightTrigger()
         .whileTrue(Commands.startEnd(() -> sDrive.toggleSlowMode(), sDrive::stop, sDrive));
 
-    // Operator controller configurations
+    // # Operator controller configuration
+
+    // Arm Controls (Controlled by right stick Y-axis)
     sArm.setDefaultCommand(ArmCommands.joystickDrive(sArm, () -> -operatorController.getRightY()));
 
+    // Arm Positions
+    operatorController.povDown().whileTrue(ShootFar.run(sArm));
+    operatorController.povRight().whileTrue(ShootMedium.run(sArm));
+    operatorController.povUp().whileTrue(ShootClose.run(sArm));
+
+    // operatorController.y().whileTrue(Upwards.run(sArm)); //Note that Y is taken
+    operatorController.x().whileTrue(AmpShot.run(sArm));
+    operatorController.a().whileTrue(FloorPickup.run(sArm));
+    operatorController.b().whileTrue(StowPosition.run(sArm));
+
+    // Intake Controls (Three ways: #1 right trigger, which goes backwards, #2 “Y”, full power, and
+    // #3 is left trigger, which has sensor logic)
     sIntake.setDefaultCommand(
-        IntakeShooterControls.intakeShooterDrive(
-            sIntake,
-            sFlywheel,
-            sArm,
-            () -> operatorController.getLeftTriggerAxis(),
-            () -> operatorController.getRightTriggerAxis(),
-            () -> operatorController.leftBumper().getAsBoolean(),
-            () -> operatorController.rightBumper().getAsBoolean()));
-    // repurposed for intake override
-    //     operatorController
-    // .a()
-    // .whileTrue(
-    //     Commands.startEnd(() -> sClimber.runTargetPosition(0), sClimber::stop, sClimber));
+        IntakeShooterControls.intakeShooterDefaultCommand(
+            sIntake, () -> operatorController.getRightTriggerAxis()));
 
     operatorController
         .y()
@@ -430,6 +393,25 @@ private void configureButtonBindings() {
                 sIntake::stop,
                 sIntake));
 
+    operatorController.leftTrigger().whileTrue(new IntakeNoteAndAlign(sIntake));
+    // operatorController.leftTrigger().whileTrue(new IntakeNote(sIntake));
+
+    // Flywheel commands
+    operatorController.rightBumper().whileTrue(new ShootNote(sIntake, sFlywheel, sArm));
+    operatorController.leftBumper().whileTrue(new PreRunShooter(sFlywheel));
+    sFlywheel.setDefaultCommand(
+        new PreRunShooter(sFlywheel, true)); // Runs the flywheel slowly at all times
+
+    // Climber controls (The first one is 90% probably the one that works.)
+    // sClimber.setDefaultCommand(
+    //     ClimberCommands.buttonDrive(
+    //         sClimber, operatorController.leftBumper(), operatorController.rightBumper()));
+
+    //     operatorController
+    // .a()
+    // .whileTrue(
+    //     Commands.startEnd(() -> sClimber.runTargetPosition(0), sClimber::stop, sClimber));
+
     // operatorController
     //     .b()
     //     .whileTrue(
@@ -438,29 +420,17 @@ private void configureButtonBindings() {
     //             sClimber::stop,
     //             sClimber));
 
-    operatorController.povDown().whileTrue(ShootFar.run(sArm));
-    operatorController.povRight().whileTrue(ShootMedium.run(sArm));
-    operatorController.povUp().whileTrue(ShootClose.run(sArm));
-
-    operatorController.rightBumper().whileTrue(new ShootNote(sIntake, sFlywheel, sArm));
-    operatorController.leftBumper().whileTrue(new PreRunShooter(sIntake, sFlywheel));
-
-    // run straight up position when y is pressed on operator. Using command Upwards
-
-    operatorController.leftTrigger().whileTrue(new IntakeNoteAndAlign(sIntake));
-
-    // ! tempory testing keybinds
-    // operatorController.y().whileTrue(Upwards.run(sArm));
-    // operatorController.y().whileTrue(new IntakeNote(sIntake, sArm, sFlywheel));
-    // operatorController.y().whileTrue(new ShootNote(sIntake, sArm, sFlywheel));
-
-    // run straight forwards position when x is pressed
-    operatorController.x().whileTrue(AmpShot.run(sArm));
-    // operatorController.x().whileTrue(new AdjustNote(sIntake, sArm, sFlywheel));
-
-    operatorController.a().whileTrue(FloorPickup.run(sArm));
-
-    operatorController.b().whileTrue(StowPosition.run(sArm));
+    // driverController
+    //     .povUp()
+    //     .whileTrue(
+    //         Commands.startEnd(
+    //             () -> ClimberCommands.buttonDrive(sClimber, () -> 1), sClimber::stop, sClimber));
+    // driverController
+    //     .povDown()
+    //     .whileTrue(
+    //         Commands.startEnd(
+    //             () -> ClimberCommands.buttonDrive(sClimber, () -> -1), sClimber::stop,
+    // sClimber));
   }
 
   /**

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

@@ -21,8 +21,8 @@ public class AdjustNote extends Command {
   private Intake intake;
   private int phase =
       1; // 1 is moving note to shootersensor, 2 is moving note away from shooterSensor, 3 = done
-  private double forwardsIntakeSpeed = 0.04;
-  private double backwardsIntakeSpeed = 0.04;
+  private double forwardsIntakeSpeed = 1;
+  private double backwardsIntakeSpeed = 1;
 
   private long phase2StartTime;
 

src/main/java/frc/robot/commands/ControlCommands/ClimberCommands.java

@@ -0,0 +1,53 @@
+// Copyright 2021-2024 FRC 6328, FRC 5829
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.commands.ControlCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.climber.Climber;
+
+public class ClimberCommands {
+
+  private static final double MAX_MS = 0.01;
+
+  private ClimberCommands() {}
+
+  /** Wrist command using one axis of a joystick (controlling wrist velocity). */
+  public static Command buttonDrive(Climber climber, Trigger leftTrigger, Trigger rightTrigger) {
+    return Commands.run(
+        () -> {
+          boolean left = leftTrigger.getAsBoolean();
+          boolean right = rightTrigger.getAsBoolean();
+          double magnitude = 0;
+
+          if (left && right) { // there's probably a better way of doing this
+            magnitude = 0;
+          } else if ((left) && (!right)) {
+            magnitude = 1;
+          } else if ((!left) && (right)) {
+            magnitude = -1;
+          } else if ((!left) && (!right)) {
+            magnitude = 0;
+          }
+
+          // Calcaulate new rotational velocity
+          double position = magnitude * MAX_MS;
+
+          // Send command to wrist subsystem to run wrist
+          climber.runTargetPosition(position);
+        },
+        climber);
+  }
+}

src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java

@@ -75,7 +75,7 @@ public static Command joystickDrive(
           linearMagnitude = linearMagnitude * linearMagnitude;
           omega = Math.copySign(omega * omega, omega);
 
-          // Calcaulate new linear velocity
+          // Calculate new linear velocity
           Translation2d linearVelocity =
               new Pose2d(new Translation2d(), linearDirection)
                   .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
@@ -87,9 +87,11 @@ public static Command joystickDrive(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                   1.25 * omega * drive.getMaxAngularSpeedRadPerSec() / 2.3,
-                  isFlipped // TEST TODO
-                      ? drive.getRotation().plus(new Rotation2d(Math.PI))
-                      : drive.getRotation()));
+                  drive.getRotation() // .plus(new Rotation2d(Math.PI)) // TEST TODO
+                  // isFlipped
+                  //     ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                  //     : drive.getRotation()
+                  ));
         },
         drive);
   }