n

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9773ebdda2f94e999a2271da0f0a3196a5e9b8ca..54926c3f639abcd9e26c2efe1e0d19485527d88c 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -33,6 +33,7 @@ import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.turret.Turret;
+import frc.robot.subsystems.hood.Hood;
 import frc.robot.util.PathGroupLoader;
 import frc.robot.util.Vision.DetectedObject;
 import frc.robot.util.Vision.Vision;
@@ -52,6 +53,7 @@ public class RobotContainer {
   private Vision vision = null;
   private Turret turret = null;
   private Shooter shooter = null;
+  private Hood hood = null;
   private Climb climb = null;
   private Command auto = new DoNothing();
 
@@ -65,7 +67,7 @@ public class RobotContainer {
    * Different robots may have different subsystems.
    */
   public RobotContainer(RobotId robotId) {
-    climb = new Climb();
+    // climb = new Climb();
     // dispatch on the robot
     switch (robotId) {
       case TestBed1:
@@ -84,12 +86,11 @@ public class RobotContainer {
       case Vivace:
       case Phil:
       case Vertigo:
-        vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
-        turret = new Turret();
+        // vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
         shooter = new Shooter();
-
+        hood = new Hood();
         drive = new Drivetrain(vision, new GyroIOPigeon2());
-        driver = new PS5ControllerDriverConfig(drive, shooter, turret);
+        driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood);
         operator = new Operator(drive);
 
         // Detected objects need access to the drivetrain
@@ -123,7 +124,7 @@ public class RobotContainer {
     LiveWindow.setEnabled(false);
 
     SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis());
-    SmartDashboard.putData("Aim at thingy", new AimAtPose(drive, turret, new Pose2d(FieldConstants.field.getTagPose(26).get().getTranslation().toTranslation2d(), Rotation2d.kZero)));
+    //SmartDashboard.putData("Aim at thingy", new AimAtPose(drive, turret, new Pose2d(FieldConstants.field.getTagPose(26).get().getTranslation().toTranslation2d(), Rotation2d.kZero)));
   }
 
   /**

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index d7a02590dfa4ed954262ab242786c9a44e4dee38..8d50b893239658d95909a31ccefd52e9d89acc93 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -23,8 +23,8 @@ public class IdConstants {
     public static final int TURRET_MOTOR_ID = 20;
 
     // Shooter
-    public static final int SHOOTER_LEFT_ID = 22;
-    public static final int SHOOTER_RIGHT_ID = 23;
+    public static final int SHOOTER_LEFT_ID = 10;
+    public static final int SHOOTER_RIGHT_ID =4;
     public static final int FEEDER_ID = 21;
 
     // Climb
@@ -32,5 +32,5 @@ public class IdConstants {
     public static final int CLIMB_MOTOR_RIGHT = 49;
 
     // Hood
-    public static final int HOOD_ID = 50;
+    public static final int HOOD_ID = 11;
 }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 37cd9d3fb42e5d2bfe6756c2dd7af492810ae91f..3ac13db3d69fe3793fefc143ff715a42ac7c3f3d 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -23,6 +23,8 @@ import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.turret.Turret;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
 import lib.controllers.PS5Controller;
 import lib.controllers.PS5Controller.PS5Axis;
 import lib.controllers.PS5Controller.PS5Button;
@@ -35,16 +37,18 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private final BooleanSupplier slowModeSupplier = ()->false;
     private Shooter shooter;
     private Turret turret;
+    private Hood hood;
 
     private Pose2d alignmentPose = null;
     private Command turretAutoShoot;
     private Command simpleTurretAutoShoot;
     private TurretJoyStickAim turretJoyStickAim;
 
-    public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) {
+    public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood) {
         super(drive);
         this.shooter = shooter;
         this.turret = turret;
+        this.hood = hood;
     }
 
     public void configureControls() { 
@@ -80,32 +84,32 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         //             }));
         //driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0)));
         
-        driver.get(PS5Button.SQUARE).onTrue(
-            new InstantCommand(()->{
-                        if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){
-                            simpleTurretAutoShoot.cancel();
-                        } else{
-                            simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter);
-                            CommandScheduler.getInstance().schedule(simpleTurretAutoShoot);
-                        }
-                    })
-        );
-
-        driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse(
-            new InstantCommand(()->{
-                shooter.setFeeder(0);
-            })
-        );
-
-        driver.get(PS5Button.CIRCLE).onTrue(
-            new InstantCommand(()->{
-                turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0);
-            })
-        ).onFalse(
-            new InstantCommand(()->{
-                turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0);
-            })
-        );
+        // driver.get(PS5Button.SQUARE).onTrue(
+        //     new InstantCommand(()->{
+        //                 if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){
+        //                     simpleTurretAutoShoot.cancel();
+        //                 } else{
+        //                     simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter);
+        //                     CommandScheduler.getInstance().schedule(simpleTurretAutoShoot);
+        //                 }
+        //             })
+        // );
+
+
+        driver.get(PS5Button.SQUARE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
+        driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
+        driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0)));
+
+
+        // driver.get(PS5Button.CIRCLE).onTrue(
+        //     new InstantCommand(()->{
+        //         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0);
+        //     })
+        // ).onFalse(
+        //     new InstantCommand(()->{
+        //         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0);
+        //     })
+        // );
 
         // driver.get(PS5Button.CROSS).onTrue(
         //     new InstantCommand(()->{

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 157379a08af78b1184fb970ec4e623c0e0b8689c..53744dcec1217a09d4af18c85c32b862cdc708aa 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -2,6 +2,7 @@ package frc.robot.subsystems.hood;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -16,12 +17,14 @@ import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
 
 public class Hood extends SubsystemBase implements HoodIO{
-    private TalonFX motor = new TalonFX(IdConstants.HOOD_ID);
+    private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
 
     private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE);
        private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE);
@@ -29,9 +32,9 @@ public class Hood extends SubsystemBase implements HoodIO{
        private double MAX_VEL_RAD_PER_SEC = 25;
        private double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
 
-    private double GEAR_RATIO = HoodConstants.GEAR_RATIO;
+    private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO;
 
-    private static final PIDController positionPID = new PIDController(15, 0, 0.25);
+    private static final PIDController positionPID = new PIDController(6, 0, 0.2);
 
     private final TrapezoidProfile profile = new TrapezoidProfile(
                new TrapezoidProfile.Constraints(
@@ -41,15 +44,16 @@ public class Hood extends SubsystemBase implements HoodIO{
 
        private State setpoint = new State();
 
-       private Rotation2d goalAngle = Rotation2d.kZero;
+       private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
        private double goalVelocityRadPerSec = 0.0;
 
-       private static final double kP = 15.0;
-       private static final double kD = 0.2;
+       private static double kP = 2.0;
+       private static double kD = 0.2;
 
     private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
 
     public Hood(){
+               motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO);
         motor.setNeutralMode(NeutralModeValue.Coast);
 
         motor.getConfigurator().apply(
@@ -62,11 +66,15 @@ public class Hood extends SubsystemBase implements HoodIO{
         motor.getConfigurator().apply(
                                new com.ctre.phoenix6.configs.TalonFXConfiguration() {
                                        {
-                                               MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+                                               MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
                                        }
                                });
 
         setpoint = new State(getPositionRad(), 0.0);
+
+               SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
+               SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0)));
+               SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
     }
 
     public double getPositionRad(){
@@ -80,6 +88,7 @@ public class Hood extends SubsystemBase implements HoodIO{
 
     @Override
     public void periodic() {
+               updateInputs();
         State goalState = new State(
                                MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD),
                                goalVelocityRadPerSec);
@@ -107,7 +116,12 @@ public class Hood extends SubsystemBase implements HoodIO{
 
         Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
                Logger.recordOutput("Hood/velocitySetpoint", targetVelocity / GEAR_RATIO);
-    }
+               Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(setpoint.position) / GEAR_RATIO);
+
+               SmartDashboard.putData("Hood PID", positionPID);
+
+               SmartDashboard.putNumber("Turret Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO);
+       }
 
     @Override
        public void updateInputs() {

diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
index b2d4938e895fa409db2ba363e47008bce2f80b29..07928feb66cacdbd20639135ced70b438152d3ad 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
@@ -1,8 +1,39 @@
 package frc.robot.subsystems.hood;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
 public class HoodConstants {
-    public static final double MIN_ANGLE = 60.0;
-    public static final double MAX_ANGLE = 90.0;
+    public static final double HOOD_GEAR_RATIO = 64;
+
+    public static final double MASS = 2.46; // kilograms
+    public static final double LENGTH = 0.138 * 2; // meters
+    public static final double MOI = 0.0489969498; // kg*m^2 <-- We got this on Onshape
+
+    public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters
+
+    // public static final double MAX_VELOCITY = 5; // rad/s
+    public static final double MAX_VELOCITY = 0.30; // rad/s
+    public static final double MAX_ACCELERATION = 30; // rad/s^2
+
+    public static final double MAX_ANGLE = 82; // degrees
+    public static final double MIN_ANGLE = 58.5; // degrees 
+
+    public static final double START_ANGLE = 90-22.9; // degrees
+
+    // Arena dimensions
+    public static final double TARGET_HEIGHT = 2.44; // meters
+    public static final double SHOOTER_HEIGHT = 0.51; // meters
+
+    public static final Translation2d TRANSLATION_TARGET = new Translation2d(0, 0);
+    public static final Rotation2d ROTATION_TARGET_ANGLE = new Rotation2d();
+    // Other
+    public static final double INITIAL_VELOCTIY = 14.9; // meters per second
+
+    // Testing purposes
+    public static final double START_DISTANCE = 2; // meters
 
-    public static final double GEAR_RATIO = 70.0;
+    // Calibration Purposes
+    public static final double CURRENT_SPIKE_THRESHHOLD = 10.0; // amps
 }

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 9f5e34092db675ddbdedbc40fd3a91ed2e8dd04e..6d7ab098c9152825a48e9a479f21abbb93ac162b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -1,16 +1,20 @@
 package frc.robot.subsystems.shooter;
 
-import java.lang.annotation.Target;
-
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -20,74 +24,116 @@ import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs;
 
 public class Shooter extends SubsystemBase implements ShooterIO {
     
-    private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.RIO_CAN);
-    private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.RIO_CAN);
-
-    private TalonFX feederMotor = new TalonFX(IdConstants.FEEDER_ID, Constants.RIO_CAN);
+    private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
 
     //rotations/sec
 
     // Goal Velocity / Double theCircumfrence
     private double shooterTargetSpeed = 0;
-    private double feederPower = 0;
 
-    public double shooterPower = 1.0;
+    public double shooterPower = 0.5;
     //Velocity in rotations per second
     VelocityVoltage voltageRequest = new VelocityVoltage(0);
 
     private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
+    // for tracking current phase: used to adjust the setting
+    private FlywheelPhase phase;
+
+    private double torqueCurrentControlTolerance = 20.0;
+    private double torqueCurrentDebounceTime = 0.025;
+    private double atGoalStateDebounceTime = 0.2;
+    private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling);
+    private Debouncer atGoalStateDebouncer = new Debouncer(atGoalStateDebounceTime, DebounceType.kFalling);
+    private boolean atGoal = false;
+    @AutoLogOutput private long launchCount = 0;
+    private boolean lastTorqueCurrentControl = false;
+
+    public enum FlywheelPhase {
+        BANG_BANG,
+        CONSTANT_TORQUE,
+        START_UP
+    }    
+
     public Shooter(){
         TalonFXConfiguration config = new TalonFXConfiguration();
-        config.Slot0.kP = 0.2; //tune p value
+        config.Slot0.kP = 676767.0; //tune p value
         config.Slot0.kI = 0;
         config.Slot0.kD = 0;
         config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps
+        
+        config.TorqueCurrent.PeakForwardTorqueCurrent = 40; // this is the constant torque velocity
+        config.TorqueCurrent.PeakReverseTorqueCurrent = 0; // we are making this a BANG BANG controller for talon fx
+        config.MotorOutput.PeakForwardDutyCycle = 1.0;
+        config.MotorOutput.PeakReverseDutyCycle = 0.0;
+
         shooterMotorLeft.getConfigurator().apply(config);
         shooterMotorRight.getConfigurator().apply(config);
         
-        shooterMotorLeft.getConfigurator().apply(
+        shooterMotorRight.getConfigurator().apply(
             new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
             .withNeutralMode(NeutralModeValue.Coast)
         );
 
-        shooterMotorRight.getConfigurator().apply(
+        shooterMotorLeft.getConfigurator().apply(
             new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
         );
 
-        feederMotor.getConfigurator().apply(
-            new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
-            .withNeutralMode(NeutralModeValue.Coast)
-        );
+        // set start up for phase initially:
+        phase = FlywheelPhase.START_UP;
 
         SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY)));
-        SmartDashboard.putData("Turn on feeder", new InstantCommand(() -> setFeeder(ShooterConstants.FEEDER_RUN_POWER)));
         SmartDashboard.putData("Turn ALL off", new InstantCommand(() -> deactivateShooterAndFeeder()));
         SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0)));
-        SmartDashboard.putData("Turn off feeder", new InstantCommand(() -> setFeeder(0)));
     }
 
     public void periodic(){
-        updateInputs();
-        SmartDashboard.putNumber("Shot Power", shooterPower);
-        shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
-
-        shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
-        shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
-        // shooterMotorLeft.set(-shooterPower);
-        // shooterMotorRight.set(-shooterPower);
-        feederMotor.set(feederPower);
+        // updateInputs();
+        // runVelocity(Units.rotationsToRadians(getShooterVelcoity()));
+        // SmartDashboard.putNumber("Shot Power", shooterPower);
+        // shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
+
+        // if (phase == FlywheelPhase.BANG_BANG) { // shooter target speed is in RPS
+        //     // Duty-cycle bang-bang (apply to both)
+        //     shooterMotorLeft.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // spin as fast as possible shooter target speed
+        //     shooterMotorRight.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // same here
+        // } else {
+        //     // Torque-current bang-bang
+        //     shooterMotorLeft.setControl(new VelocityTorqueCurrentFOC(shooterTargetSpeed)); // apply constant torque current
+        //     shooterMotorRight.setControl(new VelocityTorqueCurrentFOC(shooterTargetSpeed)); // again
+        // }
+
+        shooterMotorLeft.set(-shooterPower);
+        shooterMotorRight.set(-shooterPower);
+    }
+
+    /** Run closed loop at the specified velocity. */
+    private void runVelocity(double velocityRadsPerSec) {
+        boolean inTolerance =
+            Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec)
+                <= torqueCurrentControlTolerance;
+        boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance);
+        atGoal = atGoalStateDebouncer.calculate(inTolerance);
+
+        if (!torqueCurrentControl && lastTorqueCurrentControl) {
+        launchCount++;
+        }
+        lastTorqueCurrentControl = torqueCurrentControl;
+
+        phase =
+            torqueCurrentControl
+                ? FlywheelPhase.BANG_BANG
+                : FlywheelPhase.CONSTANT_TORQUE;
+        shooterTargetSpeed = Units.radiansToRotations(velocityRadsPerSec);
+        Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec);
     }
 
