Added ports, tuned PID/FF, added sensors to kicker

team422 · Oct 9, 2024 · 2c6194e · 2c6194e
1 parent b217a3f
commit 2c6194e
Show file tree

Hide file tree

Showing 16 changed files with 145 additions and 47 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -85,11 +85,11 @@ public static final class AprilTagVisionConstants {
 
   public static final class IntakeConstants {
     public static final LoggedTunableNumber kIdleVoltage =
-        new LoggedTunableNumber("IntakeIdleVoltage", 0.0);
+        new LoggedTunableNumber("Intake Idle Voltage", 0.0);
     public static final LoggedTunableNumber kIntakeVoltage =
-        new LoggedTunableNumber("IntakeVoltage", 7.0);
+        new LoggedTunableNumber("Intake Voltage", -7.0);
     public static final LoggedTunableNumber kOuttakeVoltage =
-        new LoggedTunableNumber("OuttakeVoltage", -7.0);
+        new LoggedTunableNumber("Outtake Voltage", 7.0);
 
     // Simulation constants
     public static final DCMotor kSimGearbox = DCMotor.getNEO(1);
@@ -101,14 +101,16 @@ public static final class IntakeConstants {
 
   public static final class KickerConstants {
     public static final LoggedTunableNumber kIdleVoltage =
-        new LoggedTunableNumber("KickerIdleVoltage", 0.0);
+        new LoggedTunableNumber("Kicker Idle Voltage", 0.0);
+    public static final LoggedTunableNumber kIntakingVoltage =
+        new LoggedTunableNumber("Kicker Intake Voltage", -7.0);
     public static final LoggedTunableNumber kShootingVoltage =
-        new LoggedTunableNumber("KickerShootingVoltage", 7.0);
+        new LoggedTunableNumber("Kicker Shooting Voltage", -7.0);
     public static final LoggedTunableNumber kEjectingVoltage =
-        new LoggedTunableNumber("KickerEjectVoltage", -7.0);
+        new LoggedTunableNumber("Kicker Eject Voltage", 7.0);
 
     public static final LoggedTunableNumber kShootingTimeout =
-        new LoggedTunableNumber("KickerShootingTimeout", 1.0);
+        new LoggedTunableNumber("Kicker Shooting Timeout", 1.0);
 
     // Simulation constants
     public static final DCMotor kSimGearbox = DCMotor.getNEO(1);
@@ -120,10 +122,10 @@ public static final class KickerConstants {
 
   public static final class ShooterConstants {
     public static final LoggedTunableNumber kIdleVoltage =
-        new LoggedTunableNumber("ShooterIdleVoltage", 0.0);
+        new LoggedTunableNumber("Shooter Idle Voltage", 0.0);
     public static final LoggedTunableNumber kEjectingVoltage =
-        new LoggedTunableNumber("ShooterEjectingVoltage", 7.0);
-    public static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel P", 1.0);
+        new LoggedTunableNumber("Shooter Ejecting Voltage", 7.0);
+    public static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel P", 0.03);
     public static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel I", 0.0);
     public static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel D", 0.0);
 
@@ -133,18 +135,18 @@ public static final class ShooterConstants {
         new PIDController(kP.get(), kI.get(), kD.get());
 
     public static final LoggedTunableNumber kTopKS =
-        new LoggedTunableNumber("Top Flywheel kS", 0.0);
+        new LoggedTunableNumber("Top Flywheel kS", 0.12);
     public static final LoggedTunableNumber kTopKV =
-        new LoggedTunableNumber("Top Flywheel kV", 0.0);
+        new LoggedTunableNumber("Top Flywheel kV", 0.125);
     public static final LoggedTunableNumber kBottomKS =
-        new LoggedTunableNumber("Bottom Flywheel kS", 0.0);
+        new LoggedTunableNumber("Bottom Flywheel kS", 0.14);
     public static final LoggedTunableNumber kBottomKV =
-        new LoggedTunableNumber("Bottom Flywheel kV", 0.0);
+        new LoggedTunableNumber("Bottom Flywheel kV", 0.13);
 
     public static final LoggedTunableNumber kAmpTopVelocity =
-        new LoggedTunableNumber("AmpTopVelocity", 5.0);
+        new LoggedTunableNumber("Amp Top Velocity", 5.0);
     public static final LoggedTunableNumber kAmpBottomVelocity =
-        new LoggedTunableNumber("AmpBottomVelocity", 7.0);
+        new LoggedTunableNumber("Amp Bottom Velocity", 7.0);
 
     // Simulation constants
     public static final DCMotor kSimTopGearbox = DCMotor.getNEO(1);
@@ -172,10 +174,17 @@ public static final class Ports {
     public static final int kBackRightTurn = 10;
     public static final int kBackRightCancoder = 11;
 
-    public static final int kIntakeNeo = 1;
-    public static final int kKickerNeo = 2;
-    public static final int kTopFlywheel = 3;
-    public static final int kBottomFlywheel = 4;
+    public static final int kPigeon = 22;
+
+    public static final String kCanivoreName = "Drivetrain";
+
+    public static final int kIntakeNeo = 3;
+    public static final int kKickerNeo = 4;
+    public static final int kTopFlywheel = 5;
+    public static final int kBottomFlywheel = 6;
+
+    public static final int kBeamBreakOne = 8;
+    public static final int kBeamBreakTwo = 9;
   }
 
   public static final class FieldConstants {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -106,6 +106,8 @@ public void autonomousInit() {
     if (autonomousCommand != null) {
       autonomousCommand.schedule();
     }
+
+    RobotState.getInstance().onEnabled();
   }
 
   /** This function is called periodically during autonomous. */
@@ -122,6 +124,8 @@ public void teleopInit() {
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }
+
+    RobotState.getInstance().onEnabled();
   }
 
   /** This function is called periodically during operator control. */
@@ -133,6 +137,8 @@ public void teleopPeriodic() {}
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+
+    RobotState.getInstance().onEnabled();
   }
 
   /** This function is called periodically during test mode. */

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -105,7 +105,8 @@ private void configureSubsystems() {
 
       m_intake = new Intake(new IntakeIONeo(Ports.kIntakeNeo));
 
-      m_kicker = new Kicker(new KickerIONeo(Ports.kKickerNeo));
+      m_kicker =
+          new Kicker(new KickerIONeo(Ports.kKickerNeo, Ports.kBeamBreakOne, Ports.kBeamBreakTwo));
 
       m_shooter =
           new Shooter(
@@ -282,10 +283,15 @@ private void configureButtonBindings() {
 
     m_operatorControls
         .runIntake()
-        .whileTrue(
-            Commands.run(
+        .onTrue(
+            Commands.runOnce(
                 () -> {
                   m_robotState.updateRobotAction(RobotAction.kIntake);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_robotState.setDefaultAction();
                 }));
 
     m_operatorControls
@@ -344,10 +350,6 @@ private void configureButtonBindings() {
                   m_robotState.setDefaultAction();
                 }));
 
-    m_operatorControls
-        .runKicker()
-        .whileTrue(Commands.run(() -> m_kicker.updateState(KickerState.kShooting)));
-
     // manual override to set to idle in case of emergency
     m_operatorControls
         .setIdle()

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -179,7 +179,7 @@ public void updateRobotAction(RobotAction newAction) {
 
       case kIntake:
         m_intake.updateState(IntakeState.kIntaking);
-        m_kicker.updateState(KickerState.kIdle);
+        m_kicker.updateState(KickerState.kIntaking);
         m_shooter.updateState(ShooterState.kIdle);
         m_drive.updateProfile(DriveProfiles.kDefault);
 
@@ -258,4 +258,8 @@ public void setKicker(KickerState state) {
   public void setShooter(ShooterState state) {
     m_shooter.updateState(state);
   }
+
+  public void onEnabled() {
+    m_shooter.resetPID();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -205,6 +205,8 @@ public void periodic() {
       // Apply update
       m_poseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions);
     }
+
+    Logger.recordOutput("Drive/Profile", (DriveProfiles) m_profiles.getCurrentProfile());
   }
 
   public void defaultPeriodic() {

diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -22,12 +22,13 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.Ports;
 import java.util.OptionalDouble;
 import java.util.Queue;
 
 /** IO implementation for Pigeon2 */
 public class GyroIOPigeon2 implements GyroIO {
-  private final Pigeon2 m_pigeon = new Pigeon2(20);
+  private final Pigeon2 m_pigeon = new Pigeon2(Ports.kPigeon, Ports.kCanivoreName);
   private final StatusSignal<Double> m_yaw = m_pigeon.getYaw();
   private final StatusSignal<Double> m_pitch = m_pigeon.getPitch();
   private final StatusSignal<Double> m_roll = m_pigeon.getRoll();

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -67,27 +67,27 @@ public class ModuleIOTalonFX implements ModuleIO {
   public ModuleIOTalonFX(int index) {
     switch (index) {
       case 0:
-        m_driveTalon = new TalonFX(Ports.kFrontLeftDrive);
-        m_turnTalon = new TalonFX(Ports.kFrontLeftTurn);
-        m_cancoder = new CANcoder(Ports.kFrontLeftCancoder);
+        m_driveTalon = new TalonFX(Ports.kFrontLeftDrive, Ports.kCanivoreName);
+        m_turnTalon = new TalonFX(Ports.kFrontLeftTurn, Ports.kCanivoreName);
+        m_cancoder = new CANcoder(Ports.kFrontLeftCancoder, Ports.kCanivoreName);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 1:
-        m_driveTalon = new TalonFX(Ports.kFrontRightDrive);
-        m_turnTalon = new TalonFX(Ports.kFrontRightTurn);
-        m_cancoder = new CANcoder(Ports.kFrontRightCancoder);
+        m_driveTalon = new TalonFX(Ports.kFrontRightDrive, Ports.kCanivoreName);
+        m_turnTalon = new TalonFX(Ports.kFrontRightTurn, Ports.kCanivoreName);
+        m_cancoder = new CANcoder(Ports.kFrontRightCancoder, Ports.kCanivoreName);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 2:
-        m_driveTalon = new TalonFX(Ports.kBackLeftDrive);
-        m_turnTalon = new TalonFX(Ports.kBackLeftTurn);
-        m_cancoder = new CANcoder(Ports.kBackLeftCancoder);
+        m_driveTalon = new TalonFX(Ports.kBackLeftDrive, Ports.kCanivoreName);
+        m_turnTalon = new TalonFX(Ports.kBackLeftTurn, Ports.kCanivoreName);
+        m_cancoder = new CANcoder(Ports.kBackLeftCancoder, Ports.kCanivoreName);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 3:
-        m_driveTalon = new TalonFX(Ports.kBackRightDrive);
-        m_turnTalon = new TalonFX(Ports.kBackRightTurn);
-        m_cancoder = new CANcoder(Ports.kBackRightCancoder);
+        m_driveTalon = new TalonFX(Ports.kBackRightDrive, Ports.kCanivoreName);
+        m_turnTalon = new TalonFX(Ports.kBackRightTurn, Ports.kCanivoreName);
+        m_cancoder = new CANcoder(Ports.kBackRightCancoder, Ports.kCanivoreName);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       default:

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIONeo.java b/src/main/java/frc/robot/subsystems/intake/IntakeIONeo.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems.intake;
 
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
@@ -10,6 +11,7 @@ public class IntakeIONeo implements IntakeIO {
 
   public IntakeIONeo(int port) {
     m_motor = new CANSparkMax(port, MotorType.kBrushless);
+    m_motor.setIdleMode(IdleMode.kCoast);
     m_encoder = m_motor.getEncoder();
   }
 

diff --git a/src/main/java/frc/robot/subsystems/kicker/Kicker.java b/src/main/java/frc/robot/subsystems/kicker/Kicker.java
@@ -16,6 +16,7 @@ public class Kicker extends SubsystemBase {
 
   public enum KickerState {
     kIdle,
+    kIntaking,
     kShooting,
     kVomitting
   }
@@ -26,6 +27,7 @@ public Kicker(KickerIO io) {
 
     HashMap<Enum<?>, Runnable> periodicHash = new HashMap<>();
     periodicHash.put(KickerState.kIdle, () -> {});
+    periodicHash.put(KickerState.kIntaking, this::intakingPeriodic);
     periodicHash.put(KickerState.kShooting, this::shootingPeriodic);
     periodicHash.put(KickerState.kVomitting, () -> {});
     m_profiles = new SubsystemProfiles(KickerState.class, periodicHash, KickerState.kIdle);
@@ -41,6 +43,12 @@ public void periodic() {
     Logger.recordOutput("Kicker/State", (KickerState) m_profiles.getCurrentProfile());
   }
 
+  private void intakingPeriodic() {
+    if (m_io.hasGamePiece()) {
+      updateState(KickerState.kIdle);
+    }
+  }
+
   private void shootingPeriodic() {
     if (m_shootTimeout.hasElapsed(KickerConstants.kShootingTimeout.get())) {
       updateState(KickerState.kIdle);
@@ -52,6 +60,9 @@ public void updateState(KickerState state) {
       case kIdle:
         m_io.setVoltage(KickerConstants.kIdleVoltage.get());
         break;
+      case kIntaking:
+        m_io.setVoltage(KickerConstants.kIntakingVoltage.get());
+        break;
       case kShooting:
         m_io.setVoltage(KickerConstants.kShootingVoltage.get());
         m_shootTimeout.restart();

diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java
@@ -8,9 +8,13 @@ public static class KickerInputs {
     public double angularVelocityRPM;
     public double voltage;
     public double current;
+    public boolean beamBreakOne;
+    public boolean beamBreakTwo;
   }
 
   public void updateInputs(KickerInputs inputs);
 
   public void setVoltage(double voltage);
+
+  public boolean hasGamePiece();
 }
diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIONeo.java b/src/main/java/frc/robot/subsystems/kicker/KickerIONeo.java
@@ -1,27 +1,49 @@
 package frc.robot.subsystems.kicker;
 
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class KickerIONeo implements KickerIO {
   private CANSparkMax m_motor;
   private RelativeEncoder m_encoder;
+  private DigitalInput m_beamBreakOne;
+  private DigitalInput m_beamBreakTwo;
 
-  public KickerIONeo(int port) {
-    m_motor = new CANSparkMax(port, MotorType.kBrushless);
+  public KickerIONeo(int motorPort, int beamBreakOnePort, int beamBreakTwoPort) {
+    m_motor = new CANSparkMax(motorPort, MotorType.kBrushless);
+    m_motor.setIdleMode(IdleMode.kCoast);
     m_encoder = m_motor.getEncoder();
+    m_beamBreakOne = new DigitalInput(beamBreakOnePort);
+    m_beamBreakTwo = new DigitalInput(beamBreakTwoPort);
   }
 
   @Override
   public void updateInputs(KickerInputs inputs) {
     inputs.angularVelocityRPM = m_encoder.getVelocity();
     inputs.voltage = m_motor.getAppliedOutput() * m_motor.getBusVoltage();
     inputs.current = m_motor.getOutputCurrent();
+    inputs.beamBreakOne = beamBreakOneBroken();
+    inputs.beamBreakTwo = beamBreakTwoBroken();
   }
 
   @Override
   public void setVoltage(double voltage) {
     m_motor.setVoltage(voltage);
   }
+
+  @Override
+  public boolean hasGamePiece() {
+    return beamBreakOneBroken() || beamBreakTwoBroken();
+  }
+
+  private boolean beamBreakOneBroken() {
+    return !m_beamBreakOne.get();
+  }
+
+  private boolean beamBreakTwoBroken() {
+    return !m_beamBreakTwo.get();
+  }
 }