still in progress

CyberCoyotes · Mar 18, 2024 · 1020eaf · 1020eaf
1 parent d9ff210
commit 1020eaf
Show file tree

Hide file tree

Showing 5 changed files with 65 additions and 20 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -28,13 +28,16 @@
 import frc.robot.commands.SetIndex;
 import frc.robot.commands.SetWinch;
 import frc.robot.commands.ShootClose;
+import frc.robot.experimental.SetShooterVelocity;
+import frc.robot.experimental.ShootWhenReady;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.ShooterSubsystemVelocity;
 import frc.robot.subsystems.WinchSubsystem2;
+import frc.robot.subsystems.ShooterSubsystem2;
 
 // Getting rid of the soft unused warnings
 @SuppressWarnings("unused")
@@ -56,6 +59,7 @@ public class RobotContainer {
   IndexSubsystem index = new IndexSubsystem();
   WinchSubsystem2 winch = new WinchSubsystem2();
   ArmSubsystem arm = new ArmSubsystem();
+  ShooterSubsystem2 shooter2 = new ShooterSubsystem2();
   // #endregion Subsystems
 
   // #region commands
@@ -86,6 +90,9 @@ public class RobotContainer {
 
   private final IntakeCommandGroup intakeGroup = new IntakeCommandGroup(index, intake);
   private final IntakeRevCommandGroup intakeRevGroup = new IntakeRevCommandGroup(index, intake);
+  private final SetShooterVelocity setShooterVelocity = new SetShooterVelocity(shooter2, 10);
+  private final ShootWhenReady shootWhenReady = new ShootWhenReady(shooter2, index);
+
   // ChargeIntakeCommand chargeIntake = new ChargeIntakeCommand(drivetrain, intake, driveRequest);
 
   /* Autonomous Chooser */
@@ -111,7 +118,8 @@ public RobotContainer() {
     SmartDashboard.putData("Auto Chooser", autoChooser);
     // Shuffleboard.getTab("Auton").add("Auto Chooser", autoChooser);
 
-    shooter = new ShooterSubsystemVelocity();
+    // TODO removed
+    // shooter = new ShooterSubsystemVelocity();
 
     /* Configure the Button Bindings */
     configureBindings();
@@ -167,8 +175,16 @@ private void configureBindings() {
 
     m_driverController.rightBumper().whileTrue(new IntakeCommandGroup(index, intake));
     m_driverController.leftBumper().whileTrue(new IntakeRevCommandGroup(index, intake));
-    m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter));
-    m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0)));
+
+    /* Testing */
+    // m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter));
+    // m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0)));
+
+    // TODO Test
+//    m_driverController.rightTrigger().whileTrue(setShooterVelocity); // (shootWhenReady);
+    m_driverController.rightTrigger().whileTrue(shootWhenReady); // (shootWhenReady);
+
+
     m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75));
 
     // m_operatorController.a().whileTrue (new));

diff --git a/...va/frc/robot/experimental/SetShooter.java → ...obot/experimental/SetShooterVelocity.java b/...va/frc/robot/experimental/SetShooter.java → ...obot/experimental/SetShooterVelocity.java
@@ -1,20 +1,20 @@
 package frc.robot.experimental;
 
 import edu.wpi.first.wpilibj2.command.Command;
-// import frc.robot.subsystems.ShooterSubsystemVelocity;
-import frc.robot.experimental.ShooterSubsystem2;
 import frc.robot.subsystems.IndexSubsystem;
+import frc.robot.subsystems.ShooterSubsystem2;
 
