Add changes from 3/11/24

frc2204 · Mar 12, 2024 · 4995dec · 4995dec
1 parent f998f6b
commit 4995dec
Show file tree

Hide file tree

Showing 25 changed files with 387 additions and 177 deletions.
diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
@@ -1,6 +1,6 @@
 {
-  "enableCppIntellisense": false,
-  "currentLanguage": "java",
-  "projectYear": "2024",
-  "teamNumber": 2204
-}
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2024",
+    "teamNumber": 2204
+}
diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "frc-2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 10;
-  public static final String GIT_SHA = "4af6a0d87fab6e63d1cb30c4959e65c908757abf";
-  public static final String GIT_DATE = "2024-03-06 15:29:43 PST";
+  public static final int GIT_REVISION = 11;
+  public static final String GIT_SHA = "f998f6b7e2633dad7ed07b42397f1271f6200e79";
+  public static final String GIT_DATE = "2024-03-10 13:54:54 PDT";
   public static final String GIT_BRANCH = "james/swerve";
-  public static final String BUILD_DATE = "2024-03-09 19:35:02 PST";
-  public static final long BUILD_UNIX_TIME = 1710041702135L;
+  public static final String BUILD_DATE = "2024-03-11 21:28:28 PDT";
+  public static final long BUILD_UNIX_TIME = 1710217708081L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

