From 4ba79065bd979d073fa579bba52adcbeb7d02c06 Mon Sep 17 00:00:00 2001
From: joelzoto <118407587+joelzoto@users.noreply.github.com>
Date: Sat, 1 Feb 2025 14:33:33 -0500
Subject: [PATCH] Changed the positions for the shooter pivot and added the
 flywheel commands

---
 .../commands/AlgaeFlyWheelCommands.java       | 16 ++++++
 .../commands/SetVelocityAlgaeFlyWheel.java    | 57 +++++++++++++++++++
 .../commands/SetAngleAlgaePivot.java          | 19 ++-----
 3 files changed, 78 insertions(+), 14 deletions(-)
 create mode 100644 src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java
 create mode 100644 src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java

diff --git a/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java b/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java
new file mode 100644
index 0000000..4253f91
--- /dev/null
+++ b/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java
@@ -0,0 +1,16 @@
+package frc.robot.algaeflywheel.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.function.Supplier;
+
+public class AlgaeFlyWheelCommands {
+    public static Command setAngularVelocity(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier) {
+        return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier);
+    }
+
+    public static Command setAngularVelocity(Supplier<Rotation2d> velocitySupplier) {
+        return setAngularVelocity(velocitySupplier, velocitySupplier);
+    }
+}
diff --git a/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java b/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java
new file mode 100644
index 0000000..3d0ce89
--- /dev/null
+++ b/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java
@@ -0,0 +1,57 @@
+package frc.robot.algaeflywheel.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.algaeflywheel.AlgaeFlyWheel;
+import frc.robot.operator.OperatorXbox;
+
+import java.util.function.Supplier;
+
+public class SetVelocityAlgaeFlyWheel extends Command {
+    private final AlgaeFlyWheel flywheel;
+    private final Supplier<Rotation2d> leftVelocitySupplier, rightVelocitySupplier;
+
+    private final Rotation2d kAllowedError = Rotation2d.fromRotations(5); // 300 RPM
+
+    SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier) {
+        flywheel = AlgaeFlyWheel.getInstance();
+        this.leftVelocitySupplier = leftVelocitySupplier;
+        this.rightVelocitySupplier = rightVelocitySupplier;
+    }
+
+    SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> velocitySupplier) {
+        this(velocitySupplier, velocitySupplier);
+    }
+
+    @Override
+    public void execute() {
+        final var leftVel = leftVelocitySupplier.get();
+        final var rightVel = rightVelocitySupplier.get();
+        flywheel.setRightFlywheelVelocity(leftVel);
+        flywheel.setLeftFlywheelVelocity(rightVel);
+
+        SmartDashboard.putBoolean("Shooter Ready (left)", (Math.abs(leftVel.getRotations()) - (Math.abs(flywheel.getLeftFlywheelVelocity().getRotations()))) < kAllowedError.getRotations());
+        SmartDashboard.putBoolean("Shooter Ready (right)", (Math.abs(rightVel.getRotations()) - (Math.abs(flywheel.getRightFlywheelVelocity().getRotations()))) < kAllowedError.getRotations());
+    
+        var leftAtVel = (Math.abs(leftVel.getRotations()) - (Math.abs(flywheel.getLeftFlywheelVelocity().getRotations()))) < kAllowedError.getRotations();
+        var rightAtVel = (Math.abs(rightVel.getRotations()) - (Math.abs(flywheel.getRightFlywheelVelocity().getRotations()))) < kAllowedError.getRotations();
+        if (leftAtVel || rightAtVel) {
+            OperatorXbox.getInstance().controller.getHID().setRumble(RumbleType.kBothRumble, 1);
+        } else {
+            OperatorXbox.getInstance().controller.getHID().setRumble(RumbleType.kBothRumble, 0);
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean isInterrupted) {
+        flywheel.setRightFlywheelVelocity(Rotation2d.fromDegrees(0));
+        flywheel.setLeftFlywheelVelocity(Rotation2d.fromDegrees(0));
+    }
+}
diff --git a/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java b/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java
index c94a80f..bced1aa 100644
--- a/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java
+++ b/src/main/java/frc/robot/algaeshooterpivot/commands/SetAngleAlgaePivot.java
@@ -1,6 +1,4 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
+
 
 package frc.robot.algaeshooterpivot.commands;
 
@@ -10,7 +8,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.algaeshooterpivot.AlgaeShooterPivot;
 
-/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class SetAngleAlgaePivot extends Command {
   AlgaeShooterPivot mAlgaeShooterPivot;
     Supplier<Rotation2d> targetSupplier;
@@ -18,16 +15,10 @@ public class SetAngleAlgaePivot extends Command {
 
         public enum Preset {
         kZero(Rotation2d.fromDegrees(2)),
-        kHandoff(Rotation2d.fromDegrees(0)),
-        kHandoffClear(Rotation2d.fromDegrees(5)),
-        kShooterNear(Rotation2d.fromDegrees(0)),
-        kShooterMid(Rotation2d.fromDegrees(22.25)),
-        kShooterFarAuton(Rotation2d.fromDegrees(26)),
-        kShooterFar(Rotation2d.fromDegrees(25.5)),
-        kTrap(Rotation2d.fromDegrees(153.15)),
-        kClimb(Rotation2d.fromDegrees(170)),
-        kPass(Rotation2d.fromDegrees(10)),
-        kAmp(Rotation2d.fromDegrees(85));
+        kStowed(Rotation2d.fromDegrees(0)),
+        kElveatorHigh(Rotation2d.fromDegrees(0)),
+        kCoral(Rotation2d.fromDegrees(0)),
+        kShoot(Rotation2d.fromDegrees(0));
 
         Rotation2d target;