-public class SetShooter extends Command {
+public class SetShooterVelocity extends Command {
     // private ShooterSubsystemVelocity shooter;
     private final ShooterSubsystem2 shooter;
     private final double velocity;
 
-    public SetShooter(ShooterSubsystem2 shooter, double velocity) {
+    public SetShooterVelocity(ShooterSubsystem2 shooter, double velocity) {
         this.shooter = shooter;
         this.velocity = velocity;
 
-        addRequirements(shooter); 
+        addRequirements(shooter);
+
     }
 
     @Override

diff --git a/src/main/java/frc/robot/experimental/ShootClose2.java b/src/main/java/frc/robot/experimental/ShootClose2.java
@@ -8,7 +8,7 @@
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.experimental.ShooterSubsystem2;
+import frc.robot.subsystems.ShooterSubsystem2;
 
 
 public class ShootClose2 extends ParallelCommandGroup{
@@ -19,7 +19,7 @@ public ShootClose2(ArmSubsystem arm, IndexSubsystem index, IntakeSubsystem intak
             // new SetArmPosition(arm, Constants.ArmConstants.ARM_HOME_POSE),
             // new ShooterIndex(shooter, index).withTimeout(1.0)
 
-            new SetShooter(shooter, 60),
+            new SetShooterVelocity(shooter, 60),
             new ShootWhenReady(shooter, index)
 
         );

diff --git a/src/main/java/frc/robot/experimental/ShootWhenReady.java b/src/main/java/frc/robot/experimental/ShootWhenReady.java
@@ -1,9 +1,8 @@
 package frc.robot.experimental;
 
 import edu.wpi.first.wpilibj2.command.Command;
-// import frc.robot.subsystems.ShooterSubsystemVelocity;
-import frc.robot.experimental.ShooterSubsystem2;
 import frc.robot.subsystems.IndexSubsystem;
+import frc.robot.subsystems.ShooterSubsystem2;
 
 public class ShootWhenReady extends Command {
     // private ShooterSubsystemVelocity shooter;
@@ -14,6 +13,7 @@ public class ShootWhenReady extends Command {
     public ShootWhenReady(ShooterSubsystem2 shooter, IndexSubsystem index) {
         this.shooter = shooter;
         this.index = index;
+
         addRequirements(shooter); 
     }
 

diff --git a/...robot/experimental/ShooterSubsystem2.java → ...c/robot/subsystems/ShooterSubsystem2.java b/...robot/experimental/ShooterSubsystem2.java → ...c/robot/subsystems/ShooterSubsystem2.java
@@ -8,10 +8,11 @@
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.Constants.CANIDs;
 
-public class ShooterSubsystem2 {
+public class ShooterSubsystem2 extends SubsystemBase{
 
     // Declare variables for the motors to be controlled
     private TalonFX m_primaryMotor; // Right
@@ -21,7 +22,9 @@ public class ShooterSubsystem2 {
     private VelocityVoltage m_primaryVelocity = new VelocityVoltage(0);
     private VelocityVoltage m_secondaryVelocity = new VelocityVoltage(0);
 
-    int shooterVelocity = 60;
+    private double shooterVelocity = 10;
+    private boolean shooterlDisabled = false;
+    private double targetVelocity;
 
     public ShooterSubsystem2() {
         /* Verbose? Absolutely. Effective? I hope so */
@@ -31,8 +34,8 @@ public ShooterSubsystem2() {
         m_primaryMotor.getConfigurator().apply(new TalonFXConfiguration());
         m_secondaryMotor.getConfigurator().apply(new TalonFXConfiguration());
 
-        m_primaryMotor.setInverted(true);
-        m_secondaryMotor.setInverted(false);
+        m_primaryMotor.setInverted(false);
+        m_secondaryMotor.setInverted(false);       
 
         /* Various Configs */
         var shooterConfigs0 = new Slot0Configs();        
@@ -42,6 +45,7 @@ public ShooterSubsystem2() {
                 .withKS(0.0001) //
                 .withKV(0.12);  //
 
+
         var shooterVelocityConfig = new VoltageConfigs();
             shooterVelocityConfig
                 .withPeakForwardVoltage(8)  // FRC 2910 running 12
@@ -66,12 +70,37 @@ public ShooterSubsystem2() {
         m_primaryMotor.getConfigurator().apply(shooterMotorOutputConfigs);
         m_secondaryMotor.getConfigurator().apply(shooterMotorOutputConfigs);
 
+
     } // end of constructor
 
     /* Sets the velocity the motors are using */
-    public void setShooterVelocity(int shooterVelocity) {
-        m_primaryMotor.setControl(m_primaryVelocity.withVelocity(shooterVelocity));
-        m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(shooterVelocity * 0.95));
+    public void setShooterVelocity(double shooterVelocity) {
+        this.shooterVelocity = shooterVelocity;
+        // m_primaryMotor.setControl(m_primaryVelocity.withVelocity(shooterVelocity));
+        // m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(shooterVelocity * 0.95));
+    }
+    /*
+    public void setTargetFlywheelSpeed(double targetFlywheelSpeed) {
+        this.targetFlywheelSpeed = targetFlywheelSpeed + Units.rotationsPerMinuteToRadiansPerSecond(shootingOffset);
+    }
+     */
+
+    /* Returns the velocity of the flywheel */
+    public double getShooterVelocity() {
+        return shooterVelocity;
+    }
+
+    /* Returns the velocity of the flywheel 
+    public double getShooterVelocity() {
+        if (Robot.isSimulation()) {
+            return flywheel.getAngularVelocityRadPerSec();
+        } else {
+            return flywheelPrimaryMotor.getSelectedSensorVelocity() * FLYWHEEL_SENSOR_VELOCITY_COEFFICIENT;
+        }
+    }*/
+
+    public boolean isShooterAtTargetVelocity() {
+        return Math.abs(getShooterVelocity()) >= shooterVelocity;
     }
 
 } // end of class ShooterSubsystem2