+
     public void deactivateShooterAndFeeder() {
-        setFeeder(0);
         setShooter(0);
         System.out.println("Shooter deactivated");
     }
-    public void setFeeder(double power){
-        System.out.println("VELOCITY: " + getShooterVelcoity()); 
-        feederPower = power;
-    }
 
     public void setShooter(double velocityRPS) {
         shooterTargetSpeed = velocityRPS;
@@ -98,10 +144,6 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         this.shooterPower = power;
     }
 
-    public double getFeederVelocity() {
-        return inputs.feederVelocity;
-    }
-
     public double getShooterVelcoity() {
         return inputs.leftShooterVelocity; // assuming they are the same rn
     }
@@ -109,9 +151,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     public void updateInputs() {
         inputs.leftShooterVelocity = shooterMotorLeft.getVelocity().getValueAsDouble();
         inputs.rightShooterVelocity = shooterMotorRight.getVelocity().getValueAsDouble();
-        inputs.feederVelocity = feederMotor.getVelocity().getValueAsDouble();
         inputs.leftShooterCurrent = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
-        inputs.rightShooterVelocity = shooterMotorRight.getStatorCurrent().getValueAsDouble();
-        inputs.feederVelocity = feederMotor.getStatorCurrent().getValueAsDouble();
+        inputs.rightShooterCurrent = shooterMotorRight.getStatorCurrent().getValueAsDouble();
     }
-}
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index ece65435e14f9b3870e5ee3904fac94bbbfb4c93..4a01beb208e16041932aff4d11949d5bf8bcea5b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -1,6 +1,8 @@
 package frc.robot.subsystems.turret;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
 
 public class TurretConstants {
     public static double MAX_ANGLE = 180;
@@ -21,4 +23,7 @@ public class TurretConstants {
 
     public static double LEFT_ENCODER_OFFSET = 0; // degrees
     public static double RIGHT_ENCODER_OFFSET = 0; // degrees
-}
+
+    public static Translation2d DISTANCE_FROM_ROBOT_CENTER = new Translation2d(0,0);
+
+}
\ No newline at end of file