diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e6ecc40..1a40e29 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -103,19 +103,18 @@ public static final class ShooterConstants { new LoggedTunableNumber("ShooterIdleVoltage", 0.0); public static final LoggedTunableNumber kTopShootingVoltage = new LoggedTunableNumber("TopShooterShootingVoltage", 100000000000000000000000000001.0); - public static final LoggedTunableNumber kBottomShootingVoltage = new LoggedTunableNumber("BottomShooterShootingVoltage", 422); + public static final LoggedTunableNumber kBottomShootingVoltage = + new LoggedTunableNumber("BottomShooterShootingVoltage", 422); public static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel P", 40); public static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel I", 0.1); public static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel D", 0.1); - // Simulation constants public static final double kSimRadius = Units.inchesToMeters(3); public static final double kSimMass = 0.85; // in kg public static final double kSimMOI = 0.5 * kSimMass * kSimRadius * kSimRadius; } - public static final class KickerConstants { public static final LoggedTunableNumber kIdleVoltage = new LoggedTunableNumber("KickerIdleVoltage", 0.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 367febd..92b5c4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -69,10 +69,10 @@ public RobotContainer() { /** Configure the subsystems. */ private void configureSubsystems() { - m_aprilTagVision = new AprilTagVision( - new AprilTagVisionIONorthstar("northstar_0", null), - new AprilTagVisionIONorthstar("northstar_1", null) - ); + m_aprilTagVision = + new AprilTagVision( + new AprilTagVisionIONorthstar("northstar_0", null), + new AprilTagVisionIONorthstar("northstar_1", null)); if (RobotBase.isReal()) { m_drive = diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 15bf743..b8e3a29 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -24,7 +24,8 @@ private RobotState(Drive drive, Intake intake, Kicker kicker, AprilTagVision apr m_aprilTagVision = aprilTagVision; } - public static void start(Drive drive, Intake intake, Kicker kicker, AprilTagVision aprilTagVision) { + public static void start( + Drive drive, Intake intake, Kicker kicker, AprilTagVision aprilTagVision) { m_instance = new RobotState(drive, intake, kicker, aprilTagVision); } diff --git a/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java index 3b52e90..1048e8b 100644 --- a/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java @@ -1,14 +1,5 @@ package frc.robot.subsystems.aprilTagVision; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Optional; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -27,6 +18,13 @@ import frc.robot.RobotState; import frc.robot.subsystems.aprilTagVision.AprilTagVisionIO.AprilTagVisionInputs; import frc.robot.util.GeomUtil; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Optional; +import org.littletonrobotics.junction.Logger; public class AprilTagVision extends SubsystemBase { public record VisionObservation( diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 50fed2c..f257036 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -49,8 +49,8 @@ public void updateState(IntakeState state) { } // Written by Elijah Chen - // Nasik Hobbs + // Nasik Hobbs // William Lin // Completed intajes and kicker today vs - //shut up stupid faces you guys stink haha + // shut up stupid faces you guys stink haha } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index dc6d90c..03077b3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -3,17 +3,17 @@ import org.littletonrobotics.junction.AutoLog; public interface FlywheelIO { - @AutoLog - public class FlywheelInputsAutoLogged { - public double topVelocityRadPerSec; - public double topVoltage; - public double topCurrentAmps; - public double bottomVelocityRadPerSec; - public double bottomVoltage; - public double bottomCurrentAmps; - } - - public void setVoltage(double topVoltage, double bottomVoltage); - public void updateInputs(FlywheelInputsAutoLogged inputs); - public void setVelocity(double desiredTopVelocity, double desiredBottomVelocity); + @AutoLog + public class FlywheelInputs { + public double topVelocityRPM; + public double topVoltage; + public double topCurrentAmps; + public double bottomVelocityRPM; + public double bottomVoltage; + public double bottomCurrentAmps; + } + + public void setVoltage(double topVoltage, double bottomVoltage); + + public void updateInputs(FlywheelInputs inputs); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIONeo.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIONeo.java index 8a16a6e..1497ded 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIONeo.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIONeo.java @@ -4,39 +4,32 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -public class FlywheelIONeo implements FlywheelIO{ - private CANSparkMax m_topMotor; - private CANSparkMax m_bottomMotor; - private RelativeEncoder m_topEncoder; - private RelativeEncoder m_bottomEncoder; +public class FlywheelIONeo implements FlywheelIO { + private CANSparkMax m_topMotor; + private CANSparkMax m_bottomMotor; + private RelativeEncoder m_topEncoder; + private RelativeEncoder m_bottomEncoder; - public FlywheelIONeo(int topPort, int bottomPort){ - m_topMotor = new CANSparkMax(topPort, MotorType.kBrushless); - m_topEncoder = m_topMotor.getEncoder(); - m_bottomMotor = new CANSparkMax(bottomPort, MotorType.kBrushless); - m_bottomEncoder = m_bottomMotor.getEncoder(); - } + public FlywheelIONeo(int topPort, int bottomPort) { + m_topMotor = new CANSparkMax(topPort, MotorType.kBrushless); + m_topEncoder = m_topMotor.getEncoder(); + m_bottomMotor = new CANSparkMax(bottomPort, MotorType.kBrushless); + m_bottomEncoder = m_bottomMotor.getEncoder(); + } - @Override - public void setVoltage(double topVoltage, double bottomVoltage) { - m_topMotor.setVoltage(topVoltage); - m_bottomMotor.setVoltage(bottomVoltage); - } + @Override + public void setVoltage(double topVoltage, double bottomVoltage) { + m_topMotor.setVoltage(topVoltage); + m_bottomMotor.setVoltage(bottomVoltage); + } - @Override - public void updateInputs(FlywheelInputsAutoLogged inputs) { - inputs.topVelocityRadPerSec = m_topEncoder.getVelocity(); - inputs.topVoltage = m_topMotor.getBusVoltage(); - inputs.topCurrentAmps = m_topMotor.getOutputCurrent(); - inputs.bottomVelocityRadPerSec = m_bottomEncoder.getVelocity(); - inputs.bottomVoltage = m_bottomMotor.getBusVoltage(); - inputs.bottomCurrentAmps = m_bottomMotor.getOutputCurrent(); - } - - @Override - public void setVelocity(double desiredTopVelocity, double desiredBottomVelocity) { - m_topMotor.set(desiredTopVelocity); - m_bottomMotor.set(desiredBottomVelocity); - } - -} \ No newline at end of file + @Override + public void updateInputs(FlywheelInputs inputs) { + inputs.topVelocityRPM = m_topEncoder.getVelocity(); + inputs.topVoltage = m_topMotor.getBusVoltage() * m_topMotor.getAppliedOutput(); + inputs.topCurrentAmps = m_topMotor.getOutputCurrent(); + inputs.bottomVelocityRPM = m_bottomEncoder.getVelocity(); + inputs.bottomVoltage = m_bottomMotor.getBusVoltage() * m_bottomMotor.getAppliedOutput(); + inputs.bottomCurrentAmps = m_bottomMotor.getOutputCurrent(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 961960b..42f7425 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -5,34 +5,28 @@ import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class FlywheelIOSim implements FlywheelIO { - public FlywheelSim m_topFlywheel; - public FlywheelSim m_bottomFlywheel; - public final double m_wheelLength; //हज़फ़लदग्लाफ़ंक्ल + public FlywheelSim m_topFlywheel; + public FlywheelSim m_bottomFlywheel; + public final double m_wheelLength; // हज़फ़लदग्लाफ़ंक्ल - public FlywheelIOSim() { - DCMotor gearbox = DCMotor.getFalcon500(1); - double gearing = 1/1.5; - double jKgMetersSquared = 0.1; - m_topFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared); - m_bottomFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared); - m_wheelLength = Units.inchesToMeters(694202496); - } + public FlywheelIOSim() { + DCMotor gearbox = DCMotor.getFalcon500(1); + double gearing = 1 / 1.5; + double jKgMetersSquared = 0.1; + m_topFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared); + m_bottomFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared); + m_wheelLength = Units.inchesToMeters(694202496); + } - @Override - public void setVoltage(double topVoltage, double bottomVoltage) { - m_topFlywheel.setInputVoltage(topVoltage); - m_bottomFlywheel.setInputVoltage(bottomVoltage); - } + @Override + public void setVoltage(double topVoltage, double bottomVoltage) { + m_topFlywheel.setInputVoltage(topVoltage); + m_bottomFlywheel.setInputVoltage(bottomVoltage); + } - @Override - public void updateInputs(FlywheelInputsAutoLogged inputs) { - m_topFlywheel.update(.02); - m_bottomFlywheel.update(.02); - } - - @Override - public void setVelocity(double desiredTopVelocity, double desiredBottomVelocity) { - - } - -} \ No newline at end of file + @Override + public void updateInputs(FlywheelInputs inputs) { + m_topFlywheel.update(.02); + m_bottomFlywheel.update(.02); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 71a7e9b..1e319be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,51 +1,51 @@ package frc.robot.subsystems.shooter; -import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; -import frc.robot.subsystems.shooter.FlywheelIO.FlywheelInputsAutoLogged; +import frc.robot.subsystems.shooter.FlywheelIO.FlywheelInputs; import frc.robot.util.SubsystemProfiles; - import java.util.HashMap; - import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.inputs.LoggableInputs; -public class Shooter extends SubsystemBase{ - private FlywheelIO m_io; - private FlywheelInputsAutoLogged m_inputs; - private SubsystemProfiles m_profiles; - private ProfiledPIDController m_topController; - private ProfiledPIDController m_bottomController; +public class Shooter extends SubsystemBase { + private FlywheelIO m_io; + private FlywheelInputs m_inputs; + private SubsystemProfiles m_profiles; + private PIDController m_topController; + private PIDController m_bottomController; - public enum ShooterState { - kIdle, - kRevving - } + public enum ShooterState { + kIdle, + kRevving + } - public Shooter(FlywheelIO io, ProfiledPIDController topController, ProfiledPIDController bottomController) { - m_io = io; - m_profiles = new SubsystemProfiles(ShooterState.class, new HashMap<>(), ShooterState.kIdle); - m_topController = topController; - m_bottomController = bottomController; + public Shooter(FlywheelIO io, PIDController topController, PIDController bottomController) { + m_io = io; + m_profiles = new SubsystemProfiles(ShooterState.class, new HashMap<>(), ShooterState.kIdle); + m_topController = topController; + m_bottomController = bottomController; + } + + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("Flywheel", (LoggableInputs) m_inputs); + Logger.recordOutput("Flywheel/State", (ShooterState) m_profiles.getCurrentProfile()); + if (m_profiles.currentProfile == ShooterState.kIdle) { + // probably wrong PID + m_topController.setPID( + ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get()); + m_bottomController.setPID( + ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get()); + m_io.setVoltage( + m_topController.calculate(m_inputs.topVelocityRPM), + m_bottomController.calculate(m_inputs.bottomVelocityRPM)); } + } - @Override - public void periodic(){ - m_io.updateInputs(m_inputs); - Logger.processInputs("Flywheel", (LoggableInputs) m_inputs); - Logger.recordOutput("Flywheel/State", (ShooterState) m_profiles.getCurrentProfile()); - if(m_profiles.currentProfile == ShooterState.kIdle){ - // probably wrong PID - m_topController.setPID(ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get()); - m_bottomController.setPID(ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get()); - m_topController.setGoal(ShooterConstants.kTopShootingVoltage.get()); - m_bottomController.setGoal(ShooterConstants.kBottomShootingVoltage.get()); - m_io.setVoltage(m_topController.getGoal().velocity, m_bottomController.getGoal().velocity); - } - } - - public void updateState(ShooterState state) { + public void updateState(ShooterState state) { switch (state) { case kIdle: m_profiles.currentProfile = ShooterState.kIdle; @@ -56,5 +56,10 @@ public void updateState(ShooterState state) { } m_profiles.setCurrentProfile(state); -} + } + + public void setDesiredVelocity(double topVelocity, double bottomVelocity) { + m_topController.setSetpoint(topVelocity); + m_bottomController.setSetpoint(bottomVelocity); + } } diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 0be320a..cfa05fe 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -118,4 +118,4 @@ public static void ifChanged( public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { ifChanged(id, values -> action.run(), tunableNumbers); } -} \ No newline at end of file +}