diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8cfef36..16a2bd6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,13 +8,6 @@ package frc.robot; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.revrobotics.spark.FeedbackSensor; - -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.RobotBase; /** @@ -40,147 +33,5 @@ public static enum Mode { REPLAY } - public static class ShooterConstants { - - public enum ShooterState { - SHOOTER, - REVERSE, - IDLE - } - - public static final boolean attached = true; - public static final int id = 50; - public static final boolean inverted = false; - public static final boolean breakType = false; - public static final double gearRatio = 1 / 1; - - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; - // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might Add later - - public static final Current stallLimit = Units.Amps.of(60); - public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 1; - public static final double maxReverseOutput = 0.5; - - public static final AngularVelocity shooterSpeed = Units.RPM.of(4000); - public static final AngularVelocity reverseSpeed = Units.RPM.of(-1000); - public static final AngularVelocity idleSpeed = Units.RPM.of(0); - - public enum ShooterModes { - SHOOTER(shooterSpeed, 1.0), - REVERSE(reverseSpeed, -0.5), - IDLE(idleSpeed, 0.0); - - public AngularVelocity speed; - public double output; - - ShooterModes(AngularVelocity speed, double output) { - this.speed = speed; - this.output = output; - } - } - } - - public static class IndexerConstants { - - public enum IndexerState { - INDEXER, - REVERSE, - IDLE - } - - public static final boolean attached = true; - - public static final int id = -1; - - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; - - public static final boolean invert = false; - public static final boolean breakType = false; - public static final double gearRatio = 1 / 1; - - - public static final Current stallLimit = Units.Amps.of(60); - public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; - - public static final double indexerSpeed = 0.5; - public static final double reverseSpeed = -0.5; - public static final double idleSpeed = 0.0; - public enum IndexerModes { - INDEXER(indexerSpeed), - REVERSE(reverseSpeed), - IDLE(idleSpeed); - - public double output; - - IndexerModes(double output) { - this.output = output; - } - } - } - - public static final class IntakeRollerIOConstants { - public static final boolean attached = true; - public static final boolean useRpm = false; - - public static final int id = -1; - - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; - public static final double maxIAccum = 0.2; - - public static final double gearRatio = 1 / 1; - - public static final Current forwardTorqueLimit = Units.Amps.of(80); - public static final Current reverseTorqueLimit = Units.Amps.of(80); - - public static final boolean invert = false; - public static final boolean gravityType = false; - public static final boolean breakType = false; - - public static final FeedbackSensorSourceValue feedbackSensorCTRE = - FeedbackSensorSourceValue.FusedCANcoder; - public static final FeedbackSensor feedbackSensorREV = - FeedbackSensor.kPrimaryEncoder; - - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; - - public static final boolean useFMaxRotation = true; - public static final boolean useRMaxRotation = true; - public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); - public static final Angle maxFowardRotation = Units.Rotation.of(5); - - public static final boolean useStallLimit = true; - public static final boolean useSupplyLimit = true; - public static final Current stallLimit = Units.Amps.of(80); - public static final Current supplyLimit = Units.Amps.of(60); - - public static final AngularVelocity rollerIOSpeed = Units.RPM.of(3000); - public static final AngularVelocity outtakeSpeed = Units.RPM.of(0); - public static final AngularVelocity idleSpeed = Units.RPM.of(0); - - public enum RollerIOModes { - IDLE(idleSpeed, 0.0), - INTAKE(rollerIOSpeed, 8.4), - OUTTAKE(outtakeSpeed, -4.8); - - public AngularVelocity speed; - // TODO: Make sure voltage is just output * 12 (for 12V) - public double voltage; - - RollerIOModes(AngularVelocity speed, double voltage) { - this.speed = speed; - this.voltage = voltage; - } - } - } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c39a65..de14205 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,33 +7,47 @@ package frc.robot; -import static frc.robot.subsystems.vision.VisionConstants.*; - import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.IntakeRollerIOConstants.RollerIOModes; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.ClimberIOSim; +import frc.robot.subsystems.climber.ClimberIOSparkFlex; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.intake.roller.Roller; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.hopper.CarpetIO; +import frc.robot.subsystems.hopper.CarpetIOSim; +import frc.robot.subsystems.hopper.CarpetIOSparkFlex; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.intake.pivot.PivotIO; +import frc.robot.subsystems.intake.pivot.PivotIOSim; +import frc.robot.subsystems.intake.pivot.PivotIOSparkFlex; import frc.robot.subsystems.intake.roller.RollerIO; import frc.robot.subsystems.intake.roller.RollerIOSim; import frc.robot.subsystems.intake.roller.RollerIOSparkFlex; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOLimelight; -import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.angler.AnglerIO; +import frc.robot.subsystems.shooter.angler.AnglerIOSim; +import frc.robot.subsystems.shooter.angler.AnglerIOTalonFX; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; import frc.team5431.titan.core.joysticks.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -47,8 +61,11 @@ public class RobotContainer { // Subsystems private final Drive drive; - private final Vision vision; - private final Roller roller; +// private final Vision vision; + private final Intake intake; + private final Shooter shooter; + private final Carpet carpet; + private final Climber climber; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -73,15 +90,16 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.BackRight)); // Real robot, instantiate hardware IO implementations - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOLimelight(camera0Name, drive::getRotation), - new VisionIOLimelight(camera1Name, drive::getRotation)); - roller = - new Roller( - new RollerIOSparkFlex()); - + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOLimelight(camera0Name, drive::getRotation), + // new VisionIOLimelight(camera1Name, drive::getRotation)); + + intake = new Intake(new RollerIOSim(), new PivotIOSim()); + shooter = new Shooter(new AnglerIOSim(), new FlywheelIOSim()); + carpet = new Carpet(new CarpetIOSim()); + climber = new Climber(new ClimberIOSim()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -116,17 +134,21 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), - new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), + // new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); - roller = - new Roller( - new RollerIOSim() {}); + intake = + new Intake(new RollerIOSim(), new PivotIOSim()); + shooter = + new Shooter(new AnglerIOSim(), new FlywheelIOSim()); + carpet = + new Carpet(new CarpetIOSim()); + climber = + new Climber(new ClimberIOSim()); break; - default: // Replayed robot, disable IO implementations drive = @@ -137,9 +159,11 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); - roller = new Roller(new RollerIO() {}); - + // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); + intake = new Intake(new RollerIO() {}, new PivotIO() {}); + shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {}); + carpet = new Carpet(new CarpetIO() {}); + climber = new Climber(new ClimberIO() {}); break; } @@ -165,6 +189,10 @@ public RobotContainer() { configureDriverBindings(); configureOperatorBindings(); + + + SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); + } /** @@ -182,18 +210,18 @@ private void configureDriverBindings() { () -> -driver.getLeftX(), () -> -driver.getRightX())); - // Lock to 0° when A button is held - driver - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> Rotation2d.kZero)); + // // Lock to 0° when A button is held + // driver + // .a() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, + // () -> -driver.getLeftY(), + // () -> -driver.getLeftX(), + // () -> Rotation2d.kZero)); - // Switch to X pattern when X button is pressed - driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // // Switch to X pattern when X button is pressed + // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed driver @@ -224,7 +252,13 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - operator.a().onTrue(roller.runIntakeCommand(RollerIOModes.INTAKE, true)); + // Default Commands + intake.setDefaultCommand(intake.runIntakeeCommand(IntakeMode.OUT_IDLE)); + + + + operator.a().whileTrue(intake.runIntakeeCommand(IntakeMode.INTAKE)); + operator.b().whileTrue(intake.runIntakeeCommand(IntakeMode.OUTTAKE)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index d41c38b..bd1daa8 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -1,321 +1,284 @@ -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - package frc.robot.generated; import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.*; import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -import frc.robot.Constants; -// Generated by the Tuner X Swerve Project Generator +// Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; - - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(Constants.CANBUS.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(2.33).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.125; + + private static final double kDriveGearRatio = 5.902777777777778; + private static final double kSteerGearRatio = 18.75; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 49; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 9; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.301513671875); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = true; + + private static final Distance kFrontLeftXPos = Inches.of(11); + private static final Distance kFrontLeftYPos = Inches.of(11.5); + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 3; + private static final int kFrontRightEncoderId = 10; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.318603515625); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = true; + + private static final Distance kFrontRightXPos = Inches.of(11); + private static final Distance kFrontRightYPos = Inches.of(-11.5); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 8; + private static final int kBackLeftEncoderId = 12; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181396484375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = true; + + private static final Distance kBackLeftXPos = Inches.of(-11); + private static final Distance kBackLeftYPos = Inches.of(11.5); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 6; + private static final int kBackRightEncoderId = 11; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.458984375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = true; + + private static final Distance kBackRightXPos = Inches.of(-11); + private static final Distance kBackRightYPos = Inches.of(-11.5); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + // /** + // * Creates a CommandSwerveDrivetrain instance. + // * This should only be called once in your robot program,. + // */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + // ); + // } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]ᵀ, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, - * y, theta]ᵀ, with units in meters and radians - * @param modules Constants for each specific module + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } } - } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..ea6267c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.climber.ClimberConstants.ClimberModes; + +public class Climber extends SubsystemBase { + private final ClimberIO climberIO; + private final ClimberIOInputsAutoLogged carpetInputs = new ClimberIOInputsAutoLogged(); + + private ClimberModes mode; + + public Climber(ClimberIO climberIO) { + this.climberIO = climberIO; + this.mode = ClimberModes.STOW; + } + + @Override + public void periodic() { + climberIO.updateInputs(carpetInputs); + Logger.processInputs("Climber/", carpetInputs); + + Logger.recordOutput("Climber/Mode", mode); + } + + public void runClimberEnum(ClimberModes climberMode) { + this.mode = climberMode; + climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); + } + + public Command runCarpetCommand(ClimberModes climberModes) { + return new RunCommand(() -> { + this.runClimberEnum(climberModes); + }, this).withName("Carpet.runCarpetEnum"); + } + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..f3b35d3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.pathplanner.lib.config.PIDConstants; +import com.revrobotics.spark.FeedbackSensor; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; + +public class ClimberConstants { + public enum ClimberModes { + STOW(Units.Rotation.of(0.0)), + CLIMB(Units.Rotation.of(5)); + + public Angle positionAngle; + + ClimberModes(Angle positionAngle) { + this.positionAngle = positionAngle; + } + } + + public static final boolean attached = true; + + public static final int id = -1; + + PIDConstants pidConstants = new PIDConstants(1, 0, 0); + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = (42.0 / 11.0) * (42.0 / 18.0); + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kPrimaryEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..d2fdc26 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + public boolean climberConnected = false; + public double appliedVoltage = 0.0; + public double positionAngle = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ClimberIOInputs inputs) {} + + /** Run the motor to the specified position. */ + public default void setClimberPosition(double positionAngle) {} +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java new file mode 100644 index 0000000..266e503 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climber; + +public class ClimberIOSim implements ClimberIO { + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java new file mode 100644 index 0000000..2de40a0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -0,0 +1,50 @@ +package frc.robot.subsystems.climber; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkFlex; + +import static frc.robot.util.SparkUtil.*; + +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class ClimberIOSparkFlex implements ClimberIO { + private final SparkFlex sparkFlex = new SparkFlex(ClimberConstants.id, null); + private final RelativeEncoder encoder = sparkFlex.getEncoder(); + + public static class PivotSparkFlexConfig extends REVMechanism.Config { + public PivotSparkFlexConfig() { + super("PivotSparkFlex", ClimberConstants.id); + configPIDGains(ClimberConstants.p, ClimberConstants.i, ClimberConstants.d); + configFeedbackSensorSource(ClimberConstants.feedbackSensorREV); + // configGear(ClimberConstants.gearRatio); + // configGravity(ClimberConstants.gravityType); + configSmartCurrentLimit(ClimberConstants.stallLimit, ClimberConstants.supplyLimit); + configSmartStallCurrentLimit(ClimberConstants.stallLimit); + configReverseSoftLimit( + ClimberConstants.maxReverseRotation, ClimberConstants.useRMaxRotation); + configForwardSoftLimit( + ClimberConstants.maxFowardRotation, ClimberConstants.useFMaxRotation); + } + } + + public ClimberIOSparkFlex() { + sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.positionAngle = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setClimberPosition(double positionAngle) { + sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, + ClosedLoopSlot.kSlot0); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java new file mode 100644 index 0000000..1dd4b97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class ClimberIOTalonFX implements ClimberIO { + private final TalonFX talon = new TalonFX(ClimberConstants.id, Constants.CANBUS); + + public static class ClimberTalonFXConfig extends CTREMechanism.Config { + public ClimberTalonFXConfig() { + super("ClimberTalonFX", Constants.CANBUS); + configPIDGains(ClimberConstants.p, ClimberConstants.i, ClimberConstants.d); + configNeutralBrakeMode(ClimberConstants.breakType); + configFeedbackSensorSource(ClimberConstants.feedbackSensorCTRE); + configGearRatio(ClimberConstants.gearRatio); + configSupplyCurrentLimit(ClimberConstants.supplyLimit); + } + } + + + private StatusSignal appliedVoltage; + private StatusSignal pivotPosition; + private StatusSignal currentAmps; + + // No clue stole from ModuleIO + private final Debouncer climberConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private ClimberTalonFXConfig config = new ClimberTalonFXConfig(); + + public ClimberIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + var climberStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.climberConnected = climberConnectedDebounce.calculate(climberStatus.isOK()); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.positionAngle = pivotPosition.getValue().in(Rotation); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setClimberPosition(double positionAngle) { + PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + talon.setControl(mm); + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java new file mode 100644 index 0000000..43d6c7f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.feeder; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.feeder.FeederConstants.FeederState; +import frc.robot.subsystems.feeder.FeederIO.FeederIOInputs; +import lombok.Getter; +import lombok.Setter; + +public class Feeder extends SubsystemBase { + + private final FeederIO feederIO; + private final FeederIOInputs feederInputs = new FeederIOInputs(); + + @Getter @Setter private FeederModes feederMode; + @Getter @Setter private FeederState feederState; + + public Feeder(FeederIO feederIO) { + this.feederIO = feederIO; + this.feederMode = FeederModes.IDLE; + this.feederState = FeederState.IDLE; + } + + @Override + public void periodic() { + feederIO.updateInputs(feederInputs); + // TODO: Add Logger.processInputs once auto-logged classes are generated + // Logger.processInputs("Feeder", feederInputs); + + SmartDashboard.putString("Feeder Mode", feederMode.toString()); + SmartDashboard.putBoolean("Feeder Connected", feederInputs.feederConnected); + SmartDashboard.putNumber("Feeder Voltage", feederInputs.appliedVoltage); + SmartDashboard.putNumber("Feeder RPM", feederInputs.RPM); + SmartDashboard.putNumber("Feeder Current", feederInputs.currentAmps); + + Logger.recordOutput("Feeder/Mode", feederMode); + Logger.recordOutput("Feeder/State", feederState); + + switch (this.feederMode) { + case IDLE: + this.feederState = FeederState.IDLE; + break; + case FEEDER: + this.feederState = FeederState.FEEDER; + break; + case REVERSE: + this.feederState = FeederState.REVERSE; + break; + } + } + + public void runFeederEnum(FeederModes feederMode) { + this.feederMode = feederMode; + feederIO.setPercentOutput(feederMode.output); + } + + public Command runFeederCommand(FeederModes feederMode) { + return new RunCommand(() -> runFeederEnum(feederMode), this).withName("Feeder.runEnum"); + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java new file mode 100644 index 0000000..7b8d5e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.feeder; + +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Current; + +public class FeederConstants { + + public enum FeederState { + FEEDER, + REVERSE, + IDLE + } + + public static final boolean attached = true; + + public static final int id = -1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + + public static final boolean invert = false; + public static final boolean breakType = false; + public static final double gearRatio = 1 / 1; + + public static final Current stallLimit = Units.Amps.of(60); + public static final Current supplyLimit = Units.Amps.of(80); + public static final double maxForwardOutput = 0.5; + public static final double maxReverseOutput = -0.5; + + public static final double FeederSpeed = 0.5; + public static final double reverseSpeed = -0.5; + public static final double idleSpeed = 0.0; + + public enum FeederModes { + FEEDER(FeederSpeed), + REVERSE(reverseSpeed), + IDLE(idleSpeed); + + public double output; + + FeederModes(double output) { + this.output = output; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java new file mode 100644 index 0000000..ab06013 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.feeder; + +import org.littletonrobotics.junction.AutoLog; + +public interface FeederIO { + @AutoLog + public static class FeederIOInputs { + public boolean feederConnected = false; + public double appliedVoltage = 0.0; + public double RPM = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FeederIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setFeederVoltage(double voltage) {} + + /** Run the motor at the specified percent output. */ + public default void setPercentOutput(double percent) {} + +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java new file mode 100644 index 0000000..04992e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.feeder; + +public class FeederIOSim implements FeederIO { + +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java new file mode 100644 index 0000000..c9d6dac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.feeder; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import static frc.robot.util.SparkUtil.*; + + +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class FeederIOSparkFlex implements FeederIO { + private final SparkFlex sparkFlex = new SparkFlex(FeederConstants.id, MotorType.kBrushless); + + + public static class FeederSparkFlexConfig extends REVMechanism.Config { + public FeederSparkFlexConfig() { + super("PivotSparkFlex", FeederConstants.id); + configPIDGains(FeederConstants.p, FeederConstants.i, FeederConstants.d); + configSmartCurrentLimit(FeederConstants.stallLimit, FeederConstants.supplyLimit); + configSmartStallCurrentLimit(FeederConstants.stallLimit); + } + } + + public FeederIOSparkFlex() { + + sparkFlex.configure(new FeederSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setFeederVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } + +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java new file mode 100644 index 0000000..dc90de4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -0,0 +1,72 @@ +package frc.robot.subsystems.feeder; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class FeederIOTalonFX implements FeederIO { + private final TalonFX talon = new TalonFX(FeederConstants.id, Constants.CANBUS); + + public static class FeederIOTalonFXConfig extends CTREMechanism.Config { + public FeederIOTalonFXConfig() { + super("Feeder", Constants.CANBUS); + + configNeutralBrakeMode(FeederConstants.breakType); + configStatorCurrentLimit(FeederConstants.stallLimit); + configSupplyCurrentLimit(FeederConstants.supplyLimit); + configForwardSoftLimit(FeederConstants.maxForwardOutput, true); + configReverseSoftLimit(FeederConstants.maxReverseOutput, true); + configPIDGains(FeederConstants.p, FeederConstants.i, FeederConstants.d); + configPeakOutput(FeederConstants.maxForwardOutput, FeederConstants.maxReverseOutput); + configGearRatio(FeederConstants.gearRatio); + configMotorInverted(FeederConstants.invert); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal feederRPM; + private StatusSignal currentAmps; + + private final Debouncer feederConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private FeederIOTalonFXConfig config = new FeederIOTalonFXConfig(); + + public FeederIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + feederRPM = talon.getVelocity(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, feederRPM); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + var feederStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, feederRPM); + + inputs.feederConnected = feederConnectedDebounce.calculate(feederStatus.isOK()); + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.RPM = feederRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setFeederVoltage(double voltage) { + talon.setVoltage(voltage); + } + + @Override + public void setPercentOutput(double percent) { + talon.set(percent); + } +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java new file mode 100644 index 0000000..e8b6f20 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.hopper; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.pathplanner.lib.config.PIDConstants; +import com.revrobotics.spark.FeedbackSensor; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +public class CarpetConstants { + public enum CarpetModes { + IDLE(Units.Volts.of(0.0)), + INTAKE(Units.Volts.of(8.4)); + + public Voltage voltage; + + CarpetModes(Voltage voltage) { + this.voltage = voltage; + } + } + + public static final class CarpetRollerConstants { + public static final boolean attached = true; + + public static final int id = -1; + + PIDConstants pidConstants = new PIDConstants(1, 0, 0); + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kPrimaryEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + } + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIO.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIO.java new file mode 100644 index 0000000..7ce81f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.hopper; + +import org.littletonrobotics.junction.AutoLog; + +public interface CarpetIO { + @AutoLog + public static class CarpetIOInputs { + public boolean rollerConnected = false; + public double appliedVoltage = 0.0; + public double RPM = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(CarpetIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setRollerVoltage(double voltage) {} + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java new file mode 100644 index 0000000..98a07c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.hopper; + +public class CarpetIOSim implements CarpetIO { + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java new file mode 100644 index 0000000..a6265a1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.hopper; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkFlex; + +import static frc.robot.util.SparkUtil.*; + +import frc.robot.subsystems.hopper.CarpetConstants.CarpetRollerConstants; +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class CarpetIOSparkFlex extends CarpetIOTalonFX { + private final SparkFlex sparkFlex = new SparkFlex(CarpetRollerConstants.id, null); + private final RelativeEncoder encoder = sparkFlex.getEncoder(); + + public static class CarpetIOSparkFlexConfig extends REVMechanism.Config { + public CarpetIOSparkFlexConfig() { + super("CarpetSparkFlex", CarpetRollerConstants.id); + configPIDGains(CarpetRollerConstants.p, CarpetRollerConstants.i, CarpetRollerConstants.d); + configFeedbackSensorSource(CarpetRollerConstants.feedbackSensorREV); + // configGear(CarpetIOConstants.gearRatio); + // configGravity(CarpetIOConstants.gravityType); + configSmartCurrentLimit(CarpetRollerConstants.stallLimit, CarpetRollerConstants.supplyLimit); + configSmartStallCurrentLimit(CarpetRollerConstants.stallLimit); + } + } + + public CarpetIOSparkFlex() { + sparkFlex.configure(new CarpetIOSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(CarpetIOInputs inputs) { + ifOk(sparkFlex, encoder::getVelocity, (value) -> inputs.RPM = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setRollerVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java new file mode 100644 index 0000000..563ba28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.hopper; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetRollerConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class CarpetIOTalonFX implements CarpetIO { + private final TalonFX talon = new TalonFX(CarpetRollerConstants.id, Constants.CANBUS); + + public static class CarpetIOTalonFXConfig extends CTREMechanism.Config { + public CarpetIOTalonFXConfig() { + super("RollerTalonFX",Constants.CANBUS); + configPIDGains(CarpetRollerConstants.p, CarpetRollerConstants.i, CarpetRollerConstants.d); + configNeutralBrakeMode(CarpetRollerConstants.breakType); + configFeedbackSensorSource(CarpetRollerConstants.feedbackSensorCTRE); + // configGearRatio(CarpetRoller.gearRatio); + // configGravityType(CarpetRoller.gravityType); + configSupplyCurrentLimit(CarpetRollerConstants.supplyLimit); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal rollerRPM; + private StatusSignal currentAmps; + + // No clue what this means copied from ModuleIO + private final Debouncer carpetConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private CarpetIOTalonFXConfig config = new CarpetIOTalonFXConfig(); + + public CarpetIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + rollerRPM = talon.getVelocity(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, rollerRPM); + } + + @Override + public void updateInputs(CarpetIOInputs inputs) { + var carpetStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + + inputs.rollerConnected = carpetConnectedDebounce.calculate(carpetStatus.isOK()); + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.RPM = rollerRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setRollerVoltage(double voltage) { + talon.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/hopper/carpet.java b/src/main/java/frc/robot/subsystems/hopper/carpet.java new file mode 100644 index 0000000..f147957 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/carpet.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.hopper; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; +import frc.robot.subsystems.hopper.CarpetIO.CarpetIOInputs; + +public class Carpet extends SubsystemBase { + private final CarpetIO carpetIO; + private final CarpetIOInputsAutoLogged carpetInputs = new CarpetIOInputsAutoLogged(); + + private CarpetModes mode; + + public Carpet(CarpetIO carpetIO) { + this.carpetIO = carpetIO; + this.mode = CarpetModes.IDLE; + } + + @Override + public void periodic() { + carpetIO.updateInputs(carpetInputs); + Logger.processInputs("Carpet/", carpetInputs); + + Logger.recordOutput("Carpet/Mode", mode); + } + + public void runRollerEnum(CarpetModes carpetMode) { + this.mode = carpetMode; + carpetIO.setRollerVoltage(mode.voltage.baseUnitMagnitude()); + } + + public Command runCarpetCommand(CarpetModes carpetMode) { + return new RunCommand(() -> { + this.runRollerEnum(carpetMode); + }, this).withName("Carpet.runCarpetEnum"); + } + +} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java deleted file mode 100644 index fa5be02..0000000 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.subsystems.indexer; - -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants; -import frc.robot.Constants.IndexerConstants; -import frc.robot.Constants.IndexerConstants.IndexerModes; -import frc.robot.Constants.IndexerConstants.IndexerState; -import frc.team5431.titan.core.subsystem.CTREMechanism; -import lombok.Getter; -import lombok.Setter; - -public class Indexer extends CTREMechanism { - - @Getter @Setter private IndexerModes indexerModes; - @Getter @Setter private IndexerState indexerState; - - private boolean attached; - private TalonFX motor; - - public static class IndexerConfig extends Config { - public IndexerConfig() { - super("Indexer", IndexerConstants.id, Constants.CANBUS); - - configNeutralBrakeMode(IndexerConstants.breakType); - configStatorCurrentLimit(IndexerConstants.stallLimit, true); - configSupplyCurrentLimit(IndexerConstants.supplyLimit, true); - configForwardSoftLimit(IndexerConstants.maxForwardOutput, true); - configReverseSoftLimit(IndexerConstants.maxReverseOutput, true); - configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); - configPeakOutput(IndexerConstants.maxForwardOutput, IndexerConstants.maxReverseOutput); - configGearRatio(IndexerConstants.gearRatio); - configMotorInverted(IndexerConstants.invert); - } - } - - public Indexer(TalonFX motor, boolean attached, IndexerConfig config) { - super(motor, attached, config); - this.attached = attached; - this.motor = motor; - this.indexerModes = IndexerModes.IDLE; - this.indexerState = IndexerState.IDLE; - - config.applyTalonConfig(motor); - } - - @Override - public void periodic() { - SmartDashboard.putString("Indexer Mode", getIndexerModes().toString()); - - - switch (this.indexerModes) { - case IDLE: - setIndexerState(IndexerState.IDLE); - break; - case INDEXER: - setIndexerState(IndexerState.INDEXER); - break; - case REVERSE: - setIndexerState(IndexerState.REVERSE); - break; - } - - } - - public Command runIndexerCommand(IndexerModes indexerModes) { - return new RunCommand(() -> setPercentOutput(indexerModes.output), this).withName("Indexer.runEnum"); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..e3ce7ce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.intake.pivot.PivotIO; +import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; +import frc.robot.subsystems.intake.roller.RollerIO; +import frc.robot.subsystems.intake.roller.RollerIOInputsAutoLogged; + +public class Intake extends SubsystemBase { + private final RollerIO rollerIO; + private final PivotIO pivotIO; + private final RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); + private final PivotIOInputsAutoLogged pivotInputs = new PivotIOInputsAutoLogged(); + + private IntakeMode intakeMode; + // private IntakePivotModes pivotMode; + + + public Intake(RollerIO rollerIO, PivotIO pivotIO) { + this.rollerIO = rollerIO; + this.pivotIO = pivotIO; + this.intakeMode = IntakeMode.STOW; + } + + @Override + public void periodic() { + rollerIO.updateInputs(rollerInputs); + Logger.processInputs("Intake/Roller", rollerInputs); + + pivotIO.updateInputs(pivotInputs); + Logger.processInputs("Intake/Pivot", pivotInputs); + + Logger.recordOutput("Intake/Mode", intakeMode); + } + + public void runIntakeEnum(IntakeMode intakeMode) { + this.intakeMode = intakeMode; + rollerIO.setRollerVoltage(intakeMode.voltage.baseUnitMagnitude()); + pivotIO.setPosition(intakeMode.position.magnitude()); + } + + public Command runIntakeeCommand(IntakeMode intakeMode) { + return new RunCommand(() -> { + this.runIntakeEnum(intakeMode); + }, this).withName("Intake.runIntakeeCommand" + intakeMode.toString()); + } + + // public Command runPivotCommand(IntakeMode intakeMode) { + // return new RunCommand(() -> { + // this.runPivotEnum(intakeMode); + // }, this).withName("Intake.runPivotCommand" + intakeMode.toString()); + // } + + // public Command runRollerCommand(IntakeMode rollerMode) { + // return new RunCommand(() -> { + // this.runRollerEnum(rollerMode); + // }, this).withName("Intake.runRollerCommand" + rollerMode.toString()); + // } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..a33f971 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.pathplanner.lib.config.PIDConstants; +import com.revrobotics.spark.FeedbackSensor; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +public class IntakeConstants { + + public enum IntakeMode { + + STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), + OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), + INTAKE(Units.Volts.of(8.4), Units.Degrees.of(0.0)), + OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(180.0)); + + public Voltage voltage; + public Angle position; + + IntakeMode(Voltage voltage, Angle position) { + this.voltage = voltage; + this.position = position; + } + } + + public static final class IntakeRollerConstants { + + public static final boolean attached = true; + + public static final int id = -1; + + PIDConstants pidConstants = new PIDConstants(1, 0, 0); + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kPrimaryEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + } + + public static final class IntakePivotConstants { + public static final boolean attached = true; + + public static final int id = -1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kAbsoluteEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java new file mode 100644 index 0000000..d9d6c6b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.intake.pivot; + +import org.littletonrobotics.junction.AutoLog; + +public interface PivotIO { + + @AutoLog + public static class PivotIOInputs { + public boolean pivotConnected = false; + public double appliedVoltage = 0.0; + public double positionAngle = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(PivotIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setPivotVoltage(double voltage) {} + + /** Run the motor to the specified position. */ + public default void setPosition(double positionAngle) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java new file mode 100644 index 0000000..c88dbfb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.intake.pivot; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class PivotIOSim implements PivotIO { + private DCMotorSim pivotMotorSim; + private boolean pivotClosedLoop = false; + private double appliedVoltage = 0.0; + private double pivotFFVolts = 0.0; + + // From ModuleIOSim no clue tbh + private static final double pivot_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double PIVOT_KV = 1.0 / Units.rotationsToRadians(1.0 / pivot_KV_ROT); + private static final double PIVOT_KS = 0.0; + private static final double PIVOT_KP = 1.0; + private static final double PIVOT_KD = 0.0; + + private PIDController pivotController = new PIDController(PIVOT_KP, 0, PIVOT_KD); + + public PivotIOSim() { + this.pivotMotorSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + + // pivotMotorSim.set + pivotController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + if (pivotClosedLoop) { + appliedVoltage = pivotFFVolts + pivotController.calculate(pivotMotorSim.getAngularVelocityRadPerSec()); + } else { + pivotController.reset(); + } + + pivotMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + pivotMotorSim.update(0.02); + + inputs.pivotConnected = true; + inputs.positionAngle = pivotMotorSim.getAngularPositionRad(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = Math.abs(pivotMotorSim.getCurrentDrawAmps()); + } + + @Override + public void setPosition(double positionAngle) { + pivotClosedLoop = true; + pivotController.setSetpoint(positionAngle); + } + + public void setAppliedVoltage(double appliedVoltage) { + this.appliedVoltage = MathUtil.clamp(appliedVoltage, -12.0, 12.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java new file mode 100644 index 0000000..5e7485a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.intake.pivot; + +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkFlex; + +import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class PivotIOSparkFlex implements PivotIO { + private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, null); + private final RelativeEncoder encoder = sparkFlex.getEncoder(); + + public static class PivotSparkFlexConfig extends REVMechanism.Config { + public PivotSparkFlexConfig() { + super("PivotSparkFlex", IntakePivotConstants.id); + configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); + // configGear(IntakePivotConstants.gearRatio); + // configGravity(IntakePivotConstants.gravityType); + configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); + configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation, IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation, IntakePivotConstants.useFMaxRotation); + } + } + + public PivotIOSparkFlex() { + sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.positionAngle = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setPivotVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } + + @Override + public void setPosition(double positionAngle) { + sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, + ClosedLoopSlot.kSlot0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java new file mode 100644 index 0000000..92baf0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.intake.pivot; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +import static edu.wpi.first.units.Units.*; + +import frc.robot.Constants; +import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class PivotIOTalonFX implements PivotIO { + private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.CANBUS); + + public static class PivotTalonFXConfig extends CTREMechanism.Config { + public PivotTalonFXConfig() { + super("PivotTalonFX", Constants.CANBUS); + configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); + configNeutralBrakeMode(IntakePivotConstants.breakType); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); + configGearRatio(IntakePivotConstants.gearRatio); + configGravityType(IntakePivotConstants.gravityType); + configSupplyCurrentLimit(IntakePivotConstants.supplyLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation.in(Rotation), IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation.in(Rotation), IntakePivotConstants.useFMaxRotation); + } + } + + + private StatusSignal appliedVoltage; + private StatusSignal pivotPosition; + private StatusSignal currentAmps; + + // No clue what this means copied from ModuleIO + private final Debouncer pivotConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private PivotTalonFXConfig config = new PivotTalonFXConfig(); + + public PivotIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + var pivotStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.pivotConnected = pivotConnectedDebounce.calculate(pivotStatus.isOK()); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.positionAngle = pivotPosition.getValue().in(Rotation); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setPivotVoltage(double voltage) { + talon.setVoltage(voltage); + } + + @Override + public void setPosition(double positionAngle) { + PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + talon.setControl(mm); + } + +} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java b/src/main/java/frc/robot/subsystems/intake/roller/Roller.java deleted file mode 100644 index 081fc95..0000000 --- a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.subsystems.intake.roller; - - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.IntakeRollerIOConstants.RollerIOModes; - -public class Roller extends SubsystemBase { - - private final RollerIO rollerIO; - private final RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); - - private RollerIOModes mode; - - public Roller(RollerIO rollerIO) { - this.rollerIO = rollerIO; - this.mode = RollerIOModes.IDLE; - } - - @Override - public void periodic() { - rollerIO.updateInputs(inputs); - } - - public void runEnum(RollerIOModes rollerIOModes, boolean useRPM) { - this.mode = rollerIOModes; - if (useRPM) { - rollerIO.setRPM(rollerIOModes.speed.magnitude()); - } - else { - rollerIO.setRollerVoltage(rollerIOModes.voltage); - } - } - - public Command runIntakeCommand(RollerIOModes rollerIOModes, boolean useRPM) { - return new RunCommand(() -> this.runEnum(rollerIOModes, useRPM), this) - .withName("Intake.runEnum"); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java index 7428118..3a9364b 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java @@ -9,7 +9,7 @@ public static class RollerIOInputs { public boolean rollerConnected = false; public double appliedVoltage = 0.0; public double RPM = 0.0; - + public double currentAmps = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java index 331d3fa..fa4f894 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java @@ -9,15 +9,17 @@ public class RollerIOSim implements RollerIO { private DCMotorSim rollerMotorSim; - private PIDController rollerController; + private PIDController rollerController = new PIDController(ROLLER_KP, 0, ROLLER_KD); private boolean rollerClosedLoop = false; private double appliedVoltage = 0.0; - private double driveFFVolts = 0.0; + private double rollerFFVolts = 0.0; // From ModuleIOSim no clue tbh - private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation - private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); - private static final double DRIVE_KS = 0.0; + private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); + private static final double ROLLER_KS = 0.0; + private static final double ROLLER_KP = 1.0; + private static final double ROLLER_KD = 0.0; public RollerIOSim() { @@ -31,7 +33,7 @@ public RollerIOSim() { @Override public void updateInputs(RollerIOInputs inputs) { if (rollerClosedLoop) { - appliedVoltage = driveFFVolts + rollerController.calculate(rollerMotorSim.getAngularVelocityRadPerSec()); + appliedVoltage = rollerFFVolts + rollerController.calculate(rollerMotorSim.getAngularVelocityRadPerSec()); } else { rollerController.reset(); } @@ -42,16 +44,19 @@ public void updateInputs(RollerIOInputs inputs) { inputs.rollerConnected = true; inputs.RPM = rollerMotorSim.getAngularVelocityRadPerSec(); inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = Math.abs(rollerMotorSim.getCurrentDrawAmps()); + } @Override public void setRPM(double RPM) { rollerClosedLoop = true; - driveFFVolts = DRIVE_KS * Math.signum(RPM) + DRIVE_KV * RPM; + rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; rollerController.setSetpoint(RPM); } - public void setAppliedVoltage(double appliedVoltage) { - this.appliedVoltage = MathUtil.clamp(appliedVoltage, -12.0, 12.0); + @Override + public void setRollerVoltage(double voltage) { + this.appliedVoltage = MathUtil.clamp(voltage, -12.0, 12.0); } } diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index a7e0ff1..488d0a8 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -9,25 +9,21 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; -import frc.robot.Constants.IntakeRollerIOConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class RollerIOSparkFlex implements RollerIO { - private final SparkFlex sparkFlex = new SparkFlex(0, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakeRollerConstants.id, null); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class RollerIOSparkFlexConfig extends REVMechanism.Config { public RollerIOSparkFlexConfig() { - super("RollerSparkFlex", IntakeRollerIOConstants.id); - configPIDGains(IntakeRollerIOConstants.p, IntakeRollerIOConstants.i, IntakeRollerIOConstants.d); - configFeedbackSensorSource(IntakeRollerIOConstants.feedbackSensorREV); + super("RollerSparkFlex", IntakeRollerConstants.id); + configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); + configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorREV); // configGear(RollerIOConstants.gearRatio); // configGravity(RollerIOConstants.gravityType); - configSmartCurrentLimit(IntakeRollerIOConstants.stallLimit, IntakeRollerIOConstants.supplyLimit); - configSmartStallCurrentLimit(IntakeRollerIOConstants.stallLimit); - configReverseSoftLimit( - IntakeRollerIOConstants.maxReverseRotation, IntakeRollerIOConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeRollerIOConstants.maxFowardRotation, IntakeRollerIOConstants.useFMaxRotation); + configSmartCurrentLimit(IntakeRollerConstants.stallLimit, IntakeRollerConstants.supplyLimit); + configSmartStallCurrentLimit(IntakeRollerConstants.stallLimit); } } @@ -39,6 +35,9 @@ public RollerIOSparkFlex() { public void updateInputs(RollerIOInputs inputs) { ifOk(sparkFlex, encoder::getVelocity, (value) -> inputs.RPM = value); ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + + // figure out } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 1969919..53b06b2 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -4,66 +4,62 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.Units; + +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; -import frc.robot.Constants.IntakeRollerIOConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; public class RollerIOTalonFX implements RollerIO { - private final TalonFX talon = new TalonFX(IntakeRollerIOConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakeRollerConstants.id, Constants.CANBUS); public static class RollerTalonFXConfig extends CTREMechanism.Config { public RollerTalonFXConfig() { - super("RollerTalonFX", IntakeRollerIOConstants.id, Constants.CANBUS); - configPIDGains(IntakeRollerIOConstants.p, IntakeRollerIOConstants.i, IntakeRollerIOConstants.d); - configNeutralBrakeMode(IntakeRollerIOConstants.breakType); - configFeedbackSensorSource(IntakeRollerIOConstants.feedbackSensorCTRE); - configGearRatio(IntakeRollerIOConstants.gearRatio); - configGravityType(IntakeRollerIOConstants.gravityType); - configSupplyCurrentLimit(IntakeRollerIOConstants.supplyLimit, IntakeRollerIOConstants.useSupplyLimit); - configStatorCurrentLimit(IntakeRollerIOConstants.stallLimit, IntakeRollerIOConstants.useStallLimit); - configReverseSoftLimit( - IntakeRollerIOConstants.maxReverseRotation.in(Rotation), IntakeRollerIOConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeRollerIOConstants.maxFowardRotation.in(Rotation), IntakeRollerIOConstants.useFMaxRotation); + super("RollerTalonFX",Constants.CANBUS); + configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); + configNeutralBrakeMode(IntakeRollerConstants.breakType); + configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorCTRE); + // configGearRatio(IntakeRollerConstants.gearRatio); + // configGravityType(IntakeRollerConstants.gravityType); + configSupplyCurrentLimit(IntakeRollerConstants.supplyLimit); } } private StatusSignal appliedVoltage; private StatusSignal rollerRPM; - private StatusSignal rollerOutput; + private StatusSignal currentAmps; + + // No clue what this means copied from ModuleIO + private final Debouncer rollerConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private RollerTalonFXConfig config; + private RollerTalonFXConfig config = new RollerTalonFXConfig(); - public RollerIOTalonFX(TalonFX talon) { + public RollerIOTalonFX() { appliedVoltage = talon.getMotorVoltage(); rollerRPM = talon.getVelocity(); - rollerOutput = talon.getClosedLoopOutput(); + currentAmps = talon.getStatorCurrent(); config.applyTalonConfig(talon); - BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, rollerOutput, rollerRPM); + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, rollerRPM); } @Override public void updateInputs(RollerIOInputs inputs) { - BaseStatusSignal.refreshAll(appliedVoltage, rollerOutput, rollerRPM); + var rollerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + inputs.rollerConnected = rollerConnectedDebounce.calculate(rollerStatus.isOK()); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.RPM = rollerRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); } @Override public void setRollerVoltage(double voltage) { talon.setVoltage(voltage); } - - @Override - public void setRPM(double rpm) { - VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - talon.setControl(output); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index da5d41e..d455c1d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,70 +1,50 @@ package frc.robot.subsystems.shooter; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants; -import frc.robot.Constants.ShooterConstants; -import frc.robot.Constants.ShooterConstants.ShooterModes; -import frc.robot.Constants.ShooterConstants.ShooterState; -import frc.team5431.titan.core.subsystem.CTREMechanism; -import lombok.Getter; -import lombok.Setter; - -public class Shooter extends CTREMechanism { - - @Getter @Setter private ShooterState shooterState; - @Getter @Setter private ShooterModes shooterMode; - private boolean attached; - private TalonFX motor; - - public static class ShooterConfig extends Config { - public ShooterConfig() { - super("Shooter", ShooterConstants.id, Constants.CANBUS); - configNeutralBrakeMode(ShooterConstants.breakType); - configStatorCurrentLimit(ShooterConstants.stallLimit, true); - configSupplyCurrentLimit(ShooterConstants.supplyLimit, true); - configForwardSoftLimit(ShooterConstants.maxForwardOutput, true); - configReverseSoftLimit(ShooterConstants.maxReverseOutput, true); - configPIDGains(ShooterConstants.p, ShooterConstants.i, ShooterConstants.d); - configPeakOutput(ShooterConstants.maxForwardOutput, ShooterConstants.maxReverseOutput); - configGearRatio(ShooterConstants.gearRatio); - configMotorInverted(ShooterConstants.inverted); - } - } - - public Shooter(TalonFX motor, boolean attached, ShooterConfig config) { - super(motor, attached, config); - this.attached = attached; - this.motor = motor; - this.shooterMode = ShooterModes.IDLE; - this.shooterState = ShooterState.IDLE; - - config.applyTalonConfig(motor); - } - - @Override - public void periodic() { - SmartDashboard.putString("Shooter Mode", getShooterMode().toString()); - - - switch (this.shooterMode) { - case IDLE: - setShooterState(ShooterState.IDLE); - break; - case SHOOTER: - setShooterState(ShooterState.SHOOTER); - break; - case REVERSE: - setShooterState(ShooterState.REVERSE); - break; - } - - } - - public Command runShooterCommand(ShooterModes shooterModes) { - return new RunCommand(() -> setVelocity(shooterModes.speed), this).withName("Shooter.runEnum"); - } +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.angler.AnglerIO; +import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; + +public class Shooter extends SubsystemBase { + private final AnglerIO anglerIO; + private final FlywheelIO flywheelIO; + + private final AnglerIOInputsAutoLogged anglerInputs = new AnglerIOInputsAutoLogged(); + private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + + + public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { + this.anglerIO = anglerIO; + this.flywheelIO = flywheelIO; + } + + @Override + public void periodic() { + anglerIO.updateInputs(anglerInputs); + Logger.processInputs("Shooter/Angler", anglerInputs); + + flywheelIO.updateInputs(flywheelInputs); + Logger.processInputs("Shooter/Flywheel", flywheelInputs); + // Logger.recordOutput("Intake/Mode", mode); + } + + // public void runFlywheelEnum(IntakeMode intakeMode) { + // this.mode = intakeMode; + // flywheelIO.setRPM(mode.voltage.baseUnitMagnitude()); + // } + + // public void runAnglerEnum(IntakeMode intakeMode) { + // this.mode = intakeMode; + // anglerIO.setPosition(mode.position.magnitude()); + // } + + // public Command runIntakeCommand(IntakeMode intakeMode) { + // return new RunCommand(() -> { + // this.runFlywheelEnum(intakeMode); + // this.runAnglerEnum(intakeMode); + // }, this).withName("Shooter.runIntakeEnum"); + // } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..72be9c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; + +public class ShooterConstants { + + // public enum ShooterModes { + // SHOOT( Units.RPM.of(2000)), + // IDLE( Units.RPM.of(0)); + + // public AngularVelocity speed; + + // ShooterModes(AngularVelocity speed) { + // this.speed = speed; + // } + // } + + public static class ShooterFlywheelConstants { + public static final boolean attached = true; + public static final int followerId = 50; + public static final int leaderId = 50; + public static final boolean inverted = false; + public static final boolean breakType = false; + public static final double gearRatio = 1 / 1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might + // Add later + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + + public static final Current stallLimit = Units.Amps.of(60); + public static final Current supplyLimit = Units.Amps.of(80); + + } + + public static class ShooterAnglerConstants { + public static final boolean attached = true; + public static final int id = 50; + public static final boolean inverted = false; + public static final boolean breakType = false; + public static final double gearRatio = 3.0 / 1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might + // Add later + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + + public static final Current stallLimit = Units.Amps.of(60); + public static final Current supplyLimit = Units.Amps.of(80); + // public static final double maxForwardOutput = 1; + // public static final double maxReverseOutput = 0.5; + + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(2); + + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java new file mode 100644 index 0000000..d732bc9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.shooter.angler; + +import org.littletonrobotics.junction.AutoLog; + +public interface AnglerIO { + @AutoLog + public static class AnglerIOInputs { + public boolean anglerConnected = false; + public double appliedVoltage = 0.0; + public double positionAngle = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(AnglerIOInputs inputs) {} + + /** Run the motor to the specified position. */ + public default void setPosition(double positionAngle) { + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java new file mode 100644 index 0000000..47d428d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter.angler; + +public class AnglerIOSim implements AnglerIO { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java new file mode 100644 index 0000000..f7041a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -0,0 +1,70 @@ +package frc.robot.subsystems.shooter.angler; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class AnglerIOTalonFX implements AnglerIO { + private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANBUS); + + public static class PivotTalonFXConfig extends CTREMechanism.Config { + public PivotTalonFXConfig() { + super("AnglerTalonFX", Constants.CANBUS); + configPIDGains(ShooterAnglerConstants.p, ShooterAnglerConstants.i, ShooterAnglerConstants.d); + configNeutralBrakeMode(ShooterAnglerConstants.breakType); + configFeedbackSensorSource(ShooterAnglerConstants.feedbackSensorCTRE); + configGearRatio(ShooterAnglerConstants.gearRatio); + configSupplyCurrentLimit(ShooterAnglerConstants.supplyLimit); + configForwardSoftLimit(voltageCompSaturation, false); + } + } + + + private StatusSignal appliedVoltage; + private StatusSignal pivotPosition; + private StatusSignal currentAmps; + + // No clue stole from ModuleIO + private final Debouncer anglerConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private PivotTalonFXConfig config = new PivotTalonFXConfig(); + + public AnglerIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + + } + + @Override + public void updateInputs(AnglerIOInputs inputs) { + var anglerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.anglerConnected = anglerConnectedDebounce.calculate(anglerStatus.isOK()); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.positionAngle = pivotPosition.getValue().in(Rotation); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setPosition(double positionAngle) { + PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + talon.setControl(mm); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java new file mode 100644 index 0000000..31cee28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.shooter.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + @AutoLog + public static class FlywheelIOInputs { + public boolean flywheelConnected = false; + public double leaderAppliedVoltage = 0.0; + public double leaderRPM = 0.0; + public double leaderAmps = 0.0; + + public double followerAppliedVoltage = 0.0; + public double followerRPM = 0.0; + public double followerAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FlywheelIOInputs inputs) {} + + /** Run the motor to the specified rotation per minute. */ + public default void setRPM(double rpm) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..c692a8d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.shooter.flywheel; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class FlywheelIOSim implements FlywheelIO { + // private DCMotorSim flywheelMotorSim; + // private PIDController flywheelController = new PIDController(ROLLER_KP, 0, ROLLER_KD); + // private boolean flywheelClosedLoop = false; + // private double appliedVoltage = 0.0; + // private double rollerFFVolts = 0.0; + + // // From ModuleIOSim no clue tbh + // private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + // private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); + // private static final double ROLLER_KS = 0.0; + // private static final double ROLLER_KP = 1.0; + // private static final double ROLLER_KD = 0.0; + + + // public FlywheelIOSim() { + // this.flywheelMotorSim = new DCMotorSim( + // LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + + // } + + // @Override + // public void updateInputs(FlywheelIOInputs inputs) { + // if (flywheelClosedLoop) { + // appliedVoltage = rollerFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); + // } else { + // flywheelController.reset(); + // } + + // flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + // flywheelMotorSim.update(0.02); + + // inputs.flywheelConnected = true; + // inputs.l = flywheelMotorSim.getAngularVelocityRadPerSec(); + // inputs.appliedVoltage = appliedVoltage; + // inputs.currentAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + // } + + // @Override + // public void setRPM(double RPM) { + // flywheelClosedLoop = true; + // rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; + // flywheelController.setSetpoint(RPM); + // } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 0000000..d37be19 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,89 @@ +package frc.robot.subsystems.shooter.flywheel; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class FlywheelIOTalonFX implements FlywheelIO { + private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANBUS); + private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANBUS); + + public static class FlywheelTalonFXConfig extends CTREMechanism.Config { + public FlywheelTalonFXConfig() { + super("FlywheelTalonFX", Constants.CANBUS); + configNeutralBrakeMode(ShooterFlywheelConstants.breakType); + configFeedbackSensorSource(ShooterFlywheelConstants.feedbackSensorCTRE); + configNeutralBrakeMode(ShooterFlywheelConstants.breakType); + configPIDGains(ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); + configGearRatio(ShooterFlywheelConstants.gearRatio); + configMotorInverted(ShooterFlywheelConstants.inverted); + } + } + + private StatusSignal leaderAppliedVoltage; + private StatusSignal leaderFlywheelRPM; + private StatusSignal leaderAmps; + + private StatusSignal followerAppliedVoltage; + private StatusSignal followerFlywheelRPM; + private StatusSignal followerAmps; + + // No clue stole from ModuleIO + private final Debouncer flywheelConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); + + public FlywheelIOTalonFX() { + leaderAppliedVoltage = leader.getMotorVoltage(); + leaderFlywheelRPM = leader.getVelocity(); + leaderAmps = leader.getStatorCurrent(); + + followerAppliedVoltage = follower.getMotorVoltage(); + followerFlywheelRPM = follower.getVelocity(); + followerAmps = follower.getStatorCurrent(); + + config.applyTalonConfig(leader); + config.applyTalonConfig(follower); + + // will need to config whether aligned or inverted later + follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Aligned)); + + BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + + inputs.flywheelConnected = flywheelConnectedDebounce.calculate(flywheelStatus.isOK()); + + inputs.leaderAppliedVoltage = leaderAppliedVoltage.getValueAsDouble(); + inputs.leaderRPM = leaderFlywheelRPM.getValue().in(RPM); + inputs.leaderAmps = leaderAmps.getValueAsDouble(); + + inputs.followerAppliedVoltage = followerAppliedVoltage.getValueAsDouble(); + inputs.followerRPM = followerFlywheelRPM.getValue().in(RPM); + inputs.followerAmps = followerAmps.getValueAsDouble(); + } + + @Override + public void setRPM(double rpm) { + VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); + leader.setControl(output); + } +} diff --git a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java index db26ebb..b3a94d5 100644 --- a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java +++ b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java @@ -224,7 +224,6 @@ public void toggleTorqueCurrentLimit(Current enabledLimit, boolean enabled) { public static class Config { public String title; - public int id; public TalonFXConfiguration talonConfig; public double voltageCompSaturation; // 12V by default @@ -239,13 +238,12 @@ public static class Config { public PositionTorqueCurrentFOC positionFOC = new PositionTorqueCurrentFOC(0); public VelocityTorqueCurrentFOC velocityTorqueCurrentFOC = new VelocityTorqueCurrentFOC(0); public DutyCycleOut percentOutput = new DutyCycleOut(0); // Percent Output control using percentage of supply - // voltage //Should + // voltage //Should // normally use VoltageOut - public Config(String title, int id, CANBus canbus) { + public Config(String title, CANBus canbus) { this.title = title; this.voltageCompSaturation = 12.0; - this.id = id; talonConfig = new TalonFXConfiguration(); /* Put default config settings for all mechanisms here */ @@ -284,14 +282,14 @@ public void configMotorInverted(boolean isInverted) { } } - public void configSupplyCurrentLimit(Current supplyLimit, boolean enabled) { + public void configSupplyCurrentLimit(Current supplyLimit) { talonConfig.CurrentLimits.SupplyCurrentLimit = supplyLimit.in(Units.Amps); - talonConfig.CurrentLimits.SupplyCurrentLimitEnable = enabled; + talonConfig.CurrentLimits.SupplyCurrentLimitEnable = true; } - public void configStatorCurrentLimit(Current statorLimit, boolean enabled) { + public void configStatorCurrentLimit(Current statorLimit) { talonConfig.CurrentLimits.StatorCurrentLimit = statorLimit.in(Units.Amps); - talonConfig.CurrentLimits.StatorCurrentLimitEnable = enabled; + talonConfig.CurrentLimits.StatorCurrentLimitEnable = true; } public void configForwardTorqueCurrentLimit(Current currentLimit) {