Merge branch 'Subsystem-Boilerplate' into Intake-boilerplate

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

@@ -0,0 +1,245 @@
+package frc4388.robot.subsystems;
+
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.motorcontrol.Talon;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.configurable.ConfigurableDouble;
+
+public class TestRobot extends SubsystemBase {
+
+    // TalonFX m_intakeMotor;
+    // TalonFX m_armMotor;
+    // TalonFX m_storageMotor;
+    TalonFX m_outerShooter;
+    TalonFX m_innerShooter;
+
+    ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0);
+
+
+    ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0);
+    ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0);
+
+    ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0);
+    ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0);
+    ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0);
+
+    VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0);
+    VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0);
+
+    public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second
+
+    public enum RobotStare {
+        IntakeDown,
+        Idle,
+        Shoot
+    }
+
+    public RobotStare robotStare = RobotStare.Idle;
+
+
+    public TestRobot(
+        // TalonFX intakeMotor,
+        // TalonFX armMotor,
+        // TalonFX storageMotor,
+        TalonFX outerShooter,
+        TalonFX innerShooter
+    ) {
+        // m_intakeMotor = intakeMotor;
+        // m_armMotor = armMotor;
+        // m_storageMotor = storageMotor;
+        m_outerShooter = outerShooter;
+        m_innerShooter = innerShooter;
+
+        // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG);
+        // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG);
+        // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG);
+        m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG);
+        m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
+
+        m_outerShooter.getConfigurator().apply(SHOOTER_PID);
+        m_innerShooter.getConfigurator().apply(SHOOTER_PID);
+
+        outerVelocity.Slot = 0;
+        innerVelocity.Slot = 0;
+    }
+
+    // public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration()
+    //     .withCurrentLimits(
+    //         new CurrentLimitsConfigs()
+    //             .withStatorCurrentLimit(40) // TODO: tune???
+    //             .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
+    //         ).withMotorOutput(
+    //             new MotorOutputConfigs()
+    //                 .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
+    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    // );
+    // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
+    //     .withCurrentLimits(
+    //         new CurrentLimitsConfigs()
+    //             .withStatorCurrentLimit(40) // TODO: tune???
+    //             .withStatorCurrentLimitEnable(true)
+    //         ).withMotorOutput(
+    //             new MotorOutputConfigs()
+    //                 .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
+    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    // );
+
+    // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
+    //     .withCurrentLimits(
+    //         new CurrentLimitsConfigs()
+    //             .withStatorCurrentLimit(40) // TODO: tune???
+    //             .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
+    //         ).withMotorOutput(
+    //             new MotorOutputConfigs()
+    //                 .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
+    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    // );
+    public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(40) // TODO: tune???
+                .withStatorCurrentLimitEnable(true)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
+                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    );
+    public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(40) // TODO: tune???
+                .withStatorCurrentLimitEnable(true)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
+                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+                    
+    );
+
+    public static Slot0Configs SHOOTER_PID;
+
+    static {
+        SHOOTER_PID = new Slot0Configs();
+        SHOOTER_PID.kV = 0.12;
+        // SHOOTER_PID.kP = 0.11;
+        // SHOOTER_PID.kI = 0.48;
+        // SHOOTER_PID.kD = 0.01;
+        SHOOTER_PID.kP = 0.3;
+        SHOOTER_PID.kI = 0.0;
+        SHOOTER_PID.kD = 0.0;
+    }
+
+    
+    ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12);
+    ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11);
+    ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48);
+    ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01);
+
+    @Override
+    public void periodic() {
+        
+        SHOOTER_PID = new Slot0Configs();
+        SHOOTER_PID.kV = kV.get();
+        SHOOTER_PID.kP = kP.get();
+        SHOOTER_PID.kI = kI.get();
+        SHOOTER_PID.kD = kD.get();
+
+        m_outerShooter.getConfigurator().apply(SHOOTER_PID);
+        m_innerShooter.getConfigurator().apply(SHOOTER_PID);
+
+        
+        SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond));
+        SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond));
+
+
+        switch (robotStare) {
+            case Idle:
+            
+                // Move the arm to the up position
+                PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get());
+                // m_armMotor.setControl(armPosition);
+            
+
+                // // Stop the intake motor
+                // m_intakeMotor.set(0);
+
+                // // Stop the storage motor
+                // m_storageMotor.set(0);
+
+                
+                // Stop the outer shooter motor
+                m_outerShooter.set(0);
+                // Stop the inner shooter motor
+                m_innerShooter.set(0);
+
+                break;
+            case IntakeDown:
+                // Move the arm to the down sposition
+                PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get());
+                // m_armMotor.setControl(armPosition1);
+
+                // Move balls into the robot because the arm is down
+                VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get());
+                // m_intakeMotor.setControl(intakeVelocity);
+
+                // Move balls into the main box
+                VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get());
+                // m_storageMotor.setControl(storageVelocity);
+
+                
+                // Stop the outer shooter motor
+                m_outerShooter.set(0);
+                // Stop the inner shooter motor
+                m_innerShooter.set(0);
+
+                break;
+            case Shoot:
+
+                // Move the arm to the up position
+                PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get());
+                // m_armMotor.setControl(armPosition2);
+
+                // // Stop the intake motor
+                // m_intakeMotor.set(0);
+
+                // // Move the balls into the 
+                VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get());
+                // m_storageMotor.setControl(storageVelocity1);
+                        
+                // outerVelocity.
+                // m_outerShooter.setControl(outerVelocity);
+                double outerRPM = outerRate.get();
+                m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
+
+
+                
+                // m_innerShooter.setControl(innerVelocity);
+                // m_innerShooter.set(innerRate.get());
+
+                double innerRPM = innerRate.get();
+                m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
+
+                
+                // SmartDashboard.putNumber("Test", outerRate.get());
+
+                break;
+            default:
+                break;
+        }
+
+
+        
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