diff --git a/src/main/java/org/rambots/Robot.kt b/src/main/java/org/rambots/Robot.kt
@@ -52,7 +52,7 @@ object Robot : LoggedRobot() {
 
         when (Constants.getMode()) {
             Constants.Mode.REAL -> {
-//                Logger.addDataReceiver(WPILOGWriter())
+                Logger.addDataReceiver(WPILOGWriter())
                 Logger.addDataReceiver(NT4Publisher())
             }
 

diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt
@@ -14,25 +14,22 @@ package org.rambots
 
 import com.pathplanner.lib.auto.AutoBuilder
 import edu.wpi.first.math.geometry.*
-import edu.wpi.first.wpilibj.PS5Controller
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import edu.wpi.first.wpilibj2.command.Subsystem
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser
 import org.rambots.commands.*
-import org.rambots.subsystems.ArmSubsystem
-import org.rambots.subsystems.ElevatorSubsystem
+import org.rambots.subsystems.*
 import org.rambots.subsystems.drive.*
 import org.rambots.subsystems.drive.DriveConstants.moduleConfigs
 import org.rambots.subsystems.vision.AprilTagVision
 import org.rambots.subsystems.vision.AprilTagVisionIO
 import org.rambots.subsystems.vision.AprilTagVisionIOLimelight
 import org.rambots.subsystems.vision.AprilTagVisionIOPhotonVisionSIM
 import org.rambots.util.VisionHelpers.TimestampedVisionUpdate
-import java.util.function.Consumer
-import kotlin.math.pow
 
 
 /**
@@ -166,21 +163,56 @@ object RobotContainer {
         drive.defaultCommand = DriveCommands.joystickDrive(
             drive,
             driveController,
-            { (-controller.leftY).pow(3.0) },
-            { (-controller.leftX).pow(3.0) },
+            { -controller.leftY },
+            { -controller.leftX },
             { -controller.rightX }
         )
         ArmSubsystem.defaultCommand = ArmPositionCommand { ArmSubsystem.desiredPosition }
         ElevatorSubsystem.defaultCommand = ElevatorPositionCommand { ElevatorSubsystem.desiredPosition }
+        WristSubsystem.defaultCommand = WristPositionCommand { WristSubsystem.desiredPosition }
 
-        controller.L1().whileTrue(AmpScoring())
-        controller.R2().whileTrue(SourceIntake())
 
-        controller.square().whileTrue(ArmPositionCommand { -70.0 })
-        controller.circle().whileTrue(ArmPositionCommand { 0.0 })
+//        controller.L1().whileTrue(AmpScoring())
+//        controller.R2().whileTrue(SourceIntake())
+//
+//        controller.square().whileTrue(ArmPositionCommand { -70.0 })
+//        controller.circle().whileTrue(ArmPositionCommand { 0.0 })
+
+//        controller.povUp().whileTrue(ElevatorPositionCommand { 0.0 })
+//        controller.povDown().whileTrue(ElevatorPositionCommand { -48.0 })
+//
+//        controller.povLeft().whileTrue(WristPositionCommand { -125.0 })
+//        controller.povRight().whileTrue(WristPositionCommand { 0.0 })
+
+//        controller.cross().whileTrue(Commands.startEnd({ ShooterSubsystem.intake() }, { ShooterSubsystem.stopIntake()}, ShooterSubsystem ))
+//        controller.circle().whileTrue(Commands.startEnd({ShooterSubsystem.shoot(3000.0, 3000.0)}, {ShooterSubsystem.stopShooter()}, ShooterSubsystem))
+
+        controller.L2().onTrue(SuperStructure.intakeCommand)
+        controller.L2().onFalse(SuperStructure.homeCommandFromIntake)
 
-        controller.povUp().whileTrue(ElevatorPositionCommand { 0.0 })
-        controller.povDown().whileTrue(ElevatorPositionCommand { -70.0 })
+        controller.circle().onTrue(Commands.runOnce({ ShooterSubsystem.shoot(6000.0, 6000.0) }, ShooterSubsystem))
+        controller.circle().onFalse(Commands.runOnce({ ShooterSubsystem.stopShooter() }, ShooterSubsystem))
+
+        controller.square().onTrue(Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem))
+        controller.square().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem))
+
+        controller.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++}))
+        controller.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--}))
+
+
+//        controller.R2().whileTrue(
+//            Commands.startEnd(
+//                {
+//                    ShooterSubsystem.shoot(7000.0, 7000.0)
+//                    ShooterSubsystem.intake()
+//                },
+//                {
+//                    ShooterSubsystem.stopShooter()
+//                    ShooterSubsystem.stopIntake()
+//                },
+//                ShooterSubsystem
+//            )
+//        )
 
 //        controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType))
 

diff --git a/src/main/java/org/rambots/commands/ArmPositionCommand.kt b/src/main/java/org/rambots/commands/ArmPositionCommand.kt
@@ -1,14 +1,11 @@
 package org.rambots.commands
 
 import edu.wpi.first.wpilibj2.command.Command
-import org.rambots.config.ArmConstants.ARM_ACCEPTABLE_ERROR
 import org.rambots.subsystems.ArmSubsystem
-import org.rambots.util.CommandUtils.generateErrorRange
 
-class ArmPositionCommand(private val angle: () -> Double, private val finishState: () -> Double) : Command() {
+class ArmPositionCommand(private val angle: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() {
 
-    constructor(angle: () -> Double) : this(angle, angle)
-    constructor(angle: Double) : this ({angle})
+    constructor(angle: () -> Double) : this(angle, { true })
 
     init {
         addRequirements(ArmSubsystem)
@@ -23,7 +20,7 @@ class ArmPositionCommand(private val angle: () -> Double, private val finishStat
     }
 
     override fun isFinished(): Boolean {
-        return generateErrorRange(ArmSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke())
+        return finishCondition.invoke(ArmSubsystem.position)
     }
 
 }
diff --git a/src/main/java/org/rambots/commands/AutoShootCommand.kt b/src/main/java/org/rambots/commands/AutoShootCommand.kt
@@ -0,0 +1,5 @@
+package org.rambots.commands
+
+class AutoShootCommand {
+
+}
diff --git a/src/main/java/org/rambots/commands/DriveCommands.java b/src/main/java/org/rambots/commands/DriveCommands.java
@@ -39,27 +39,16 @@ private DriveCommands() {
     /**
      * Field relative drive command using two joysticks (controlling linear and angular velocities).
      */
-    public static Command joystickDrive(
-            Drive drive,
-            DriveController driveMode,
-            DoubleSupplier xSupplier,
-            DoubleSupplier ySupplier,
-            DoubleSupplier omegaSupplier) {
+    public static Command joystickDrive(Drive drive, DriveController driveMode, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) {
         return Commands.runOnce(
                 () -> {
                     // Apply deadband
-                    double linearMagnitude =
-                            MathUtil.applyDeadband(
-                                    Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
-                    Rotation2d linearDirection =
-                            new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+                    double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
+                    Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
                     double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);
                     if (driveMode.isHeadingControlled()) {
                         final var targetAngle = driveMode.getHeadingAngle();
-                        omega =
-                                Drive.getThetaController()
-                                        .calculate(
-                                                drive.getPose().getRotation().getRadians(), targetAngle.get().getRadians());
+                        omega = Drive.getThetaController().calculate(drive.getPose().getRotation().getRadians(), targetAngle.get().getRadians());
                         if (Drive.getThetaController().atGoal()) {
                             omega = 0;
                         }
@@ -71,19 +60,12 @@ public static Command joystickDrive(
                     omega = Math.copySign(omega * omega, omega);
 
                     // Calcaulate new linear velocity
-                    Translation2d linearVelocity =
-                            new Pose2d(new Translation2d(), linearDirection)
-                                    .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
-                                    .getTranslation();
+                    Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection).transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
 
                     // Convert to field relative speeds & send command
-                    boolean isFlipped =
-                            DriverStation.getAlliance().isPresent()
-                                    && DriverStation.getAlliance().get() == Alliance.Red;
+                    boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red;
                     drive.runVelocity(
-                            ChassisSpeeds.fromFieldRelativeSpeeds(
-                                    linearVelocity.getX() * drivetrainConfig.maxLinearVelocity(),
-                                    linearVelocity.getY() * drivetrainConfig.maxLinearVelocity(),
+                            ChassisSpeeds.fromFieldRelativeSpeeds(linearVelocity.getX() * drivetrainConfig.maxLinearVelocity(), linearVelocity.getY() * drivetrainConfig.maxLinearVelocity(),
                                     omega * drivetrainConfig.maxAngularVelocity(),
                                     isFlipped
                                             ? drive.getRotation().plus(new Rotation2d(Math.PI))

diff --git a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt b/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt
@@ -1,14 +1,11 @@
 package org.rambots.commands
 
 import edu.wpi.first.wpilibj2.command.Command
-import org.rambots.config.ArmConstants.ARM_ACCEPTABLE_ERROR
 import org.rambots.subsystems.ElevatorSubsystem
-import org.rambots.util.CommandUtils.generateErrorRange
 
-class ElevatorPositionCommand(private val position: () -> Double, private val finishState: () -> Double) : Command() {
+class ElevatorPositionCommand(private val position: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() {
 
-    constructor(position: () -> Double) : this(position, position)
-    constructor(position: Double) : this ({position})
+    constructor(position: () -> Double) : this(position, { true })
 
     init {
         addRequirements(ElevatorSubsystem)
@@ -23,7 +20,7 @@ class ElevatorPositionCommand(private val position: () -> Double, private val fi
     }
 
     override fun isFinished(): Boolean {
-        return generateErrorRange(ElevatorSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke())
+        return finishCondition.invoke(ElevatorSubsystem.position)
     }
 
 }
diff --git a/src/main/java/org/rambots/commands/MultiDistanceShot.java b/src/main/java/org/rambots/commands/MultiDistanceShot.java
diff --git a/src/main/java/org/rambots/commands/PathFinderAndFollow.java b/src/main/java/org/rambots/commands/PathFinderAndFollow.java
@@ -18,6 +18,7 @@
  * A command that runs pathfindThenFollowPath based on the current drive mode.
  */
 public class PathFinderAndFollow extends Command {
+
     private final Supplier<DriveController.DriveModeType> driveModeSupplier;
     private Command scoreCommand;
     private Command pathRun;

diff --git a/src/main/java/org/rambots/commands/ShooterCommand.kt b/src/main/java/org/rambots/commands/ShooterCommand.kt
@@ -0,0 +1,27 @@
+package org.rambots.commands
+
+import edu.wpi.first.wpilibj2.command.Command
+import org.rambots.subsystems.ShooterSubsystem
+
+class ShooterCommand(private val topVelocity: () -> Double, private val bottomVelocity: () -> Double , private val finishCondition: (topVelocity: Double, bottomVelocity: Double) -> Boolean): Command() {
+
+    constructor(topVelocity: () -> Double, bottomVelocity: () -> Double): this(topVelocity, bottomVelocity, { _,_ -> true })
+
+    init {
+        addRequirements(ShooterSubsystem)
+    }
+
+    override fun initialize() {
+        ShooterSubsystem.topDesiredVelocity = topVelocity.invoke()
+        ShooterSubsystem.bottomDesiredVelocity = bottomVelocity.invoke()
+    }
+
+    override fun execute() {
+        ShooterSubsystem.shoot(topVelocity.invoke(), bottomVelocity.invoke())
+    }
+
+    override fun isFinished(): Boolean {
+        return finishCondition.invoke(topVelocity.invoke(), bottomVelocity.invoke())
+    }
+
+}