From 578be9dec55b1af27287721dac0eab07bbaf073c Mon Sep 17 00:00:00 2001
From: siyoyoCode <siyona101215@gmail.com>
Date: Sun, 4 Feb 2024 11:54:19 -0800
Subject: [PATCH] edited shooter flywheel ready command

---
 .../flywheel/ShooterFlywheelReadyCommand.java       | 13 ++++---------
 .../subsystems/shooter/ShooterFeederSubsystem.java  |  3 ++-
 .../shooter/ShooterFlywheelSubsystem.java           |  9 +++++++--
 3 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java
index 06545d36..dbec7d07 100644
--- a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java
+++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java
@@ -16,15 +16,10 @@ public void initialize() {
         shooterSubsystem.setShooterMotorSpeed(shooterSubsystem.SHOOTER_MOTOR_SPEED);
     }
 
-    //pivot: vertical, auto-aim
-    //feed: load, shoot
-    //shooter: stop, ready shooters
-
-    //load note
-    //shoot
-    //ready shooter
-    //vertical 
-    //auto-angle
+    @Override
+    public boolean isFinished(){
+        return (Math.abs(shooterSubsystem.getFlywheelSpeed() - shooterSubsystem.SHOOTER_MOTOR_SPEED) < shooterSubsystem.SPEED_TOLERANCE);
+    }
 
     @Override
     public void end(boolean interrupted) {
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java
index a7f6527e..25ecc198 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java
@@ -32,9 +32,10 @@ public ShooterFeederSubsystem(){
     public void setFeederMotorSpeed(double speed){
         feederMotor.set(TalonSRXControlMode.PercentOutput, speed);
         System.out.println("feeding motor speed is: " + feederMotor.getMotorOutputPercent());
-
     }
 
+
+
     public int getProximity(){
         System.out.println("proximity: " + shooterSensor.getProximity());
         return shooterSensor.getProximity();
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
index a24abbd2..fccc518d 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
@@ -14,13 +14,13 @@ public class ShooterFlywheelSubsystem extends SubsystemBase {
 
     //change later (ALL OF THEM ARE PLACEHOLDERS)
     int IDNUMBER = 10; //so I remember to change them later
-    // public static final int SHOOTER_MOTOR_ONE_ID = 13;
-    // public static final int SHOOTER_MOTOR_TWO_ID = 14;
     public final double SHOOTER_MOTOR_SPEED = 1;
+    public final double SPEED_TOLERANCE = 0.7;
 
     //motors
     private final CANSparkMax shooterMotor;
     private final CANSparkMax shooterMotorTwo;
+    private double currentSpeed;
 
     //devices
     private ShooterState currentState; //an enum state thing
@@ -54,7 +54,12 @@ public void setShooterState(ShooterState newState){
 
     public void setShooterMotorSpeed(double speed){
         shooterMotor.setVoltage(-speed * 12);
+        currentSpeed = speed;
         System.out.println("shooter motor speed is: " + shooterMotor.get());
     }
 
+    public double getFlywheelSpeed(){
+        return currentSpeed;
+    }
+
 }
\ No newline at end of file