added climber stuff and tweaked auto stuff (im tweakin)

src/main/java/frc4388/robot/subsystems/Climber.java

@@ -0,0 +1,39 @@
+// 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 frc4388.robot.subsystems;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
+import frc4388.robot.Constants.ClimbConstants;
+
+//! 6.5C Scoring Criteria for Stage
+
+public class Climber extends SubsystemBase {
+  /** Creates a new Climber. */
+  TalonFX climbMotor;
+
+  public Climber(TalonFX climbMotor) {
+    this.climbMotor = climbMotor;
+  }
+
+  public void climbOut() {
+    climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED);
+  }
+
+  public void climbIn() {
+    climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+  }
+
+  public void stopClimb() {
+    climbMotor.set(0.d);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc4388/robot/subsystems/Limelight.java

@@ -71,7 +71,7 @@ public class Limelight extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
 
-    isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false);
+    isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
     double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
 
     if(newPose != cameraPose){

src/main/java/frc4388/robot/subsystems/Shooter.java

@@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.ShooterConstants;
 
+import frc4388.robot.subsystems.Limelight;
+
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -22,13 +24,24 @@ public class Shooter extends SubsystemBase {
   private TalonFX leftShooter;
   private TalonFX rightShooter;
 
+  private Limelight limelight;
+
+  private int spinMode = 0;
+  // 0 = Stop / Coast
+  // 1 = Idle
+  // 2 = Idle Near Speaker
+  // 3 = Spin
+  // 4 = SingleSpin
+
   private double smartDashboardShooterSpeed;
 
   /** Creates a new Shooter. */
-  public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) {
+  public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
     leftShooter  = leftTalonFX;
     rightShooter = rightTalonFX;
 
+    limelight = tmplimelight;
+
     leftShooter.setNeutralMode(NeutralModeValue.Coast);
     rightShooter.setNeutralMode(NeutralModeValue.Coast);
     SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
@@ -42,32 +55,43 @@ public class Shooter extends SubsystemBase {
 
   public void singleSpin() {
     leftShooter.set(1.0);
+    spinMode = 4;
   }
 
   public void singleSpin(double speed) {
     leftShooter.set(speed);
+    spinMode = 4;
   }
 
   public void spin() {
     spin(smartDashboardShooterSpeed);
+    spinMode = 3;
   }
 
   public void spin(double speed) {
     leftShooter.set(-speed);    
     rightShooter.set(-speed);
+    spinMode = 3;
   }
 
   public void spin(double leftSpeed, double rightSpeed) {
     leftShooter.set(leftSpeed);    
     rightShooter.set(-rightSpeed);
+    spinMode = 3;
   }
 
   public void stop() {
     spin(0.d);
+    spinMode = 0;
   }
 
   public void idle() {
-      spin(ShooterConstants.SHOOTER_IDLE);
+    spin(ShooterConstants.SHOOTER_IDLE);
+    spinMode = 1;
+  }
+
+  public int getMode(){
+    return spinMode;
   }
 
   @Override
@@ -77,6 +101,14 @@ public class Shooter extends SubsystemBase {
     //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
     smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
 
-    
+    // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
+    // Else if the robot is not near the speaker, then set the speed back to idle.
+    if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){
+      leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);    
+      rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
+      spinMode = 2;
+    }else if(!limelight.isNearSpeaker() && spinMode == 2){
+      idle();
+    }
   }
 }