@@ -85,4 +85,4 @@ public class IntakeReal implements IntakeIO {
         state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
         state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
     }
-}
+}

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -1,9 +1,10 @@
 package frc4388.robot.subsystems.shooter;
 
-import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.Degrees;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
@@ -12,10 +13,17 @@ import frc4388.utility.status.CanDevice;
 
 public class ShooterConstants {
     // Motor conversions
-    // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.;
+    
-    public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
-    public static final double FLYWHEEL_GEAR_RATIO = 1.;
     public static final double FEEDER_INCHES_PER_ROT = 1.;
+    public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.;
+    public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.;
+    public static final double INDEXER_GEAR_RATIO = 1.;
+
+    public static Slot0Configs SHOOTER_PID = new Slot0Configs()
+        .withKV(0.0)
+        .withKP(0.0)
+        .withKI(0.0)
+        .withKD(0.0);
 
     // Limits
 
@@ -44,7 +52,7 @@ public class ShooterConstants {
         public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
     }
 
-    public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration()
+    public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
         .withCurrentLimits(
             new CurrentLimitsConfigs()
                 .withStatorCurrentLimit(40) // TODO: tune???
@@ -54,7 +62,7 @@ public class ShooterConstants {
                     .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
                     .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
     );
-    public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration()
+    public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
         .withCurrentLimits(
             new CurrentLimitsConfigs()
                 .withStatorCurrentLimit(40) // TODO: tune???
@@ -64,7 +72,7 @@ public class ShooterConstants {
                     .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
                     .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
     );
-    public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration()
+    public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
         .withCurrentLimits(
             new CurrentLimitsConfigs()
                 .withStatorCurrentLimit(40) // TODO: tune???

src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java

@@ -24,19 +24,26 @@ public interface ShooterIO {
         // Angle shooterTargetPitch = Rotations.of(0);
         // Current pitchMotorCurrent = Amps.of(0);
 
-        AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
+        AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0);
-        AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
+        AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0);
-        Current flywheelMotorCurrent = Amps.of(0);
+        AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0);
 
-        LinearVelocity feederVelocity = InchesPerSecond.of(0);
+        AngularVelocity motor1Velocity = RotationsPerSecond.of(0);
-        LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
+        AngularVelocity motor2Velocity = RotationsPerSecond.of(0);
-        Current feederMotorCurrent = Amps.of(0);
+        AngularVelocity indexerVelocity = RotationsPerSecond.of(0);
+        
+        Current motor1Current = Amps.of(0);
+        Current motor2Current = Amps.of(0);
+        Current indexerCurrent = Amps.of(0);
+
+        LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
     }
 
     // public default void setShooterAngle(ShooterState state, Angle angle) {}
     // public default void setShooterPitch(ShooterState state, Angle angle) {}
-    public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
+    public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
-    public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
+    public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
+    public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
 
     public default void updateInputs(ShooterState state) {}
 }

