Fixed shooter + Spindexer is now tracking ball, but it is wrong will fix later

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 1af09981b40be948c4044632a5951979d20da27a..af372ff15274bcba8c5916fa44225cd0f5091516 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -1,121 +1,111 @@
-package frc.robot.subsystems.shooter;
-
-import static edu.wpi.first.units.Units.MetersPerSecond;
+package frc.robot.subsystems.Shooter;
 
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
-import com.ctre.phoenix6.controls.Follower; // Added for following
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
+import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
+import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
+import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 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;
-import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs;
 
 public class Shooter extends SubsystemBase implements ShooterIO {
     
-    private TalonFX motorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
-    private TalonFX motorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_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);
+
+    // Goal Velocity / Double theCircumfrence
+    private double shooterTargetSpeed = 0;
+    private double feederPower = 0;
 
-    public VelocityVoltage flywheelVelocityVoltage = new VelocityVoltage(0).withSlot(0);
-    
-    // Goal Velocity
-    private double targetSpeedMPS = 0;
 
-    private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
+    public boolean shooterAtMaxSpeed = false;
+    public boolean ballDetected = false;
+    //Velocity in rotations per second
+    VelocityVoltage voltageRequest = new VelocityVoltage(0);
 
-    private double torqueCurrentControlTolerance = 5.0;
-    private double torqueCurrentDebounceTime = 0.025;
+    private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
-    private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling);
-    @AutoLogOutput private long launchCount = 0;
-    private boolean lastTorqueCurrentControl = false; 
+    double powerModifier;
 
     public Shooter(){
+
+        updateInputs();
+        
         TalonFXConfiguration config = new TalonFXConfiguration();
-        config.Slot0.kP = 0.15; 
+        config.Slot0.kP = 0.1; //tune p value
         config.Slot0.kI = 0;
-        config.Slot0.kD = 0.03;
-        config.Slot0.kV = 0.20; 
-
-        config.MotionMagic = new MotionMagicConfigs().withMotionMagicCruiseVelocity(90).withMotionMagicAcceleration(130);
-
-        motorLeft.getConfigurator().apply(config);
-        motorRight.getConfigurator().apply(config);
-
-        motorRight.getConfigurator().apply(
-            new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
+        config.Slot0.kD = 0;
+        config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps
+        shooterMotorLeft.getConfigurator().apply(config);
+        shooterMotorRight.getConfigurator().apply(config);
+        
+        shooterMotorLeft.getConfigurator().apply(
+            new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Coast)
         );
 
-        motorLeft.getConfigurator().apply(
+        shooterMotorRight.getConfigurator().apply(
             new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
         );
-
-        motorRight.setControl(new Follower(motorLeft.getDeviceID(), MotorAlignmentValue.Opposed));
-
-        SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY)));
-        SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0)));
     }
 
+    @Override
     public void periodic(){
         updateInputs();
-        Logger.processInputs("Shooter", inputs);
-        countLaunch(Units.rotationsToRadians(targetSpeedMPS));
 
-        double rps = Units.radiansToRotations(targetSpeedMPS / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2.0));
-        motorLeft.setControl(flywheelVelocityVoltage.withVelocity(rps));
+        powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier);
+        SmartDashboard.putNumber("shooter power modifier", powerModifier);
+        shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier));
+        shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier));          
     }
 
-    private void countLaunch(double velocityRadsPerSec) {
-        boolean inTolerance =
-            Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec)
-                <= torqueCurrentControlTolerance;
-        
-        boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance); 
-
-        if (!torqueCurrentControl && lastTorqueCurrentControl) {
-            launchCount++;
-        }
-        lastTorqueCurrentControl = torqueCurrentControl;
+    public void deactivateShooterAndFeeder() {
+        setFeeder(0);
+        setShooter(0);
+    }
 
-        Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec);
+    public void setFeeder(double power){
+        // System.out.println("VELOCITY: " + getShooterVelcoity()); 
+        feederPower = power;
     }
 
-    public void deactivateShooterAndFeeder() {
-        setShooter(0);
+    public void setShooter(double linearVelocityMps) {
+        shooterTargetSpeed = linearVelocityMps;
     }
 
-    public void setShooter(double velocityMPS) {
-        targetSpeedMPS = velocityMPS;
+    /**@return velocity in m/s */
+    public double getShooterVelcoity(){
+        return inputs.shooterSpeedLeft;
     }
 
-    public double getShooterVelcoity() {
-        return inputs.leftShooterVelocity; 
+    @Override
+    public void updateInputs(){
+        inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble());
+        inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble());
+        Logger.processInputs("Shooter", inputs);
     }
 
-    public boolean atTargetVelocity(){
-        return Math.abs(getShooterVelcoity() - targetSpeedMPS) < 0.1; // Tolerance of 0.1 mps
+    public boolean atTargetSpeed(){
+        return Math.abs(getShooterVelcoity() - shooterTargetSpeed) < 1.0;
     }
 
-    public void updateInputs() {
-        inputs.leftShooterVelocity = Units.rotationsToRadians(motorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
-        inputs.rightShooterVelocity =Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
-        inputs.leftShooterCurrent = motorLeft.getStatorCurrent().getValueAsDouble();
-        inputs.rightShooterCurrent = motorRight.getStatorCurrent().getValueAsDouble();
+    @AutoLogOutput(key="Shooter/TargetSpeed")
+    public double getTargetVelocity(){
+        return Units.rotationsToRadians(shooterTargetSpeed);
     }
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
index 00585ff225131c866686db18631edeac0edcb36a..872b1365d565d234b0d72d07c0e5ea46f75dd1d7 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -5,12 +5,8 @@ import org.littletonrobotics.junction.AutoLog;
 public interface ShooterIO {
     @AutoLog
     public static class ShooterIOInputs {
-        public double rightShooterVelocity = 0;
-        public double leftShooterVelocity = 0;
-        public double feederVelocity = 0;
-        public double rightShooterCurrent = 0;
-        public double leftShooterCurrent = 0;
-        public double feederCurrent = 0;
+        public double shooterSpeedLeft = 0.0;
+        public double shooterSpeedRight = 0.0;
     }
 
     public void updateInputs();

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 0ba8e4d756be2e06d924bcb6f56e370117b1b90b..a0106a47c496831ae40d79019f8b6a55842c56e3 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -12,7 +12,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO{
     TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID);
 
     private double power = 0.0;
-
+    public int ballCount = 0;
     private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
 
     public Spindexer(){
@@ -23,8 +23,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO{
     public void periodic() {
         power = SmartDashboard.getNumber("Spindexer Power", power);
         SmartDashboard.putNumber("Spindexer Power", power);
-
         motor.set(power);
+        if (inputs.spindexerVelocity < SpindexerConstants.spindexerVelocityWithBall) {
+            ballCount++;
+        }
     }
 
     public void maxSpindexer(){

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
new file mode 100644 (file)
index 0000000..a48f7c4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
@@ -0,0 +1,5 @@
+package frc.robot.subsystems.spindexer;
+
+public class SpindexerConstants {
+    public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
+}