Merge pull request #10 from awtybots/development_features

Reimplemented a variety of improvements from the old development branch

.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 {
@@ -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
@@ -293,56 +291,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));
@@ -355,23 +313,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()
@@ -381,6 +342,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(
@@ -389,29 +369,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/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

@@ -69,7 +69,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()))
@@ -81,7 +81,11 @@ public static Command joystickDrive(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                   1.25 * omega * drive.getMaxAngularSpeedRadPerSec() / 2.3,
-                  drive.getRotation()));
+                  drive.getRotation() //.plus(new Rotation2d(Math.PI))
+                  // isFlipped
+                  //     ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                  //     : drive.getRotation()
+                  ));
         },
         drive);
   }

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

@@ -15,59 +15,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.flywheel.Flywheel;
 import frc.robot.subsystems.intake.Intake;
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 public class IntakeShooterControls {
   private static final double DEADBAND = 0.3;
-  private static final double MAXINCHESPERSECOND = 3;
 
-  private IntakeShooterControls() {}
+  public static Command intakeShooterDefaultCommand(
+      Intake intake, DoubleSupplier rightTriggerSupplier) {
 
-  public static Command intakeShooterDrive(
-      Intake intake,
-      Flywheel flywheel,
-      Arm arm,
-      DoubleSupplier leftTriggerSupplier,
-      DoubleSupplier rightTriggerSupplier,
-      BooleanSupplier leftBumperSupplier,
-      BooleanSupplier rightBumperSupplier) {
-    // Left trigger runs outtake, right trigger triggers intake, and right bumper triggers the
-    // shooter (and the intake once at velocity).
-    // Intake stops on detection of note in first mode.
     return Commands.run(
         () -> {
-          /*
-          if (leftBumperSupplier.getAsBoolean()) {
-            // flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity);
-          }
-          if (rightBumperSupplier.getAsBoolean()) {
-
-          } else {
-
-            // double fwdSpeed = leftTriggerSupplier.getAsDouble();
-            double revSpeed = rightTriggerSupplier.getAsDouble();
-            // fwdSpeed = MathUtil.applyDeadband(fwdSpeed, DEADBAND);
-            // revSpeed = MathUtil.applyDeadband(revSpeed, DEADBAND);
-
-            double stickMagnitude = -revSpeed;
-            stickMagnitude =
-                stickMagnitude * stickMagnitude * stickMagnitude; // more control over lower speeds
-
-            if (Math.abs(revSpeed) > DEADBAND) {
-              intake.runPercentSpeed(Constants.IntakeConstants.percentPower * stickMagnitude);
-            }
-          }
-          */
           if (Math.abs(rightTriggerSupplier.getAsDouble()) > DEADBAND) {
             intake.runPercentSpeed(
                 -Constants.IntakeConstants.percentPower * rightTriggerSupplier.getAsDouble());
           }
         },
-        intake,
-        flywheel);
+        intake);
   }
 }

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

@@ -14,27 +14,19 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
-import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.intake.Intake;
 
 // Moves the note so that it is detected by the conveySensor but not shooterSensor
 
 public class IntakeNote extends Command {
 
   private Intake intake;
-  private Drive drive;
 
   public IntakeNote(Intake intake) {
     this.intake = intake;
     addRequirements(intake);
   }
 
-  public IntakeNote(Intake intake, Drive drive) { // TODO move forward and back
-    this.intake = intake;
-    this.drive = drive;
-    addRequirements(intake, drive);
-  }
-
   // Called once at the beginning
   @Override
   public void initialize() {}