src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java

@@ -1,38 +1,43 @@
 package frc4388.robot.subsystems.shooter;
 
 import static edu.wpi.first.units.Units.InchesPerSecond;
-import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
-import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import edu.wpi.first.units.measure.*;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.configurable.ConfigurableDouble;
 
 public class ShooterReal implements ShooterIO {
 
-    // TalonFX m_angleMotor;
+    TalonFX m_shooter1Motor;
-    // TalonFX m_pitchMotor;
+    TalonFX m_shooter2Motor;
-    TalonFX m_flywheelMotor;
+    TalonFX m_indexerMotor;
-    TalonFX m_feederMotor;
+    VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
+    VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
+    VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
+
 
     public ShooterReal(
-        // TalonFX angleMotor,
+         TalonFX shooter1Motor,
-        // TalonFX pitchMotor,
+         TalonFX shooter2Motor,
-        TalonFX flywheelMotor,
+         TalonFX indexerMotor
-        TalonFX feederMotor
     ) {
-        // m_angleMotor = angleMotor;
+        m_shooter1Motor= shooter1Motor;
-        // m_pitchMotor = pitchMotor;
+        m_shooter2Motor= shooter2Motor;
-        m_flywheelMotor = flywheelMotor;
+        m_indexerMotor = indexerMotor;
-        m_feederMotor = feederMotor;
+        m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
+        m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
+        m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
 
-        // Apply the configs
+        
-        // m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
+        m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
-        // m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
+        m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
-        m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
+        m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
-        m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
     }
 
     private Angle clampAng(Angle x, Angle min, Angle max){
@@ -45,6 +50,10 @@ public class ShooterReal implements ShooterIO {
         }
     }
 
+
+
+   
+
     // // TODO: Test
     // @Override
     // public void setShooterAngle(ShooterState state, Angle angle) {
@@ -75,37 +84,36 @@ public class ShooterReal implements ShooterIO {
     //     m_pitchMotor.setControl(posRequest);
     // }
     
+
     @Override
-    public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
+    public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
-        state.flywheelTargetVelocity = angularVelocity;
+        state.motor1TargetVelocity = target;
-        // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
+        state.motor2TargetVelocity = target;
-        double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO;
+
-        VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
+        double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
-        m_feederMotor.setControl(velocity);
+        m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps));
+        m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps));
     }
 
     @Override
-    public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {
+    public void setIndexerVelocity(ShooterState state, AngularVelocity target) {
-        state.feederTargetVelocity = linearVelocity;
+        state.indexerTargetVelocity = target;
-        // (IN / SEC) * (ROT / IN) = (ROT / SEC)
+        double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
-        double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT;
+        m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
-        VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed);
-        m_feederMotor.setControl(velRequest);
     }
 
     @Override
     public void updateInputs(ShooterState state) {
-        // state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
-        // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
 
-        // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
+        state.motor1Velocity = m_shooter1Motor.getVelocity().getValue();
-        // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
+        state.motor2Velocity = m_shooter2Motor.getVelocity().getValue();
+        state.indexerVelocity = m_indexerMotor.getVelocity().getValue();
 
-        state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
+        state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
-        state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
+        
-
+        state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
-        state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
+        state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue();
-        state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue();
+        state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue();
     }
     
 }