diff --git a/build.gradle b/build.gradle index d47ce0f..d2b99db 100644 --- a/build.gradle +++ b/build.gradle @@ -105,7 +105,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE @@ -123,53 +127,53 @@ tasks.withType(JavaCompile) { project.compileJava.dependsOn(createVersionFile) gversion { - srcDir = "src/main/java/" - classPackage = "frc.robot" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" // Use preferred time zone - indent = " " + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" // Use preferred time zone + indent = " " } spotless { - java { - target fileTree('.') { + java { + target fileTree('.') { include '**/*.java' exclude '**/build/**', '**/build-*/**' - } - toggleOffOn() - googleJavaFormat() - removeUnusedImports() - trimTrailingWhitespace() - endWithNewline() - } - groovyGradle { - target fileTree('.') { + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree('.') { include '**/*.gradle' exclude '**/build/**', '**/build-*/**' - } - greclipse() - indentWithSpaces(4) - trimTrailingWhitespace() - endWithNewline() - } - format 'xml', { - target fileTree('.') { + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + format 'xml', { + target fileTree('.') { include '**/*.xml' exclude '**/build/**', '**/build-*/**' - } - eclipseWtp('xml') - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } - format 'misc', { - target fileTree('.') { + } + eclipseWtp('xml') + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } + format 'misc', { + target fileTree('.') { include '**/*.md', '**/.gitignore' exclude '**/build/**', '**/build-*/**' - } - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } -} \ No newline at end of file + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index dea90db..f1fd325 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -1,8 +1,6 @@ package frc.robot; -/** - * Automatically generated file containing build version information. - */ +/** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Forte-2-5"; @@ -15,5 +13,5 @@ public final class BuildConstants { public static final long BUILD_UNIX_TIME = 1728614968375L; public static final int DIRTY = 1; - private BuildConstants(){} + private BuildConstants() {} } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a108d7a..e68e691 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,6 @@ public static enum Mode { } public static final boolean isTuning = false; - public static class RobotMap { public static class Drive { @@ -56,8 +55,8 @@ public static class Drive { public static final boolean frontLeftDriveInvert = true; public static final boolean frontRightDriveInvert = false; - public static final boolean backLeftDriveInvert = false; //true? - public static final boolean backRightDriveInvert = true; //false? + public static final boolean backLeftDriveInvert = false; // true? + public static final boolean backRightDriveInvert = true; // false? public static final boolean frontLeftTurnInvert = false; public static final boolean frontRightTurnInvert = false; @@ -74,8 +73,6 @@ public static class Drive { public static final double backLeftOffset = 1.8345500230789185; public static final double backRightOffset = 2.437732458114624; - - public static final int gyro = 10; } @@ -97,7 +94,6 @@ public static class Shooter { public static class Climb { public static final int climber = 31; } - } public static class ControlConstants { @@ -127,7 +123,7 @@ public static class DriveConstants { public static final double updateFrequency = 100; // public static final double maxLinearVelocity = Units.feetToMeters(20.4); - public static final double maxLinearVelocity = Units.feetToMeters(1.4); + public static final double maxLinearVelocity = Units.feetToMeters(1.4); public static final double maxLinearAccel = 8.0; public static final double maxAngularVelocity = 10; @@ -156,7 +152,7 @@ public static class DriveConstants { public static double kSDriveReplay = 0.0; public static double kVDriveReplay = 0.0; public static double kADriveReplay = 0.0; - + public static double kPTurnReplay = 0.0; public static double kDTurnReplay = 0.0; } @@ -193,28 +189,28 @@ public static class ClimbConstants { public static double kGSim = 0.0; public static double kVSim = 0.0; public static double kASim = 0.0; -} + } public static class IntakeConstants { public static final double pivotRatio = 25.0 * 48.0 / 44.0 * 48.0 / 24.0; public static final double pivotMOI = 0.0022842632; public static final double pivotLength = Units.inchesToMeters(9.41628595); - + public static final double pivotOffset = 1.1; public static final boolean pivotInvert = true; public static final double maxPivotVelocity = 10.6081112; - public static final double maxPivotAccel = 5; + public static final double maxPivotAccel = 5; - public static final double pivotAbsConversion = Math.PI * 2.0/((48.0/44.0)*(48.0/24.0)); + public static final double pivotAbsConversion = Math.PI * 2.0 / ((48.0 / 44.0) * (48.0 / 24.0)); public static final double pivotEncConversion = 2.0 * Math.PI / pivotRatio; public static final double rollerMOI = 0.011328; - + public static final double up = .25; public static final double down = 2.1; public static final double simOffset = 1.27838411; - + public static final int pivotCurrentLimit = 30; public static final int rollerCurrentLimit = 70; @@ -224,7 +220,7 @@ public static class IntakeConstants { public static final double kVRoller = 0.0029; public static final double kARoller = 0; - + public static double kPPivotReal = .7; public static double kPRollerReal = 0.0000; @@ -238,7 +234,7 @@ public static class IntakeConstants { public static double kPPivotReplay = 0.3; public static double kPRollerReplay = 10; - public static double kSRollerReplay = 0.0; + public static double kSRollerReplay = 0.0; } public static class FeederConstants { @@ -265,7 +261,7 @@ public static class ShooterConstants { // public static final double pivotMOI = .0001; public static final double maxPivotVelocity = 20; - public static final double maxPivotAccel = 5; + public static final double maxPivotAccel = 5; public static final double pivotAbsConversion = Math.PI * 2.0; public static final double pivotEncConversion = 2.0 * Math.PI / pivotRatio; @@ -275,8 +271,8 @@ public static class ShooterConstants { public static final double stallTimeout = 0.0; public static final double down = 0.01; - public static final double up = down + Math.PI/4; - + public static final double up = down + Math.PI / 4; + public static final double shooterMOI = 0.00920287973; public static final int pivotCurrentLimit = 25; @@ -284,7 +280,7 @@ public static class ShooterConstants { public static final double kGPivot = 0.381640625; public static final double kVPivot = 0.875; public static final double kAPivot = 0.00; - + public static final double kVShooter = 0.0055; // COMP: 0.0055, DEMO: 0.0041875 public static final double kAShooter = 0.00; @@ -293,15 +289,16 @@ public static class ShooterConstants { public static final double kSShooterReal = 0; public static final double kPPivot = 5.0; - + public static final double kPShooterSim = 0.5; public static final double kIShooterSim = 0.0; public static final double kSShooterSim = 0.5; - + public static final double kPShooterReplay = 0.0; public static final double kIShooterReplay = 0.0; public static final double kSShooterReplay = 0.0; } + public static class SimConstants { public static final double loopTime = 0.02; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b254a45..b900136 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -157,4 +157,4 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} -} \ 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 ded3f4b..0d0aabe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,12 +20,9 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotMap; -import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.climber.Climb; import frc.robot.subsystems.climber.ClimbIOReplay; import frc.robot.subsystems.climber.ClimbIOSim; @@ -57,221 +54,232 @@ import frc.robot.subsystems.shooter.ShooterIOSparkMax; /** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in - * the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of - * the robot (including + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Subsystems - // private final SwerveSubsystem m_drive = new SwerveSubsystem(new - // File(Filesystem.getDeployDirectory(), - // "swerve/swerve"));; - private final Drive m_drive; - private final Climb m_climber; - private final Intake m_intake; - private final Feeder m_feeder; - private final Pivot m_pivot; - private final Shooter m_shooter; - private final Visualizer m_visualizer; + // Subsystems + // private final SwerveSubsystem m_drive = new SwerveSubsystem(new + // File(Filesystem.getDeployDirectory(), + // "swerve/swerve"));; + private final Drive m_drive; + private final Climb m_climber; + private final Intake m_intake; + private final Feeder m_feeder; + private final Pivot m_pivot; + private final Shooter m_shooter; + private final Visualizer m_visualizer; - // Controller - private final CommandXboxController m_driver = new CommandXboxController(0); - private final CommandXboxController m_operator = new CommandXboxController(1); + // Controller + private final CommandXboxController m_driver = new CommandXboxController(0); + private final CommandXboxController m_operator = new CommandXboxController(1); - // final CommandXboxController driverXbox = new CommandXboxController(0); - // private final SwerveSubsystem drivebase + // final CommandXboxController driverXbox = new CommandXboxController(0); + // private final SwerveSubsystem drivebase - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - m_drive = new Drive( - new GyroIOPigeon2Phoenix6(), - new ModuleIOReal(0), - new ModuleIOReal(1), - new ModuleIOReal(2), - new ModuleIOReal(3)); - m_climber = new Climb(new ClimbIOSparkMax()); - m_intake = new Intake(new IntakeIOSparkMax()); - m_feeder = new Feeder(new FeederIOSparkMax(), - new BeambreakIOReal(RobotMap.Shooter.feederBeambreak), - new BeambreakIOReal(RobotMap.Shooter.shooterBeambreak)); - m_pivot = new Pivot(new PivotIOSparkMax()); - m_shooter = new Shooter(new ShooterIOSparkMax()); - break; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + m_drive = + new Drive( + new GyroIOPigeon2Phoenix6(), + new ModuleIOReal(0), + new ModuleIOReal(1), + new ModuleIOReal(2), + new ModuleIOReal(3)); + m_climber = new Climb(new ClimbIOSparkMax()); + m_intake = new Intake(new IntakeIOSparkMax()); + m_feeder = + new Feeder( + new FeederIOSparkMax(), + new BeambreakIOReal(RobotMap.Shooter.feederBeambreak), + new BeambreakIOReal(RobotMap.Shooter.shooterBeambreak)); + m_pivot = new Pivot(new PivotIOSparkMax()); + m_shooter = new Shooter(new ShooterIOSparkMax()); + break; - case SIM: - // Sim robot, instantiate physics sim IO implementations - m_drive = new Drive( - new GyroIOReplay() { - }, - new ModuleIOSim("FrontLeft"), - new ModuleIOSim("FrontRight"), - new ModuleIOSim("BackLeft"), - new ModuleIOSim("BackRight")); - m_climber = new Climb(new ClimbIOSim()); - m_intake = new Intake(new IntakeIOSim()); - m_feeder = new Feeder(new FeederIOSim(), - new BeambreakIOSim(RobotMap.Shooter.feederBeambreak), - new BeambreakIOSim(RobotMap.Shooter.shooterBeambreak)); - m_pivot = new Pivot(new PivotIOSim()); - m_shooter = new Shooter(new ShooterIOSim()); - break; + case SIM: + // Sim robot, instantiate physics sim IO implementations + m_drive = + new Drive( + new GyroIOReplay() {}, + new ModuleIOSim("FrontLeft"), + new ModuleIOSim("FrontRight"), + new ModuleIOSim("BackLeft"), + new ModuleIOSim("BackRight")); + m_climber = new Climb(new ClimbIOSim()); + m_intake = new Intake(new IntakeIOSim()); + m_feeder = + new Feeder( + new FeederIOSim(), + new BeambreakIOSim(RobotMap.Shooter.feederBeambreak), + new BeambreakIOSim(RobotMap.Shooter.shooterBeambreak)); + m_pivot = new Pivot(new PivotIOSim()); + m_shooter = new Shooter(new ShooterIOSim()); + break; - default: - // Replayed robot, disable IO implementations - m_drive = new Drive( - new GyroIOReplay() { - }, - new ModuleIOReplay() { - }, - new ModuleIOReplay() { - }, - new ModuleIOReplay() { - }, - new ModuleIOReplay() { - }); - m_climber = new Climb(new ClimbIOReplay()); - m_intake = new Intake(new IntakeIOReplay()); - m_feeder = new Feeder(new FeederIOReplay(), new BeambreakIOReplay(), - new BeambreakIOReplay()); - m_pivot = new Pivot(new PivotIOReplay()); - m_shooter = new Shooter(new ShooterIOReplay()); - break; + default: + // Replayed robot, disable IO implementations + m_drive = + new Drive( + new GyroIOReplay() {}, + new ModuleIOReplay() {}, + new ModuleIOReplay() {}, + new ModuleIOReplay() {}, + new ModuleIOReplay() {}); + m_climber = new Climb(new ClimbIOReplay()); + m_intake = new Intake(new IntakeIOReplay()); + m_feeder = + new Feeder(new FeederIOReplay(), new BeambreakIOReplay(), new BeambreakIOReplay()); + m_pivot = new Pivot(new PivotIOReplay()); + m_shooter = new Shooter(new ShooterIOReplay()); + break; + } + m_visualizer = new Visualizer(m_climber, m_intake, m_pivot); + // Configure the button bindings + configureButtonBindings(); + } - } - m_visualizer = new Visualizer(m_climber, m_intake, m_pivot); - // Configure the button bindings - configureButtonBindings(); - } + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { - /** - * Use this method to define your button->command mappings. Buttons can be - * created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing - * it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { + // Driver Controller + m_drive.setDefaultCommand( + m_drive.runVoltageTeleopFieldRelative( + () -> + new ChassisSpeeds( + -teleopAxisAdjustment(m_driver.getLeftY()) * DriveConstants.maxLinearVelocity, + -teleopAxisAdjustment(m_driver.getLeftX()) * DriveConstants.maxLinearVelocity, + -teleopAxisAdjustment(m_driver.getRightX()) + * DriveConstants.maxLinearVelocity))); - // Driver Controller - m_drive.setDefaultCommand( - m_drive.runVoltageTeleopFieldRelative( - () -> new ChassisSpeeds( - -teleopAxisAdjustment(m_driver.getLeftY()) * - DriveConstants.maxLinearVelocity, - -teleopAxisAdjustment(m_driver.getLeftX()) * - DriveConstants.maxLinearVelocity, - -teleopAxisAdjustment(m_driver.getRightX()) - * DriveConstants.maxLinearVelocity))); + m_intake.setDefaultCommand(m_intake.setIntakeUp()); - m_intake.setDefaultCommand( - m_intake.setIntakeUp()); + // left trigger -> climb up + m_driver.leftTrigger(0.1).onTrue(m_climber.setDutyCycle(-1)).onFalse(m_climber.setDutyCycle(0)); - // left trigger -> climb up - m_driver.leftTrigger(0.1).onTrue( - m_climber.setDutyCycle(-1)).onFalse( - m_climber.setDutyCycle(0)); + // right trigger -> climb up + m_driver.rightTrigger(0.1).onTrue(m_climber.setDutyCycle(1)).onFalse(m_climber.setDutyCycle(0)); - // right trigger -> climb up - m_driver.rightTrigger(0.1).onTrue( - m_climber.setDutyCycle(1)).onFalse( - m_climber.setDutyCycle(0)); + // Operator Controller - // Operator Controller + // D-Pad Up for intake down, rollers forward, until note in feeder beambreak + m_operator + .povUp() + .whileTrue( + Commands.parallel(m_intake.setIntakeDown(false), m_feeder.setRPM(() -> 2500)) + .until(() -> m_feeder.feederBeambreakObstructed())) + .onFalse(m_intake.setIntakeUp()); - // D-Pad Up for intake down, rollers forward, until note in feeder beambreak - m_operator.povUp().whileTrue( - Commands.parallel( - m_intake.setIntakeDown(false), - m_feeder.setRPM(() -> 2500)) - .until(() -> m_feeder.feederBeambreakObstructed())) - .onFalse(m_intake.setIntakeUp()); + // D-Pad Down for intake down, rollers backward + m_operator + .povDown() + .whileTrue(Commands.parallel(m_intake.setIntakeDown(true), m_feeder.setRPM(() -> -3000))) + .onFalse(m_intake.setIntakeUp()); - // D-Pad Down for intake down, rollers backward - m_operator.povDown().whileTrue( - Commands.parallel( - m_intake.setIntakeDown(true), - m_feeder.setRPM(() -> -3000))) - .onFalse(m_intake.setIntakeUp()); + // Y for shooter at subwoofer + m_operator + .y() + .whileTrue( + Commands.parallel( + m_pivot.setPivotTarget(() -> Units.degreesToRadians(45)), + m_shooter.setRPM(() -> 5800, 0.3)) + .until(() -> m_shooter.atSetpoint()) + .andThen( + m_feeder + .setRPM(() -> 3000) + .until( + () -> + (!m_feeder.feederBeambreakObstructed() + && !m_feeder.shooterBeambreakObstructed())))); - // Y for shooter at subwoofer - m_operator.y().whileTrue( - Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(45)), - m_shooter.setRPM(() -> 5800, 0.3)).until(() -> m_shooter.atSetpoint()) - .andThen(m_feeder.setRPM(() -> 3000) - .until(() -> (!m_feeder.feederBeambreakObstructed() - && !m_feeder.shooterBeambreakObstructed())))); + // X for shooter at amp + m_operator + .leftBumper() + .whileTrue( + Commands.parallel( + m_feeder.setVoltage(() -> 0), + m_shooter.stopShooter(), + Commands.sequence(m_pivot.setPivotVoltage(() -> -1))) + .until(() -> m_pivot.atSetpoint() || m_pivot.isStalled()) + .andThen( + Commands.either( + m_pivot.resetEncoder(), m_pivot.runZero(), () -> m_pivot.isStalled()) + .andThen( + m_pivot + .setPivotTarget(() -> m_pivot.getAngleRadians()) + .andThen(m_pivot.setPivotVoltage(() -> 0))))); - // X for shooter at amp - m_operator.leftBumper().whileTrue( - Commands.parallel( - m_feeder.setVoltage(() -> 0), - m_shooter.stopShooter(), - Commands.sequence(m_pivot.setPivotVoltage(() -> -1))) - .until(() -> m_pivot.atSetpoint() - || m_pivot.isStalled()) - .andThen( - Commands.either(m_pivot.resetEncoder(), - m_pivot.runZero(), - () -> m_pivot.isStalled()).andThen(m_pivot.setPivotTarget(() -> m_pivot.getAngleRadians()).andThen( - m_pivot.setPivotVoltage(() -> 0)) - ))); + // m_operator.rightBumper().whileTrue( + // m_pivot.runCurrentZeroing() + // ); - // m_operator.rightBumper().whileTrue( - // m_pivot.runCurrentZeroing() - // ); + // B for shooter at podium or feeding + m_operator + .b() + .whileTrue( + Commands.parallel( + m_pivot.setPivotTarget(() -> Units.degreesToRadians(20)), + m_shooter.setRPM(() -> 5800, 1.0)) + .until(() -> m_shooter.atSetpoint()) + .andThen( + m_feeder + .setRPM(() -> 2000) + .until( + () -> + (!m_feeder.feederBeambreakObstructed() + && !m_feeder.shooterBeambreakObstructed())))); - // B for shooter at podium or feeding - m_operator.b().whileTrue( - Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(20)), - m_shooter.setRPM(() -> 5800, 1.0)).until(() -> m_shooter.atSetpoint()) - .andThen(m_feeder.setRPM(() -> 2000) - .until(() -> (!m_feeder.feederBeambreakObstructed() - && !m_feeder.shooterBeambreakObstructed())))); + // A for shooter at source + m_operator + .a() + .whileTrue( + Commands.parallel( + m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)), + m_shooter.setRPM(() -> -1500, 1.0), + m_feeder.setRPM(() -> -1500), + m_intake.setRollerRPM(() -> -1000)) + .until( + () -> + (m_feeder.feederBeambreakObstructed() + && !m_feeder.shooterBeambreakObstructed())) + .andThen( + Commands.parallel( + m_shooter.stopShooter(), + m_pivot.setPivotTarget(() -> Units.degreesToRadians(0.0))))); - // A for shooter at source - m_operator.a().whileTrue( - Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)), - m_shooter.setRPM(() -> -1500, 1.0), - m_feeder.setRPM(() -> -1500), - m_intake.setRollerRPM(() -> -1000)) - .until(() -> (m_feeder.feederBeambreakObstructed() - && !m_feeder.shooterBeambreakObstructed())) - .andThen(Commands.parallel(m_shooter.stopShooter(), - m_pivot.setPivotTarget( - () -> Units.degreesToRadians(0.0))))); + m_operator + .leftTrigger(0.1) + .onTrue( + Commands.parallel( + m_shooter.setRPM(() -> 5800, 0.3), + m_feeder.setRPM(() -> 2000), + m_intake.setRollerRPM(() -> 2000))) + .onFalse( + m_shooter + .stopShooter() + .andThen(m_pivot.setPivotTarget(() -> Units.degreesToRadians(0)))); + } - m_operator.leftTrigger(0.1).onTrue( - Commands.parallel(m_shooter.setRPM(() -> 5800, 0.3), - m_feeder.setRPM(() -> 2000), - m_intake.setRollerRPM(() -> 2000))) - .onFalse(m_shooter.stopShooter() - .andThen(m_pivot.setPivotTarget(() -> Units.degreesToRadians(0)))); + public void robotPeriodic() { + m_visualizer.periodic(); + } - } + private static double teleopAxisAdjustment(double x) { + return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02); + } - public void robotPeriodic() { - m_visualizer.periodic(); - } - - private static double teleopAxisAdjustment(double x) { - return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02); - } - - public Command getAutonomousCommand() { - return null; - } -} \ No newline at end of file + public Command getAutonomousCommand() { + return null; + } +} diff --git a/src/main/java/frc/robot/Visualizer.java b/src/main/java/frc/robot/Visualizer.java index a7a68f1..eb1cd79 100644 --- a/src/main/java/frc/robot/Visualizer.java +++ b/src/main/java/frc/robot/Visualizer.java @@ -1,7 +1,5 @@ package frc.robot; -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.util.Units; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -17,74 +15,124 @@ import frc.robot.subsystems.climber.Climb; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.pivot2.Pivot; +import org.littletonrobotics.junction.Logger; public class Visualizer extends SubsystemBase { - private Mechanism2d m_main; - - private MechanismLigament2d m_climberMech; - private MechanismLigament2d m_intakeMech; - private MechanismLigament2d m_pivotMech; - - private MechanismLigament2d m_climberTarget; - private MechanismLigament2d m_intakeTarget; - private MechanismLigament2d m_pivotTarget; - private MechanismLigament2d m_pivotRelative; - - private MechanismRoot2d m_climberRoot; - private MechanismRoot2d m_intakeRoot; - private MechanismRoot2d m_pivotRoot; - - private Climb m_climber; - private Intake m_intake; - private Pivot m_pivot; - - public Visualizer(Climb climber, Intake intake, Pivot pivot) { - m_climber = climber; - m_intake = intake; - m_pivot = pivot; - - m_main = new Mechanism2d(Units.inchesToMeters(48.0), Units.inchesToMeters(48.0)); - m_climberRoot = m_main.getRoot("climber_base", Units.inchesToMeters(24+3.0), Units.inchesToMeters(3.75)); - m_intakeRoot = m_main.getRoot("intake_pivot", Units.inchesToMeters(24-8.835736), Units.inchesToMeters(10.776049)); - m_pivotRoot = m_main.getRoot("pivot_pivot", Units.inchesToMeters(24-3.485625), Units.inchesToMeters(11.5)); - m_main.getRoot("Robot", Units.inchesToMeters(24-(26.5/2)), Units.inchesToMeters(7)).append( - new MechanismLigament2d("frame", Units.inchesToMeters(26+7.5), 0, 15, new Color8Bit(Color.kYellow)) - ); - - m_climberMech = m_climberRoot.append(new MechanismLigament2d("Climb", Units.inchesToMeters(17.25), 90, 8, new Color8Bit(Color.kFirstRed))); - m_climberTarget = m_climberRoot.append(new MechanismLigament2d("Climb Target", Units.inchesToMeters(17.25), 90, 2, new Color8Bit(Color.kRed))); - - m_intakeMech = m_intakeRoot.append(new MechanismLigament2d("Intake", Units.inchesToMeters(14.914264), 83.649627, 8, new Color8Bit(Color.kFirstBlue))); - m_intakeTarget = m_intakeRoot.append(new MechanismLigament2d("Intake Target", Units.inchesToMeters(14.914264), 83.649627, 2, new Color8Bit(Color.kBlue))); - - m_pivotMech = m_pivotRoot.append(new MechanismLigament2d("Shooter", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kWhite))); - m_pivotRelative = m_pivotRoot.append(new MechanismLigament2d("Shooter Relative", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kGray))); - m_pivotTarget = m_pivotRoot.append(new MechanismLigament2d("Shooter Target", Units.inchesToMeters(13.1001837), 12, 2, new Color8Bit(Color.kBeige))); - - SmartDashboard.putData("Climb Up", (Sendable) m_climber.setExtensionCmd(() -> ClimbConstants.maxHeight)); - SmartDashboard.putData("Climb Down", (Sendable) m_climber.setExtensionCmd(() -> 0)); - - SmartDashboard.putData("Intake Up", (Sendable) m_intake.setPivotTarget(() -> IntakeConstants.up)); - SmartDashboard.putData("Intake Down", (Sendable) m_intake.setPivotTarget(() -> IntakeConstants.down)); - - SmartDashboard.putData("Shooter 0", (Sendable) m_pivot.setPivotTarget(() -> ShooterConstants.down)); - SmartDashboard.putData("Shooter 56", (Sendable) m_pivot.setPivotTarget(() -> ShooterConstants.up)); - - SmartDashboard.putData("Pos", (Sendable) m_pivot.setPivotVoltage(() -> 5)); - SmartDashboard.putData("NegPos", (Sendable) m_pivot.setPivotVoltage(() -> -5)); - } - - public void periodic() { - m_climberMech.setLength((m_climber.getExtensionMeters()) + Units.inchesToMeters(13.5)); - m_climberTarget.setLength((m_climber.getTargetMeters()) + Units.inchesToMeters(13.5)); - - m_intakeMech.setAngle(Units.radiansToDegrees(m_intake.getAngleRadians())); - m_intakeTarget.setAngle(Units.radiansToDegrees(m_intake.getTargetRadians())); - - m_pivotMech.setAngle(Units.radiansToDegrees(m_pivot.getAngleRadians())); - m_pivotRelative.setAngle(Units.radiansToDegrees(m_pivot.getRelativeRadians())); - m_pivotTarget.setAngle(Units.radiansToDegrees(m_pivot.getTargetRadians())); - - Logger.recordOutput("Visualizer/FullRobot", m_main); - } + private Mechanism2d m_main; + + private MechanismLigament2d m_climberMech; + private MechanismLigament2d m_intakeMech; + private MechanismLigament2d m_pivotMech; + + private MechanismLigament2d m_climberTarget; + private MechanismLigament2d m_intakeTarget; + private MechanismLigament2d m_pivotTarget; + private MechanismLigament2d m_pivotRelative; + + private MechanismRoot2d m_climberRoot; + private MechanismRoot2d m_intakeRoot; + private MechanismRoot2d m_pivotRoot; + + private Climb m_climber; + private Intake m_intake; + private Pivot m_pivot; + + public Visualizer(Climb climber, Intake intake, Pivot pivot) { + m_climber = climber; + m_intake = intake; + m_pivot = pivot; + + m_main = new Mechanism2d(Units.inchesToMeters(48.0), Units.inchesToMeters(48.0)); + m_climberRoot = + m_main.getRoot("climber_base", Units.inchesToMeters(24 + 3.0), Units.inchesToMeters(3.75)); + m_intakeRoot = + m_main.getRoot( + "intake_pivot", Units.inchesToMeters(24 - 8.835736), Units.inchesToMeters(10.776049)); + m_pivotRoot = + m_main.getRoot( + "pivot_pivot", Units.inchesToMeters(24 - 3.485625), Units.inchesToMeters(11.5)); + m_main + .getRoot("Robot", Units.inchesToMeters(24 - (26.5 / 2)), Units.inchesToMeters(7)) + .append( + new MechanismLigament2d( + "frame", Units.inchesToMeters(26 + 7.5), 0, 15, new Color8Bit(Color.kYellow))); + + m_climberMech = + m_climberRoot.append( + new MechanismLigament2d( + "Climb", Units.inchesToMeters(17.25), 90, 8, new Color8Bit(Color.kFirstRed))); + m_climberTarget = + m_climberRoot.append( + new MechanismLigament2d( + "Climb Target", Units.inchesToMeters(17.25), 90, 2, new Color8Bit(Color.kRed))); + + m_intakeMech = + m_intakeRoot.append( + new MechanismLigament2d( + "Intake", + Units.inchesToMeters(14.914264), + 83.649627, + 8, + new Color8Bit(Color.kFirstBlue))); + m_intakeTarget = + m_intakeRoot.append( + new MechanismLigament2d( + "Intake Target", + Units.inchesToMeters(14.914264), + 83.649627, + 2, + new Color8Bit(Color.kBlue))); + + m_pivotMech = + m_pivotRoot.append( + new MechanismLigament2d( + "Shooter", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kWhite))); + m_pivotRelative = + m_pivotRoot.append( + new MechanismLigament2d( + "Shooter Relative", + Units.inchesToMeters(13.1001837), + 12, + 8, + new Color8Bit(Color.kGray))); + m_pivotTarget = + m_pivotRoot.append( + new MechanismLigament2d( + "Shooter Target", + Units.inchesToMeters(13.1001837), + 12, + 2, + new Color8Bit(Color.kBeige))); + + SmartDashboard.putData( + "Climb Up", (Sendable) m_climber.setExtensionCmd(() -> ClimbConstants.maxHeight)); + SmartDashboard.putData("Climb Down", (Sendable) m_climber.setExtensionCmd(() -> 0)); + + SmartDashboard.putData( + "Intake Up", (Sendable) m_intake.setPivotTarget(() -> IntakeConstants.up)); + SmartDashboard.putData( + "Intake Down", (Sendable) m_intake.setPivotTarget(() -> IntakeConstants.down)); + + SmartDashboard.putData( + "Shooter 0", (Sendable) m_pivot.setPivotTarget(() -> ShooterConstants.down)); + SmartDashboard.putData( + "Shooter 56", (Sendable) m_pivot.setPivotTarget(() -> ShooterConstants.up)); + + SmartDashboard.putData("Pos", (Sendable) m_pivot.setPivotVoltage(() -> 5)); + SmartDashboard.putData("NegPos", (Sendable) m_pivot.setPivotVoltage(() -> -5)); + } + + public void periodic() { + m_climberMech.setLength((m_climber.getExtensionMeters()) + Units.inchesToMeters(13.5)); + m_climberTarget.setLength((m_climber.getTargetMeters()) + Units.inchesToMeters(13.5)); + + m_intakeMech.setAngle(Units.radiansToDegrees(m_intake.getAngleRadians())); + m_intakeTarget.setAngle(Units.radiansToDegrees(m_intake.getTargetRadians())); + + m_pivotMech.setAngle(Units.radiansToDegrees(m_pivot.getAngleRadians())); + m_pivotRelative.setAngle(Units.radiansToDegrees(m_pivot.getRelativeRadians())); + m_pivotTarget.setAngle(Units.radiansToDegrees(m_pivot.getTargetRadians())); + + Logger.recordOutput("Visualizer/FullRobot", m_main); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climb.java b/src/main/java/frc/robot/subsystems/climber/Climb.java index f550234..a660e67 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climb.java +++ b/src/main/java/frc/robot/subsystems/climber/Climb.java @@ -1,119 +1,111 @@ package frc.robot.subsystems.climber; -import java.util.function.DoubleSupplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.ClimbConstants; import frc.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; public class Climb extends SubsystemBase { - private final ClimbIO io; - private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); - - private LoggedTunableNumber kP = new LoggedTunableNumber("Climb/kP"); - private LoggedTunableNumber kI = new LoggedTunableNumber("Climb/kI"); - private LoggedTunableNumber kD = new LoggedTunableNumber("Climb/kD"); - - private LoggedTunableNumber kS = new LoggedTunableNumber("Climb/kS"); - private LoggedTunableNumber kG = new LoggedTunableNumber("Climb/kG"); - private LoggedTunableNumber kV = new LoggedTunableNumber("Climb/kV"); - private LoggedTunableNumber kA = new LoggedTunableNumber("Climb/kA"); - private LoggedTunableNumber kFF = new LoggedTunableNumber("Climb/kFF"); - - public Climb(ClimbIO io) { - this.io = io; - - switch (Constants.currentMode) { - case REAL: - kP.initDefault(ClimbConstants.kPReal); - kI.initDefault(ClimbConstants.kIReal); - kD.initDefault(ClimbConstants.kDReal); - - kFF.initDefault(ClimbConstants.kFFReal); - io.setSimpleFF(kFF.get()); - break; - - case REPLAY: - kP.initDefault(ClimbConstants.kPReplay); - kI.initDefault(ClimbConstants.kIReplay); - kD.initDefault(ClimbConstants.kDReplay); - - kFF.initDefault(ClimbConstants.kFFReal); - break; - - case SIM: - kP.initDefault(ClimbConstants.kPSim); - kI.initDefault(ClimbConstants.kISim); - kD.initDefault(ClimbConstants.kDSim); - - kS.initDefault(ClimbConstants.kSSim); - kG.initDefault(ClimbConstants.kGSim); - kV.initDefault(ClimbConstants.kVSim); - kA.initDefault(ClimbConstants.kASim); - io.setFF(kS.get(), kG.get(), kV.get(), kA.get()); - break; - - default: - kP.initDefault(0.0); - kI.initDefault(0.0); - kD.initDefault(0.0); - - kFF.initDefault(0.0); - break; - } - - io.setPID(kP.get(), kI.get(), kD.get()); - - } - - @Override - public void periodic() { - io.processInputs(inputs); - Logger.processInputs("Climb", inputs); - - LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); - LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setSimpleFF(kFF.get()), kFF); - } - - public void setTargetMeters(double meters) { - io.setTargetMeters(meters); - inputs.climberTargetMeters = meters; + private final ClimbIO io; + private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + + private LoggedTunableNumber kP = new LoggedTunableNumber("Climb/kP"); + private LoggedTunableNumber kI = new LoggedTunableNumber("Climb/kI"); + private LoggedTunableNumber kD = new LoggedTunableNumber("Climb/kD"); + + private LoggedTunableNumber kS = new LoggedTunableNumber("Climb/kS"); + private LoggedTunableNumber kG = new LoggedTunableNumber("Climb/kG"); + private LoggedTunableNumber kV = new LoggedTunableNumber("Climb/kV"); + private LoggedTunableNumber kA = new LoggedTunableNumber("Climb/kA"); + private LoggedTunableNumber kFF = new LoggedTunableNumber("Climb/kFF"); + + public Climb(ClimbIO io) { + this.io = io; + + switch (Constants.currentMode) { + case REAL: + kP.initDefault(ClimbConstants.kPReal); + kI.initDefault(ClimbConstants.kIReal); + kD.initDefault(ClimbConstants.kDReal); + + kFF.initDefault(ClimbConstants.kFFReal); + io.setSimpleFF(kFF.get()); + break; + + case REPLAY: + kP.initDefault(ClimbConstants.kPReplay); + kI.initDefault(ClimbConstants.kIReplay); + kD.initDefault(ClimbConstants.kDReplay); + + kFF.initDefault(ClimbConstants.kFFReal); + break; + + case SIM: + kP.initDefault(ClimbConstants.kPSim); + kI.initDefault(ClimbConstants.kISim); + kD.initDefault(ClimbConstants.kDSim); + + kS.initDefault(ClimbConstants.kSSim); + kG.initDefault(ClimbConstants.kGSim); + kV.initDefault(ClimbConstants.kVSim); + kA.initDefault(ClimbConstants.kASim); + io.setFF(kS.get(), kG.get(), kV.get(), kA.get()); + break; + + default: + kP.initDefault(0.0); + kI.initDefault(0.0); + kD.initDefault(0.0); + + kFF.initDefault(0.0); + break; } - - public Command setDutyCycle(double dutyCycle) { - return run(() -> { - io.setOpenLoopDutyCycle(dutyCycle); - inputs.climberTargetSpeed = dutyCycle; + io.setPID(kP.get(), kI.get(), kD.get()); + } + + @Override + public void periodic() { + io.processInputs(inputs); + Logger.processInputs("Climb", inputs); + + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); + LoggedTunableNumber.ifChanged(hashCode(), () -> io.setSimpleFF(kFF.get()), kFF); + } + + public void setTargetMeters(double meters) { + io.setTargetMeters(meters); + inputs.climberTargetMeters = meters; + } + + public Command setDutyCycle(double dutyCycle) { + return run( + () -> { + io.setOpenLoopDutyCycle(dutyCycle); + inputs.climberTargetSpeed = dutyCycle; }); - } - - public Command setExtensionCmd(DoubleSupplier meters) { - return run(() -> setTargetMeters(meters.getAsDouble())); - } - - public Command runCurrentHoming() { - return Commands.runOnce( - () -> io.setVoltage(-1.0) - ).until( - () -> inputs.climberCurrentAmps > 40.0 - ).finallyDo( - () -> io.resetEncoder(0.0) - ); - } - - public double getExtensionMeters() { - return inputs.climberPositionMeters; - } - - public double getTargetMeters() { - return inputs.climberTargetMeters; - } -} \ No newline at end of file + } + + public Command setExtensionCmd(DoubleSupplier meters) { + return run(() -> setTargetMeters(meters.getAsDouble())); + } + + public Command runCurrentHoming() { + return Commands.runOnce(() -> io.setVoltage(-1.0)) + .until(() -> inputs.climberCurrentAmps > 40.0) + .finallyDo(() -> io.resetEncoder(0.0)); + } + + public double getExtensionMeters() { + return inputs.climberPositionMeters; + } + + public double getTargetMeters() { + return inputs.climberTargetMeters; + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimbIO.java b/src/main/java/frc/robot/subsystems/climber/ClimbIO.java index 33f8fa7..87d1271 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimbIO.java @@ -4,35 +4,34 @@ /** Add your docs here. */ public interface ClimbIO { - @AutoLog - public static class ClimbIOInputs { - public double climberPositionMeters = 0.0; - public double climberTargetMeters = 0.0; - public double climberVelocityMetersPerSecond = 0.0; - public double climberAppliedVolts = 0.0; - public double climberCurrentAmps = 0.0; - public double climberTempCelsius = 0.0; - public double climberTargetSpeed = 0.0; - } + @AutoLog + public static class ClimbIOInputs { + public double climberPositionMeters = 0.0; + public double climberTargetMeters = 0.0; + public double climberVelocityMetersPerSecond = 0.0; + public double climberAppliedVolts = 0.0; + public double climberCurrentAmps = 0.0; + public double climberTempCelsius = 0.0; + public double climberTargetSpeed = 0.0; + } - public abstract void processInputs(final ClimbIOInputsAutoLogged inputs); + public abstract void processInputs(final ClimbIOInputsAutoLogged inputs); - public abstract void setTargetMeters(final double meters); + public abstract void setTargetMeters(final double meters); - public abstract void setVoltage(final double voltage); + public abstract void setVoltage(final double voltage); - public abstract void setOpenLoopDutyCycle(final double dutyCycle); + public abstract void setOpenLoopDutyCycle(final double dutyCycle); - public abstract void stop(); + public abstract void stop(); - public abstract void setPID(double kP, double kI, double kD); + public abstract void setPID(double kP, double kI, double kD); - public void setSimpleFF(double kFF); + public void setSimpleFF(double kFF); - public void setFF(double kS, double kG, double kV, double kA); + public void setFF(double kS, double kG, double kV, double kA); - public abstract void resetEncoder(final double position); + public abstract void resetEncoder(final double position); - public abstract void resetEncoder(); - -} \ No newline at end of file + public abstract void resetEncoder(); +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimbIOReplay.java b/src/main/java/frc/robot/subsystems/climber/ClimbIOReplay.java index ab7aa1c..fa8c763 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimbIOReplay.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimbIOReplay.java @@ -1,31 +1,31 @@ package frc.robot.subsystems.climber; public class ClimbIOReplay implements ClimbIO { - public void processInputs(final ClimbIOInputsAutoLogged inputs) {} + public void processInputs(final ClimbIOInputsAutoLogged inputs) {} - public void setTargetMeters(final double meters) {} + public void setTargetMeters(final double meters) {} - public void setVoltage(final double voltage) {} + public void setVoltage(final double voltage) {} - public void setOpenLoopDutyCycle(final double dutyCycle) {} + public void setOpenLoopDutyCycle(final double dutyCycle) {} - public void stop() { - setVoltage(0); - } + public void stop() { + setVoltage(0); + } - public void setPID(double kP, double kI, double kD) {} + public void setPID(double kP, double kI, double kD) {} - public void setFF(double kFF) {} + public void setFF(double kFF) {} - public void resetEncoder(final double position) {} + public void resetEncoder(final double position) {} - public void resetEncoder() { - resetEncoder(0.0); - } + public void resetEncoder() { + resetEncoder(0.0); + } - @Override - public void setSimpleFF(double kFF) {} + @Override + public void setSimpleFF(double kFF) {} - @Override - public void setFF(double kS, double kG, double kV, double kA) {} + @Override + public void setFF(double kS, double kG, double kV, double kA) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimbIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimbIOSim.java index 424b4b4..4d440af 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimbIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimbIOSim.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.climber; +import static frc.robot.Constants.*; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -8,81 +10,76 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import static frc.robot.Constants.*; - public class ClimbIOSim implements ClimbIO { - private ElevatorSim sim = new ElevatorSim( - DCMotor.getNEO(1), - ClimbConstants.gearRatio, - 1, - ClimbConstants.spoolRadius, - ClimbConstants.minHeight, - ClimbConstants.maxHeight, - true, - 0 - ); - - private double appliedVolts = 0.0; - private ProfiledPIDController pid = new ProfiledPIDController( - ClimbConstants.kPSim, - ClimbConstants.kISim, - ClimbConstants.kDSim, - new Constraints(ClimbConstants.maxVelocity, ClimbConstants.maxAccel) - ); - - private ElevatorFeedforward ff = new ElevatorFeedforward( - ClimbConstants.kSSim, - ClimbConstants.kGSim, - ClimbConstants.kVSim, - ClimbConstants.kASim - ); - - @Override - public void processInputs(final ClimbIOInputsAutoLogged inputs) { - if (DriverStation.isDisabled()) { - stop(); - } - - sim.update(0.02); - inputs.climberPositionMeters = sim.getPositionMeters(); - inputs.climberVelocityMetersPerSecond = sim.getVelocityMetersPerSecond(); - inputs.climberAppliedVolts = appliedVolts; - inputs.climberCurrentAmps = sim.getCurrentDrawAmps(); - } - - @Override - public void setTargetMeters(final double meters) { - setVoltage(pid.calculate(sim.getPositionMeters(), meters) + ff.calculate(pid.getSetpoint().velocity)); - } - - - public void setOpenLoopDutyCycle(final double dutyCycle) { - setVoltage(dutyCycle * 12); + private ElevatorSim sim = + new ElevatorSim( + DCMotor.getNEO(1), + ClimbConstants.gearRatio, + 1, + ClimbConstants.spoolRadius, + ClimbConstants.minHeight, + ClimbConstants.maxHeight, + true, + 0); + + private double appliedVolts = 0.0; + private ProfiledPIDController pid = + new ProfiledPIDController( + ClimbConstants.kPSim, + ClimbConstants.kISim, + ClimbConstants.kDSim, + new Constraints(ClimbConstants.maxVelocity, ClimbConstants.maxAccel)); + + private ElevatorFeedforward ff = + new ElevatorFeedforward( + ClimbConstants.kSSim, ClimbConstants.kGSim, ClimbConstants.kVSim, ClimbConstants.kASim); + + @Override + public void processInputs(final ClimbIOInputsAutoLogged inputs) { + if (DriverStation.isDisabled()) { + stop(); } - @Override - public void setVoltage(final double voltage) { - appliedVolts = voltage; - sim.setInputVoltage(MathUtil.clamp(voltage, -12.0, 12.0)); - } - - @Override - public void resetEncoder(final double position) { - sim.setState(position, 0.0); - } - - @Override - public void stop() {} - - @Override - public void setPID(double kP, double kI, double kD) {} - - @Override - public void resetEncoder() {} - - @Override - public void setSimpleFF(double kFF) {} - - @Override - public void setFF(double kS, double kG, double kV, double kA) {} -} \ No newline at end of file + sim.update(0.02); + inputs.climberPositionMeters = sim.getPositionMeters(); + inputs.climberVelocityMetersPerSecond = sim.getVelocityMetersPerSecond(); + inputs.climberAppliedVolts = appliedVolts; + inputs.climberCurrentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setTargetMeters(final double meters) { + setVoltage( + pid.calculate(sim.getPositionMeters(), meters) + ff.calculate(pid.getSetpoint().velocity)); + } + + public void setOpenLoopDutyCycle(final double dutyCycle) { + setVoltage(dutyCycle * 12); + } + + @Override + public void setVoltage(final double voltage) { + appliedVolts = voltage; + sim.setInputVoltage(MathUtil.clamp(voltage, -12.0, 12.0)); + } + + @Override + public void resetEncoder(final double position) { + sim.setState(position, 0.0); + } + + @Override + public void stop() {} + + @Override + public void setPID(double kP, double kI, double kD) {} + + @Override + public void resetEncoder() {} + + @Override + public void setSimpleFF(double kFF) {} + + @Override + public void setFF(double kS, double kG, double kV, double kA) {} +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimbIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimbIOSparkMax.java index 28a3131..eb6f881 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimbIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimbIOSparkMax.java @@ -1,96 +1,95 @@ package frc.robot.subsystems.climber; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; - +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import frc.robot.Constants.ClimbConstants; import frc.robot.Constants.RobotMap; public class ClimbIOSparkMax implements ClimbIO { - private final CANSparkMax motor = new CANSparkMax(RobotMap.Climb.climber, MotorType.kBrushless); - private final RelativeEncoder encoder = motor.getEncoder(); - - private final SparkPIDController pid = motor.getPIDController(); - - public ClimbIOSparkMax() { - - motor.restoreFactoryDefaults(); - motor.setIdleMode(IdleMode.kBrake); - motor.setCANTimeout(250); - motor.setInverted(false); - motor.enableVoltageCompensation(12.0); - motor.setSmartCurrentLimit(30); - - encoder.setPositionConversionFactor(ClimbConstants.encoderConversion); - encoder.setVelocityConversionFactor(ClimbConstants.encoderConversion / 60); - - pid.setP(ClimbConstants.kPReal); - pid.setI(ClimbConstants.kIReal); - pid.setD(ClimbConstants.kDReal); - pid.setFF(ClimbConstants.kFFReal); - pid.setOutputRange(-1, 1); - - motor.burnFlash(); - } - - @Override - public void processInputs(final ClimbIOInputsAutoLogged inputs) { - inputs.climberPositionMeters = encoder.getPosition(); - inputs.climberVelocityMetersPerSecond = encoder.getVelocity(); - inputs.climberAppliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); - inputs.climberCurrentAmps = motor.getOutputCurrent(); - inputs.climberTempCelsius = motor.getMotorTemperature(); - } - - @Override - public void setTargetMeters(final double meters) { - pid.setReference(meters, ControlType.kPosition); - } - - @Override - public void setVoltage(final double volts) { - motor.setVoltage(MathUtil.clamp(volts, -12, 12)); - } - - @Override - public void setOpenLoopDutyCycle(final double dutyCycle) { - motor.set(MathUtil.clamp(dutyCycle, -1, 1)); - } - - @Override - public void resetEncoder(final double position) { - encoder.setPosition(position); - } - - @Override - public void stop() { - motor.setVoltage(0); - } - - @Override - public void setPID(double kP, double kI, double kD) { - pid.setP(kP); - pid.setI(kI); - pid.setD(kD); - } - - @Override - public void setSimpleFF(double kFF) { - pid.setFF(kFF); - } - - @Override - public void resetEncoder() { - resetEncoder(0.0); - } - - @Override - public void setFF(double kS, double kG, double kV, double kA) { - // TODO Auto-generated method stub - } -} \ No newline at end of file + private final CANSparkMax motor = new CANSparkMax(RobotMap.Climb.climber, MotorType.kBrushless); + private final RelativeEncoder encoder = motor.getEncoder(); + + private final SparkPIDController pid = motor.getPIDController(); + + public ClimbIOSparkMax() { + + motor.restoreFactoryDefaults(); + motor.setIdleMode(IdleMode.kBrake); + motor.setCANTimeout(250); + motor.setInverted(false); + motor.enableVoltageCompensation(12.0); + motor.setSmartCurrentLimit(30); + + encoder.setPositionConversionFactor(ClimbConstants.encoderConversion); + encoder.setVelocityConversionFactor(ClimbConstants.encoderConversion / 60); + + pid.setP(ClimbConstants.kPReal); + pid.setI(ClimbConstants.kIReal); + pid.setD(ClimbConstants.kDReal); + pid.setFF(ClimbConstants.kFFReal); + pid.setOutputRange(-1, 1); + + motor.burnFlash(); + } + + @Override + public void processInputs(final ClimbIOInputsAutoLogged inputs) { + inputs.climberPositionMeters = encoder.getPosition(); + inputs.climberVelocityMetersPerSecond = encoder.getVelocity(); + inputs.climberAppliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); + inputs.climberCurrentAmps = motor.getOutputCurrent(); + inputs.climberTempCelsius = motor.getMotorTemperature(); + } + + @Override + public void setTargetMeters(final double meters) { + pid.setReference(meters, ControlType.kPosition); + } + + @Override + public void setVoltage(final double volts) { + motor.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setOpenLoopDutyCycle(final double dutyCycle) { + motor.set(MathUtil.clamp(dutyCycle, -1, 1)); + } + + @Override + public void resetEncoder(final double position) { + encoder.setPosition(position); + } + + @Override + public void stop() { + motor.setVoltage(0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + pid.setP(kP); + pid.setI(kI); + pid.setD(kD); + } + + @Override + public void setSimpleFF(double kFF) { + pid.setFF(kFF); + } + + @Override + public void resetEncoder() { + resetEncoder(0.0); + } + + @Override + public void setFF(double kS, double kG, double kV, double kA) { + // TODO Auto-generated method stub + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 64a961a..638bb15 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,10 +15,8 @@ import static edu.wpi.first.units.Units.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; +import com.google.common.collect.Streams; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -26,25 +24,16 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.ControlConstants; import frc.robot.Constants.DriveConstants; - import java.util.Arrays; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.google.common.collect.Streams; - public class Drive extends SubsystemBase { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -129,7 +118,6 @@ public void periodic() { Twist2d twist = kinematics.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } - } } @@ -214,17 +202,15 @@ public static Translation2d[] getModuleTranslations() { } public Command runVoltageTeleopFieldRelative(Supplier speeds) { - return this.run( + return this.run( () -> { var allianceSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - speeds.get(), - gyroInputs.yawPosition - ); + ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), gyroInputs.yawPosition); // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(allianceSpeeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearVelocity); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, DriveConstants.maxLinearVelocity); Logger.recordOutput("Drive/Target Speeds", discreteSpeeds); Logger.recordOutput("Drive/Speed Error", discreteSpeeds.minus(getVelocity())); @@ -233,9 +219,12 @@ public Command runVoltageTeleopFieldRelative(Supplier speeds) { ChassisSpeeds.fromRobotRelativeSpeeds(discreteSpeeds, getRotation())); // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = Streams.zip( - Arrays.stream(modules), Arrays.stream(setpointStates), (m, s) -> m.runSetpoint(s)) - .toArray(SwerveModuleState[]::new); + SwerveModuleState[] optimizedSetpointStates = + Streams.zip( + Arrays.stream(modules), + Arrays.stream(setpointStates), + (m, s) -> m.runSetpoint(s)) + .toArray(SwerveModuleState[]::new); // Log setpoint states Logger.recordOutput("SwerveStates/Setpoints", setpointStates); @@ -262,13 +251,13 @@ public void resetOffsets() { public Command resetOffsetsCmd() { return run(() -> resetOffsets()); -} + } -public Rotation2d getRotation() { - return gyroIO.getYaw(); -} + public Rotation2d getRotation() { + return gyroIO.getYaw(); + } -public void resetGyro() { - gyroIO.reset(); + public void resetGyro() { + gyroIO.reset(); + } } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index cd7ff68..4b0bd7e 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -14,7 +14,6 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; - import org.littletonrobotics.junction.AutoLog; public interface GyroIO { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2Phoenix6.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2Phoenix6.java index a69833a..5da567e 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2Phoenix6.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2Phoenix6.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotMap; - import java.util.OptionalDouble; import java.util.Queue; @@ -40,7 +39,7 @@ public GyroIOPigeon2Phoenix6() { yaw.setUpdateFrequency(DriveConstants.odometeryFrequency); yawVelocity.setUpdateFrequency(100.0); pigeon.optimizeBusUtilization(); - + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); yawPositionQueue = SparkMaxOdometryThread.getInstance() @@ -54,7 +53,6 @@ public GyroIOPigeon2Phoenix6() { } }); } - @Override public void processInputs(GyroIOInputsAutoLogged inputs) { @@ -72,19 +70,16 @@ public void processInputs(GyroIOInputsAutoLogged inputs) { yawPositionQueue.clear(); } - @Override public void setYaw(double yaw) { pigeon.setYaw(yaw, 0.1); } - @Override public Rotation2d getYaw() { return Rotation2d.fromDegrees(yaw.getValueAsDouble()); } - @Override public void reset() { setYaw(0.0); @@ -93,7 +88,7 @@ public void reset() { public Rotation2d normalizeAngle(Rotation2d yaw) { while (yaw.getRadians() < Math.PI * 2) { yaw.plus(Rotation2d.fromRadians(Math.PI)); - } + } return Rotation2d.fromRadians(yaw.getRadians() % (Math.PI * 2)); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a975420..05c42ec 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -21,7 +21,6 @@ import frc.robot.Constants.DriveConstants; import frc.util.Alert; import frc.util.LoggedTunableNumber; - import org.littletonrobotics.junction.Logger; public class Module { @@ -30,13 +29,13 @@ public class Module { private final int index; // private SimpleMotorFeedforward driveFeedforward; - + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop private Double velocitySetpoint = null; // Setpoint for closed loop control, null for open loop private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; private static final String[] moduleNames = new String[] {"FL", "FR", "BL", "BR"}; - + private LoggedTunableNumber kPDrive = new LoggedTunableNumber("Drive/kPDrive"); private LoggedTunableNumber kDDrive = new LoggedTunableNumber("Drive/kDDrive"); private LoggedTunableNumber kSDrive = new LoggedTunableNumber("Drive/kSDrive"); @@ -106,7 +105,7 @@ public Module(ModuleIO io, int index) { kPTurn.initDefault(0.0); kDTurn.initDefault(0.0); break; - } + } io.setDrivePIDFF(kPDrive.get(), 0, kDDrive.get(), kSDrive.get(), kVDrive.get(), kADrive.get()); io.setTurnPID(kPTurn.get(), 0, kDTurn.get()); @@ -120,7 +119,15 @@ public void processInputs() { io.processInputs(inputs); LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setDrivePIDFF(kPDrive.get(), 0, kDDrive.get(), kSDrive.get(), kVDrive.get(), kADrive.get()), kPDrive, kDDrive, kSDrive, kVDrive, kADrive); + hashCode(), + () -> + io.setDrivePIDFF( + kPDrive.get(), 0, kDDrive.get(), kSDrive.get(), kVDrive.get(), kADrive.get()), + kPDrive, + kDDrive, + kSDrive, + kVDrive, + kADrive); LoggedTunableNumber.ifChanged( hashCode(), () -> io.setTurnPID(kPTurn.get(), 0, kDTurn.get()), kPTurn, kDTurn); @@ -134,8 +141,6 @@ public void periodic() { String.format("Drive/%s Module/Voltage Available", io.getModuleName()), Math.abs(inputs.driveAppliedVolts - RoboRioDataJNI.getVInVoltage())); - - // Calculate positions for odometry int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together odometryPositions = new SwerveModulePosition[sampleCount]; @@ -154,10 +159,13 @@ public SwerveModuleState runSetpoint(SwerveModuleState setpoint) { final var optimizedState = SwerveModuleState.optimize(setpoint, getAngle()); inputs.targetPosition = optimizedState.angle; io.runTurnPositionSetpoint(optimizedState.angle.getRadians()); - io.runDriveVelocitySetpoint(optimizedState.speedMetersPerSecond * Math.cos(optimizedState.angle.minus(inputs.turnPosition).getRadians()), (optimizedState.speedMetersPerSecond - lastSetpoint.speedMetersPerSecond) / 0.02); + io.runDriveVelocitySetpoint( + optimizedState.speedMetersPerSecond + * Math.cos(optimizedState.angle.minus(inputs.turnPosition).getRadians()), + (optimizedState.speedMetersPerSecond - lastSetpoint.speedMetersPerSecond) / 0.02); angleSetpoint = optimizedState.angle; - + velocitySetpoint = optimizedState.speedMetersPerSecond; return optimizedState; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 8a459ba..1c33820 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -14,7 +14,6 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; - import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -60,7 +59,8 @@ public static class ModuleIOInputs { public abstract void runTurnPositionSetpoint(double angleRads); /** Configure drive PID */ - public abstract void setDrivePIDFF(double kP, double kI, double kD, double kS, double kV, double kA); + public abstract void setDrivePIDFF( + double kP, double kI, double kD, double kS, double kV, double kA); /** Configure turn PID */ public abstract void setTurnPID(double kP, double kI, double kD); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java index df830bb..4954c8a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java @@ -13,10 +13,6 @@ package frc.robot.subsystems.drive; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -28,18 +24,20 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogEncoder; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotMap; - import java.util.OptionalDouble; import java.util.Queue; @@ -77,7 +75,8 @@ public class ModuleIOReal implements ModuleIO { private final StatusSignal driveCurrent; // private int multiplier; - private final VelocityVoltage driveCurrentVelocity = new VelocityVoltage(0.0).withEnableFOC(false); + private final VelocityVoltage driveCurrentVelocity = + new VelocityVoltage(0.0).withEnableFOC(false); private final Queue turnPositionQueue; @@ -91,7 +90,8 @@ public ModuleIOReal(int index) { turnSparkMax = new CANSparkMax(RobotMap.Drive.frontLeftTurn, MotorType.kBrushless); turnInverted = RobotMap.Drive.frontLeftTurnInvert; absoluteEncoder = new AnalogEncoder(RobotMap.Drive.frontLeftEncoder); - absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.frontLeftOffset); // MUST BE CALIBRATED + absoluteEncoderOffset = + new Rotation2d(RobotMap.Drive.frontLeftOffset); // MUST BE CALIBRATED name = "FrontLeft"; // multiplier = -1; break; @@ -101,7 +101,8 @@ public ModuleIOReal(int index) { turnSparkMax = new CANSparkMax(RobotMap.Drive.frontRightTurn, MotorType.kBrushless); turnInverted = RobotMap.Drive.frontRightTurnInvert; absoluteEncoder = new AnalogEncoder(RobotMap.Drive.frontRightEncoder); - absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.frontRightOffset); // MUST BE CALIBRATED + absoluteEncoderOffset = + new Rotation2d(RobotMap.Drive.frontRightOffset); // MUST BE CALIBRATED name = "FrontRight"; // multiplier = 1; break; @@ -121,7 +122,8 @@ public ModuleIOReal(int index) { turnSparkMax = new CANSparkMax(RobotMap.Drive.backRightTurn, MotorType.kBrushless); turnInverted = RobotMap.Drive.backRightTurnInvert; absoluteEncoder = new AnalogEncoder(RobotMap.Drive.backRightEncoder); - absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.backRightOffset); // MUST BE CALIBRATED + absoluteEncoderOffset = + new Rotation2d(RobotMap.Drive.backRightOffset); // MUST BE CALIBRATED name = "BackRight"; // multiplier = 1; break; @@ -129,8 +131,6 @@ public ModuleIOReal(int index) { throw new RuntimeException("Invalid module index"); } - - driveConfig.CurrentLimits.SupplyCurrentLimit = DriveConstants.driveSupplyCurrent; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; driveConfig.CurrentLimits.StatorCurrentLimit = DriveConstants.driveStatorCurrent; @@ -147,7 +147,7 @@ public ModuleIOReal(int index) { driveConfig.MotionMagic.MotionMagicCruiseVelocity = DriveConstants.maxLinearVelocity; driveConfig.MotionMagic.MotionMagicAcceleration = DriveConstants.maxLinearAccel; driveConfig.MotionMagic.MotionMagicJerk = DriveConstants.maxLinearVelocity / 0.1; - + driveConfig.Slot0.kP = DriveConstants.kPDriveReal; driveConfig.Slot0.kI = 0.0; driveConfig.Slot0.kD = DriveConstants.kDDriveReal; @@ -165,18 +165,18 @@ public ModuleIOReal(int index) { turnRelativeEncoder.setPositionConversionFactor(DriveConstants.turnConversion); absoluteEncoder.setDistancePerRotation(2 * Math.PI); - + turnRelativeEncoder.setVelocityConversionFactor(DriveConstants.turnConversion * 60); turnPID = turnSparkMax.getPIDController(); turnSparkMax.restoreFactoryDefaults(); turnSparkMax.setCANTimeout(250); - for (int i = 0; i < 30; i ++) { + for (int i = 0; i < 30; i++) { turnPID.setFeedbackDevice(turnRelativeEncoder); turnRelativeEncoder.setPositionConversionFactor(DriveConstants.turnConversion); turnRelativeEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); - + turnSparkMax.setInverted(turnInverted); turnSparkMax.setSmartCurrentLimit(30); turnSparkMax.enableVoltageCompensation(12.0); @@ -221,26 +221,21 @@ public ModuleIOReal(int index) { return OptionalDouble.empty(); } }); - } @Override public void processInputs(ModuleIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent); - - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DriveConstants.driveRatio; - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DriveConstants.driveRatio; + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DriveConstants.driveRatio; + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DriveConstants.driveRatio; inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - inputs.turnAbsolutePosition = - Rotation2d.fromRadians(absoluteEncoder.getAbsolutePosition()); - inputs.turnPosition = - Rotation2d.fromRadians(normalizeAngle(turnRelativeEncoder.getPosition())); + inputs.turnAbsolutePosition = Rotation2d.fromRadians(absoluteEncoder.getAbsolutePosition()); + inputs.turnPosition = Rotation2d.fromRadians(normalizeAngle(turnRelativeEncoder.getPosition())); inputs.turnVelocityRadPerSec = turnRelativeEncoder.getVelocity(); inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; @@ -249,7 +244,8 @@ public void processInputs(ModuleIOInputsAutoLogged inputs) { timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryDrivePositionsRad = drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DriveConstants.driveRatio) + .mapToDouble( + (Double value) -> Units.rotationsToRadians(value) / DriveConstants.driveRatio) .toArray(); inputs.odometryTurnPositions = turnPositionQueue.stream() @@ -289,7 +285,8 @@ public String getModuleName() { } @Override - public void runDriveVelocitySetpoint(final double metersPerSecond, final double metersPerSecondSquared) { + public void runDriveVelocitySetpoint( + final double metersPerSecond, final double metersPerSecondSquared) { // Doesnt actually refresh drive velocity signal, but should be cached if (metersPerSecond == 0 && metersPerSecondSquared == 0 @@ -297,7 +294,9 @@ public void runDriveVelocitySetpoint(final double metersPerSecond, final double runDriveVoltage(0.0); } else { driveTalon.setControl( - driveCurrentVelocity.withVelocity(metersPerSecond).withFeedForward(metersPerSecondSquared * driveConfig.Slot0.kA)); + driveCurrentVelocity + .withVelocity(metersPerSecond) + .withFeedForward(metersPerSecondSquared * driveConfig.Slot0.kA)); } } @@ -327,7 +326,8 @@ public void setTurnPID(double kP, double kI, double kD) { } public double getAbsoluteEncoder() { - return normalizeAngle((absoluteEncoder.getAbsolutePosition() * 2*Math.PI) - absoluteEncoderOffset.getRadians()); + return normalizeAngle( + (absoluteEncoder.getAbsolutePosition() * 2 * Math.PI) - absoluteEncoderOffset.getRadians()); } public double normalizeAngle(double radians) { @@ -337,9 +337,9 @@ public double normalizeAngle(double radians) { @Override public void stop() { var driveRequest = driveTalon.getAppliedControl(); - if(driveRequest instanceof VoltageOut) { - driveTalon.setControl(new NeutralOut()); - } + if (driveRequest instanceof VoltageOut) { + driveTalon.setControl(new NeutralOut()); + } runTurnVoltage(0); } @@ -349,5 +349,4 @@ public void resetOffset() { turnRelativeEncoder.setPosition(getAbsoluteEncoder()); } } - } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOReplay.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOReplay.java index aa22094..3ab6a75 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOReplay.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOReplay.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.drive; public class ModuleIOReplay implements ModuleIO { - /** Updates the set of loggable inputs. */ + /** Updates the set of loggable inputs. */ public void processInputs(ModuleIOInputsAutoLogged inputs) {} /** Run the drive motor at the specified voltage. */ @@ -31,7 +31,9 @@ public void setTurnBrakeMode(boolean enable) {} /** Disable output to all motors */ public void stop() {} - public String getModuleName() { return null; } + public String getModuleName() { + return null; + } public void resetOffset() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index de8b57d..fd43fce 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -33,13 +33,14 @@ public class ModuleIOSim implements ModuleIO { private final String name; - private DCMotorSim driveSim = new DCMotorSim(DCMotor.getKrakenX60(1), DriveConstants.driveRatio, DriveConstants.driveMOI); - private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), DriveConstants.turnRatio, DriveConstants.turnMOI); - + private DCMotorSim driveSim = + new DCMotorSim(DCMotor.getKrakenX60(1), DriveConstants.driveRatio, DriveConstants.driveMOI); + private DCMotorSim turnSim = + new DCMotorSim(DCMotor.getNEO(1), DriveConstants.turnRatio, DriveConstants.turnMOI); + private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, SimConstants.loopTime); - private SimpleMotorFeedforward driveFeedforward = - new SimpleMotorFeedforward(0, 0, 0); + private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0, 0); private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, SimConstants.loopTime); @@ -92,8 +93,8 @@ public void runTurnVoltage(double volts) { @Override public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { runDriveVoltage( - feedForward + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) - ); + feedForward + + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec)); } @Override @@ -133,5 +134,4 @@ public String getModuleName() { } public void resetOffset() {} - } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index a95c955..313d454 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -17,9 +17,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; - import frc.robot.Constants.DriveConstants; - import java.util.ArrayList; import java.util.List; import java.util.Queue; diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index c0d655a..e766114 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.Notifier; import frc.robot.Constants.DriveConstants; - import java.util.ArrayList; import java.util.List; import java.util.OptionalDouble; diff --git a/src/main/java/frc/robot/subsystems/feeder/BeambreakIO.java b/src/main/java/frc/robot/subsystems/feeder/BeambreakIO.java index 03e325a..51f5ed4 100644 --- a/src/main/java/frc/robot/subsystems/feeder/BeambreakIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/BeambreakIO.java @@ -3,14 +3,12 @@ import org.littletonrobotics.junction.AutoLog; public interface BeambreakIO { - @AutoLog - public static class BeambreakIOInputs { - public boolean isObstructed = false; - } + @AutoLog + public static class BeambreakIOInputs { + public boolean isObstructed = false; + } - public default void processInputs(BeambreakIOInputs inputs) { - } + public default void processInputs(BeambreakIOInputs inputs) {} - public default void overrideObstructed(boolean isObstructed) { - } + public default void overrideObstructed(boolean isObstructed) {} } diff --git a/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReal.java b/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReal.java index d593c5d..71edcec 100644 --- a/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReal.java @@ -3,14 +3,14 @@ import edu.wpi.first.wpilibj.DigitalInput; public class BeambreakIOReal implements BeambreakIO { - private final DigitalInput beambreak; + private final DigitalInput beambreak; - public BeambreakIOReal(int dioChannel) { - this.beambreak = new DigitalInput(dioChannel); - } + public BeambreakIOReal(int dioChannel) { + this.beambreak = new DigitalInput(dioChannel); + } - @Override - public void processInputs(BeambreakIOInputs inputs) { - inputs.isObstructed = !beambreak.get(); - } + @Override + public void processInputs(BeambreakIOInputs inputs) { + inputs.isObstructed = !beambreak.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReplay.java b/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReplay.java index fdd70d1..9f6b5c1 100644 --- a/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReplay.java +++ b/src/main/java/frc/robot/subsystems/feeder/BeambreakIOReplay.java @@ -1,12 +1,8 @@ package frc.robot.subsystems.feeder; -import frc.robot.subsystems.feeder.BeambreakIOInputsAutoLogged; - public class BeambreakIOReplay implements BeambreakIO { - public void processInputs(BeambreakIOInputsAutoLogged inputs) { - } + public void processInputs(BeambreakIOInputsAutoLogged inputs) {} - public void overrideObstructed(boolean isObstructed) { - } + public void overrideObstructed(boolean isObstructed) {} } diff --git a/src/main/java/frc/robot/subsystems/feeder/BeambreakIOSim.java b/src/main/java/frc/robot/subsystems/feeder/BeambreakIOSim.java index 996f46e..1ef4882 100644 --- a/src/main/java/frc/robot/subsystems/feeder/BeambreakIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/BeambreakIOSim.java @@ -3,19 +3,19 @@ import edu.wpi.first.wpilibj.simulation.DIOSim; public class BeambreakIOSim implements BeambreakIO { - public final DIOSim beambreakSim; + public final DIOSim beambreakSim; - public BeambreakIOSim(int dioChannel) { - beambreakSim = new DIOSim(dioChannel); - } + public BeambreakIOSim(int dioChannel) { + beambreakSim = new DIOSim(dioChannel); + } - @Override - public void processInputs(BeambreakIOInputs inputs) { - inputs.isObstructed = !beambreakSim.getValue(); - } + @Override + public void processInputs(BeambreakIOInputs inputs) { + inputs.isObstructed = !beambreakSim.getValue(); + } - @Override - public void overrideObstructed(boolean isObstructed) { - this.beambreakSim.setValue(!isObstructed); - } + @Override + public void overrideObstructed(boolean isObstructed) { + this.beambreakSim.setValue(!isObstructed); + } } diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 363f155..024ab80 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -1,67 +1,62 @@ package frc.robot.subsystems.feeder; -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.FeederConstants; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import org.littletonrobotics.junction.Logger; public class Feeder extends SubsystemBase { - public FeederIO io; - public FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); + public FeederIO io; + public FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); + + public BeambreakIO feederBeambreak; + public BeambreakIOInputsAutoLogged feederBeambreakInputs = new BeambreakIOInputsAutoLogged(); + public BeambreakIO shooterBeambreak; + public BeambreakIOInputsAutoLogged shooterBeambreakInputs = new BeambreakIOInputsAutoLogged(); - public BeambreakIO feederBeambreak; - public BeambreakIOInputsAutoLogged feederBeambreakInputs = new BeambreakIOInputsAutoLogged(); - public BeambreakIO shooterBeambreak; - public BeambreakIOInputsAutoLogged shooterBeambreakInputs = new BeambreakIOInputsAutoLogged(); - - private SimpleMotorFeedforward ff; + private SimpleMotorFeedforward ff; - public Feeder(FeederIO io, BeambreakIO feederBeambreak, BeambreakIO shooterBeambreak) { - this.io = io; - this.feederBeambreak = feederBeambreak; - this.shooterBeambreak = shooterBeambreak; + public Feeder(FeederIO io, BeambreakIO feederBeambreak, BeambreakIO shooterBeambreak) { + this.io = io; + this.feederBeambreak = feederBeambreak; + this.shooterBeambreak = shooterBeambreak; - io.setPID(FeederConstants.kPReal, 0.0, 0.0); - ff = new SimpleMotorFeedforward(0.0, FeederConstants.kVReal); - } + io.setPID(FeederConstants.kPReal, 0.0, 0.0); + ff = new SimpleMotorFeedforward(0.0, FeederConstants.kVReal); + } - public void periodic() { - io.processInputs(inputs); - feederBeambreak.processInputs(feederBeambreakInputs); - shooterBeambreak.processInputs(shooterBeambreakInputs); - Logger.processInputs("Feeder", inputs); - Logger.processInputs("Feeder/FeederBeambreak", feederBeambreakInputs); - Logger.processInputs("Feeder/ShooterBeambreak", shooterBeambreakInputs); - } + public void periodic() { + io.processInputs(inputs); + feederBeambreak.processInputs(feederBeambreakInputs); + shooterBeambreak.processInputs(shooterBeambreakInputs); + Logger.processInputs("Feeder", inputs); + Logger.processInputs("Feeder/FeederBeambreak", feederBeambreakInputs); + Logger.processInputs("Feeder/ShooterBeambreak", shooterBeambreakInputs); + } - public Command setRPM(IntSupplier rpm) { + public Command setRPM(IntSupplier rpm) { return this.run( () -> { io.setRPM(rpm.getAsInt(), ff); inputs.targetRPM = rpm.getAsInt(); }); - } + } - public Command setVoltage(DoubleSupplier volts) { return this.run( () -> { io.setVoltage(volts.getAsDouble()); }); } - - public boolean feederBeambreakObstructed() { - return feederBeambreakInputs.isObstructed; - } - public boolean shooterBeambreakObstructed() { - return shooterBeambreakInputs.isObstructed; - } + public boolean feederBeambreakObstructed() { + return feederBeambreakInputs.isObstructed; + } - + public boolean shooterBeambreakObstructed() { + return shooterBeambreakInputs.isObstructed; } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index 9c33b01..55ee93a 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -1,24 +1,23 @@ package frc.robot.subsystems.feeder; -import org.littletonrobotics.junction.AutoLog; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import org.littletonrobotics.junction.AutoLog; public interface FeederIO { - @AutoLog - public static class FeederIOInputs { - public double speedRPM = 0.0; - public double targetRPM = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - public double tempCelsius = 0.0; - } - - public abstract void processInputs(final FeederIOInputsAutoLogged inputs); + @AutoLog + public static class FeederIOInputs { + public double speedRPM = 0.0; + public double targetRPM = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + } + + public abstract void processInputs(final FeederIOInputsAutoLogged inputs); - public abstract void setVoltage(double volts); + public abstract void setVoltage(double volts); - public abstract void setRPM(int rpm, SimpleMotorFeedforward ff); + public abstract void setRPM(int rpm, SimpleMotorFeedforward ff); - public abstract void setPID(double kP, double kI, double kD); + public abstract void setPID(double kP, double kI, double kD); } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReplay.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReplay.java index 37b4e53..2c758c1 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReplay.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReplay.java @@ -4,16 +4,15 @@ public class FeederIOReplay implements FeederIO { - @Override - public void processInputs(FeederIOInputsAutoLogged inputs) {} + @Override + public void processInputs(FeederIOInputsAutoLogged inputs) {} - @Override - public void setVoltage(double volts) {} + @Override + public void setVoltage(double volts) {} - @Override - public void setRPM(int rpm, SimpleMotorFeedforward ff) {} + @Override + public void setRPM(int rpm, SimpleMotorFeedforward ff) {} - @Override - public void setPID(double kP, double kI, double kD) {} - + @Override + public void setPID(double kP, double kI, double kD) {} } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java index 1fb87f9..09477be 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java @@ -9,31 +9,30 @@ import frc.robot.Constants.FeederConstants; public class FeederIOSim implements FeederIO { - private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), FeederConstants.ratio, FeederConstants.MOI); - private PIDController pid = new PIDController(FeederConstants.kPSim, 0.0, 0); - - - @Override - public void processInputs(FeederIOInputsAutoLogged inputs) { - sim.update(Constants.loopPeriodSecs); - - inputs.speedRPM = sim.getAngularVelocityRPM(); - inputs.currentAmps = sim.getCurrentDrawAmps(); - } - - @Override - public void setVoltage(double volts) { - sim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); - } - - @Override - public void setRPM(int rpm, SimpleMotorFeedforward ff) { - setVoltage(ff.calculate(rpm)); - } - - @Override - public void setPID(double kP, double kI, double kD) { - pid = new PIDController(kP, kI, kD); - } - + private FlywheelSim sim = + new FlywheelSim(DCMotor.getNEO(1), FeederConstants.ratio, FeederConstants.MOI); + private PIDController pid = new PIDController(FeederConstants.kPSim, 0.0, 0); + + @Override + public void processInputs(FeederIOInputsAutoLogged inputs) { + sim.update(Constants.loopPeriodSecs); + + inputs.speedRPM = sim.getAngularVelocityRPM(); + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setVoltage(double volts) { + sim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setRPM(int rpm, SimpleMotorFeedforward ff) { + setVoltage(ff.calculate(rpm)); + } + + @Override + public void setPID(double kP, double kI, double kD) { + pid = new PIDController(kP, kI, kD); + } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java index 0097d9c..c285d41 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkMax.java @@ -7,55 +7,53 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.robot.Constants.FeederConstants; import frc.robot.Constants.RobotMap; public class FeederIOSparkMax implements FeederIO { - private final CANSparkMax feeder = new CANSparkMax(RobotMap.Intake.feeder, MotorType.kBrushless); - private final RelativeEncoder enc = feeder.getEncoder(); - private final SparkPIDController pid = feeder.getPIDController(); - - public FeederIOSparkMax() { - feeder.restoreFactoryDefaults(); - - feeder.setSmartCurrentLimit(FeederConstants.currentLimit); - feeder.setInverted(true); - - feeder.setIdleMode(IdleMode.kBrake); - - enc.setVelocityConversionFactor(1.0); - - feeder.burnFlash(); - } - - @Override - public void processInputs(FeederIOInputsAutoLogged inputs) { - inputs.speedRPM = enc.getVelocity(); - inputs.appliedVolts = feeder.getAppliedOutput() * feeder.getBusVoltage(); - inputs.currentAmps = feeder.getOutputCurrent(); - inputs.tempCelsius = feeder.getMotorTemperature(); - } - - @Override - public void setVoltage(double volts) { - feeder.setVoltage(MathUtil.clamp(volts, -12, 12)); - } - - @Override - public void setRPM(int rpm, SimpleMotorFeedforward ff) { - rpm = MathUtil.clamp(rpm, 0, 5880); - pid.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); - } - - @Override - public void setPID(double kP, double kI, double kD) { - pid.setP(kP); - pid.setI(kI); - pid.setD(kD); - feeder.burnFlash(); - } - + private final CANSparkMax feeder = new CANSparkMax(RobotMap.Intake.feeder, MotorType.kBrushless); + private final RelativeEncoder enc = feeder.getEncoder(); + private final SparkPIDController pid = feeder.getPIDController(); + + public FeederIOSparkMax() { + feeder.restoreFactoryDefaults(); + + feeder.setSmartCurrentLimit(FeederConstants.currentLimit); + feeder.setInverted(true); + + feeder.setIdleMode(IdleMode.kBrake); + + enc.setVelocityConversionFactor(1.0); + + feeder.burnFlash(); + } + + @Override + public void processInputs(FeederIOInputsAutoLogged inputs) { + inputs.speedRPM = enc.getVelocity(); + inputs.appliedVolts = feeder.getAppliedOutput() * feeder.getBusVoltage(); + inputs.currentAmps = feeder.getOutputCurrent(); + inputs.tempCelsius = feeder.getMotorTemperature(); + } + + @Override + public void setVoltage(double volts) { + feeder.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setRPM(int rpm, SimpleMotorFeedforward ff) { + rpm = MathUtil.clamp(rpm, 0, 5880); + pid.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); + } + + @Override + public void setPID(double kP, double kI, double kD) { + pid.setP(kP); + pid.setI(kI); + pid.setD(kD); + feeder.burnFlash(); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1a5bf8e..632410e 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -4,11 +4,6 @@ package frc.robot.subsystems.intake; -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,6 +12,9 @@ import frc.robot.Constants; import frc.robot.Constants.IntakeConstants; import frc.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { public IntakeIO io; @@ -64,11 +62,14 @@ public Intake(IntakeIO io) { } io.setPivotPID(kPPivot.getAsDouble(), 0.0, 0.0); - pivotFF = new ArmFeedforward(0.0, IntakeConstants.kGPivot, IntakeConstants.kVPivot, IntakeConstants.kAPivot); - + pivotFF = + new ArmFeedforward( + 0.0, IntakeConstants.kGPivot, IntakeConstants.kVPivot, IntakeConstants.kAPivot); io.setRollerPID(kPRoller.getAsDouble(), 0.0, 0.0); - rollerFF = new SimpleMotorFeedforward(kSRoller.getAsDouble(), IntakeConstants.kVRoller, IntakeConstants.kARoller); + rollerFF = + new SimpleMotorFeedforward( + kSRoller.getAsDouble(), IntakeConstants.kVRoller, IntakeConstants.kARoller); setIntakeUp(); } @@ -83,7 +84,8 @@ public Command setPivotTarget(DoubleSupplier radians) { return this.run( () -> { io.setPivotTarget(radians.getAsDouble(), pivotFF); - inputs.pivotTargetPosition = Rotation2d.fromRadians(radians.getAsDouble() + IntakeConstants.simOffset); + inputs.pivotTargetPosition = + Rotation2d.fromRadians(radians.getAsDouble() + IntakeConstants.simOffset); }); } @@ -107,7 +109,7 @@ public Command setIntakeDown(boolean reverse) { () -> { io.setPivotTarget(IntakeConstants.down, pivotFF); inputs.pivotTargetPosition = Rotation2d.fromRadians(IntakeConstants.down); - + io.setRollerRPM(3000 * (reverse ? -1 : 1), rollerFF); inputs.rollerTargetRPM = 3000 * (reverse ? -1 : 1); }); @@ -139,4 +141,4 @@ public Command setPivotVoltage(DoubleSupplier volts) { inputs.pivotAppliedVolts = volts.getAsDouble(); }); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index da47121..d1c71ab 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -4,42 +4,40 @@ package frc.robot.subsystems.intake; -import org.littletonrobotics.junction.AutoLog; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; /** Add your docs here. */ public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { - public Rotation2d pivotPosition = new Rotation2d(); - public Rotation2d pivotTargetPosition = new Rotation2d(); - public double pivotVelocityRadPerSec = 0.0; - public double pivotAppliedVolts = 0.0; - public double pivotCurrentAmps = 0.0; - public double pivotTempCelsius = 0.0; + @AutoLog + public static class IntakeIOInputs { + public Rotation2d pivotPosition = new Rotation2d(); + public Rotation2d pivotTargetPosition = new Rotation2d(); + public double pivotVelocityRadPerSec = 0.0; + public double pivotAppliedVolts = 0.0; + public double pivotCurrentAmps = 0.0; + public double pivotTempCelsius = 0.0; - public double rollerSpeedRPM = 0.0; - public double rollerTargetRPM = 0.0; - public double rollerAppliedVolts = 0.0; - public double rollerCurrentAmps = 0.0; - public double rollerTempCelsius = 0.0; - } + public double rollerSpeedRPM = 0.0; + public double rollerTargetRPM = 0.0; + public double rollerAppliedVolts = 0.0; + public double rollerCurrentAmps = 0.0; + public double rollerTempCelsius = 0.0; + } - public abstract void processInputs(final IntakeIOInputsAutoLogged inputs); + public abstract void processInputs(final IntakeIOInputsAutoLogged inputs); - public abstract void setPivotVoltage(double volts); + public abstract void setPivotVoltage(double volts); - public abstract void setRollerVoltage(double volts); + public abstract void setRollerVoltage(double volts); - public abstract void setPivotTarget(double angle, ArmFeedforward ff); + public abstract void setPivotTarget(double angle, ArmFeedforward ff); - public abstract void setRollerRPM(int rpm, SimpleMotorFeedforward ff); - - public abstract void setPivotPID(double kP, double kI, double kD); + public abstract void setRollerRPM(int rpm, SimpleMotorFeedforward ff); - public abstract void setRollerPID(double kP, double kI, double kD); + public abstract void setPivotPID(double kP, double kI, double kD); + public abstract void setRollerPID(double kP, double kI, double kD); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReplay.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReplay.java index e39bf3b..28cd1c4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReplay.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReplay.java @@ -9,23 +9,22 @@ /** Add your docs here. */ public class IntakeIOReplay implements IntakeIO { - public void processInputs(final IntakeIOInputsAutoLogged inputs) {} + public void processInputs(final IntakeIOInputsAutoLogged inputs) {} - @Override - public void setPivotPID(double kP, double kI, double kD) {} + @Override + public void setPivotPID(double kP, double kI, double kD) {} - @Override - public void setRollerPID(double kP, double kI, double kD) {} + @Override + public void setRollerPID(double kP, double kI, double kD) {} - @Override - public void setPivotVoltage(double volts) {} + @Override + public void setPivotVoltage(double volts) {} - @Override - public void setRollerVoltage(double volts) {} + @Override + public void setRollerVoltage(double volts) {} - @Override - public void setPivotTarget(double angle, ArmFeedforward ff) {} - - public void setRollerRPM(int rpm, SimpleMotorFeedforward ff) {} + @Override + public void setPivotTarget(double angle, ArmFeedforward ff) {} + public void setRollerRPM(int rpm, SimpleMotorFeedforward ff) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 5cfb305..4bed32f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -20,53 +20,76 @@ /** Add your docs here. */ public class IntakeIOSim implements IntakeIO { - private FlywheelSim rollerSim = new FlywheelSim(DCMotor.getNEO(1), 1, IntakeConstants.rollerMOI); + private FlywheelSim rollerSim = new FlywheelSim(DCMotor.getNEO(1), 1, IntakeConstants.rollerMOI); - private SingleJointedArmSim pivotSim = new SingleJointedArmSim(DCMotor.getNEO(1), IntakeConstants.pivotRatio, IntakeConstants.pivotMOI, Units.inchesToMeters(IntakeConstants.pivotLength), IntakeConstants.up, IntakeConstants.down, true, IntakeConstants.up); - private ProfiledPIDController pivotPID = new ProfiledPIDController(IntakeConstants.kPPivotSim, 0.0, 0.0, new TrapezoidProfile.Constraints(IntakeConstants.maxPivotVelocity, IntakeConstants.maxPivotAccel)); + private SingleJointedArmSim pivotSim = + new SingleJointedArmSim( + DCMotor.getNEO(1), + IntakeConstants.pivotRatio, + IntakeConstants.pivotMOI, + Units.inchesToMeters(IntakeConstants.pivotLength), + IntakeConstants.up, + IntakeConstants.down, + true, + IntakeConstants.up); + private ProfiledPIDController pivotPID = + new ProfiledPIDController( + IntakeConstants.kPPivotSim, + 0.0, + 0.0, + new TrapezoidProfile.Constraints( + IntakeConstants.maxPivotVelocity, IntakeConstants.maxPivotAccel)); - private PIDController rollerPID = new PIDController(IntakeConstants.kPRollerSim, 0.0, 0.0); - @Override - public void processInputs(IntakeIOInputsAutoLogged inputs) { - rollerSim.update(Constants.loopPeriodSecs); - pivotSim.update(Constants.loopPeriodSecs); + private PIDController rollerPID = new PIDController(IntakeConstants.kPRollerSim, 0.0, 0.0); - inputs.pivotPosition = Rotation2d.fromRadians(pivotSim.getAngleRads()); - inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getVelocityRadPerSec()); - inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps(); + @Override + public void processInputs(IntakeIOInputsAutoLogged inputs) { + rollerSim.update(Constants.loopPeriodSecs); + pivotSim.update(Constants.loopPeriodSecs); - inputs.rollerSpeedRPM = rollerSim.getAngularVelocityRPM(); - inputs.rollerCurrentAmps = rollerSim.getCurrentDrawAmps(); - } + inputs.pivotPosition = Rotation2d.fromRadians(pivotSim.getAngleRads()); + inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getVelocityRadPerSec()); + inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps(); - @Override - public void setPivotTarget(double angle, ArmFeedforward ff) { - setPivotVoltage(pivotPID.calculate(pivotSim.getAngleRads(), angle) + ff.calculate(pivotPID.getSetpoint().position, pivotPID.getSetpoint().velocity)); - } + inputs.rollerSpeedRPM = rollerSim.getAngularVelocityRPM(); + inputs.rollerCurrentAmps = rollerSim.getCurrentDrawAmps(); + } - @Override - public void setRollerRPM(int rpm, SimpleMotorFeedforward ff) { - setRollerVoltage(ff.calculate(rpm)); - } + @Override + public void setPivotTarget(double angle, ArmFeedforward ff) { + setPivotVoltage( + pivotPID.calculate(pivotSim.getAngleRads(), angle) + + ff.calculate(pivotPID.getSetpoint().position, pivotPID.getSetpoint().velocity)); + } - @Override - public void setPivotPID(double kP, double kI, double kD) { - pivotPID = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(IntakeConstants.maxPivotVelocity, IntakeConstants.maxPivotAccel)); - } + @Override + public void setRollerRPM(int rpm, SimpleMotorFeedforward ff) { + setRollerVoltage(ff.calculate(rpm)); + } - @Override - public void setRollerPID(double kP, double kI, double kD) { - rollerPID = new PIDController(kP, kI, kD); - } + @Override + public void setPivotPID(double kP, double kI, double kD) { + pivotPID = + new ProfiledPIDController( + kP, + kI, + kD, + new TrapezoidProfile.Constraints( + IntakeConstants.maxPivotVelocity, IntakeConstants.maxPivotAccel)); + } - @Override - public void setPivotVoltage(double volts) { - pivotSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); - } + @Override + public void setRollerPID(double kP, double kI, double kD) { + rollerPID = new PIDController(kP, kI, kD); + } - @Override - public void setRollerVoltage(double volts) { - rollerSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); - } + @Override + public void setPivotVoltage(double volts) { + pivotSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } - } + @Override + public void setRollerVoltage(double volts) { + rollerSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 46b2919..93cee93 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -6,14 +6,13 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -24,94 +23,90 @@ /** Add your docs here. */ public class IntakeIOSparkMax implements IntakeIO { - private final CANSparkMax roller = new CANSparkMax(RobotMap.Intake.roller, MotorType.kBrushless); - private final CANSparkMax pivot = new CANSparkMax(RobotMap.Intake.pivot, MotorType.kBrushless); - private final RelativeEncoder rollerEnc = roller.getEncoder(); - private final RelativeEncoder pivotEnc = pivot.getEncoder(); - private final SparkAbsoluteEncoder pivotAbs = pivot.getAbsoluteEncoder(Type.kDutyCycle); - - private final SparkPIDController rollerPID = roller.getPIDController(); - private final SparkPIDController pivotPID = pivot.getPIDController(); - - public IntakeIOSparkMax() { - roller.restoreFactoryDefaults(); - pivot.restoreFactoryDefaults(); - - roller.setSmartCurrentLimit(IntakeConstants.rollerCurrentLimit); - pivot.setSmartCurrentLimit(IntakeConstants.pivotCurrentLimit); - - roller.setIdleMode(IdleMode.kCoast); - pivot.setIdleMode(IdleMode.kCoast); - - rollerEnc.setVelocityConversionFactor(1.0); - - pivotPID.setFeedbackDevice(pivotAbs); - pivotPID.setOutputRange(-12, 12); - - pivotPID.setPositionPIDWrappingEnabled(false); - pivotAbs.setInverted(IntakeConstants.pivotInvert); - pivot.setInverted(IntakeConstants.pivotInvert); - pivotAbs.setPositionConversionFactor(IntakeConstants.pivotAbsConversion); - pivotAbs.setVelocityConversionFactor(IntakeConstants.pivotAbsConversion / 60.0); - pivotAbs.setZeroOffset(IntakeConstants.pivotOffset); - - pivotEnc.setPositionConversionFactor(IntakeConstants.pivotEncConversion); - pivotEnc.setVelocityConversionFactor(IntakeConstants.pivotEncConversion / 60.0); - pivotEnc.setPosition(pivotAbs.getPosition()); - - roller.burnFlash(); - pivot.burnFlash(); - } - - - @Override - public void processInputs(IntakeIOInputsAutoLogged inputs) { - inputs.pivotPosition = Rotation2d.fromRadians(pivotAbs.getPosition()); - inputs.pivotVelocityRadPerSec = pivotEnc.getVelocity(); - inputs.pivotAppliedVolts = pivot.getAppliedOutput() * pivot.getBusVoltage(); - inputs.pivotCurrentAmps = pivot.getOutputCurrent(); - inputs.pivotTempCelsius = pivot.getMotorTemperature(); - - inputs.rollerSpeedRPM = rollerEnc.getVelocity(); - inputs.rollerAppliedVolts = roller.getAppliedOutput() * roller.getBusVoltage(); - inputs.rollerCurrentAmps = roller.getOutputCurrent(); - inputs.rollerTempCelsius = roller.getMotorTemperature(); - - - } - - public void setPivotTarget(double angle, ArmFeedforward ff) { - pivotPID.setReference(angle, ControlType.kPosition, 0, ff.calculate(angle, 0)); - } - - @Override - public void setRollerRPM(int rpm, SimpleMotorFeedforward ff) { - rpm = MathUtil.clamp(rpm, -5880, 5880); - rollerPID.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); - } - - public void setPivotPID(double kP, double kI, double kD) { - pivotPID.setP(kP); - pivotPID.setI(kI); - pivotPID.setD(kD); - pivot.burnFlash(); - } - - public void setRollerPID(double kP, double kI, double kD) { - rollerPID.setP(kP); - rollerPID.setI(kI); - rollerPID.setD(kD); - roller.burnFlash(); - } - - @Override - public void setPivotVoltage(double volts) { - pivot.setVoltage(MathUtil.clamp(volts, -12, 12)); - } - - - @Override - public void setRollerVoltage(double volts) { - roller.setVoltage(MathUtil.clamp(volts, -12, 12)); - } + private final CANSparkMax roller = new CANSparkMax(RobotMap.Intake.roller, MotorType.kBrushless); + private final CANSparkMax pivot = new CANSparkMax(RobotMap.Intake.pivot, MotorType.kBrushless); + private final RelativeEncoder rollerEnc = roller.getEncoder(); + private final RelativeEncoder pivotEnc = pivot.getEncoder(); + private final SparkAbsoluteEncoder pivotAbs = pivot.getAbsoluteEncoder(Type.kDutyCycle); + + private final SparkPIDController rollerPID = roller.getPIDController(); + private final SparkPIDController pivotPID = pivot.getPIDController(); + + public IntakeIOSparkMax() { + roller.restoreFactoryDefaults(); + pivot.restoreFactoryDefaults(); + + roller.setSmartCurrentLimit(IntakeConstants.rollerCurrentLimit); + pivot.setSmartCurrentLimit(IntakeConstants.pivotCurrentLimit); + + roller.setIdleMode(IdleMode.kCoast); + pivot.setIdleMode(IdleMode.kCoast); + + rollerEnc.setVelocityConversionFactor(1.0); + + pivotPID.setFeedbackDevice(pivotAbs); + pivotPID.setOutputRange(-12, 12); + + pivotPID.setPositionPIDWrappingEnabled(false); + pivotAbs.setInverted(IntakeConstants.pivotInvert); + pivot.setInverted(IntakeConstants.pivotInvert); + pivotAbs.setPositionConversionFactor(IntakeConstants.pivotAbsConversion); + pivotAbs.setVelocityConversionFactor(IntakeConstants.pivotAbsConversion / 60.0); + pivotAbs.setZeroOffset(IntakeConstants.pivotOffset); + + pivotEnc.setPositionConversionFactor(IntakeConstants.pivotEncConversion); + pivotEnc.setVelocityConversionFactor(IntakeConstants.pivotEncConversion / 60.0); + pivotEnc.setPosition(pivotAbs.getPosition()); + + roller.burnFlash(); + pivot.burnFlash(); + } + + @Override + public void processInputs(IntakeIOInputsAutoLogged inputs) { + inputs.pivotPosition = Rotation2d.fromRadians(pivotAbs.getPosition()); + inputs.pivotVelocityRadPerSec = pivotEnc.getVelocity(); + inputs.pivotAppliedVolts = pivot.getAppliedOutput() * pivot.getBusVoltage(); + inputs.pivotCurrentAmps = pivot.getOutputCurrent(); + inputs.pivotTempCelsius = pivot.getMotorTemperature(); + + inputs.rollerSpeedRPM = rollerEnc.getVelocity(); + inputs.rollerAppliedVolts = roller.getAppliedOutput() * roller.getBusVoltage(); + inputs.rollerCurrentAmps = roller.getOutputCurrent(); + inputs.rollerTempCelsius = roller.getMotorTemperature(); + } + + public void setPivotTarget(double angle, ArmFeedforward ff) { + pivotPID.setReference(angle, ControlType.kPosition, 0, ff.calculate(angle, 0)); + } + + @Override + public void setRollerRPM(int rpm, SimpleMotorFeedforward ff) { + rpm = MathUtil.clamp(rpm, -5880, 5880); + rollerPID.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); + } + + public void setPivotPID(double kP, double kI, double kD) { + pivotPID.setP(kP); + pivotPID.setI(kI); + pivotPID.setD(kD); + pivot.burnFlash(); + } + + public void setRollerPID(double kP, double kI, double kD) { + rollerPID.setP(kP); + rollerPID.setI(kI); + rollerPID.setD(kD); + roller.burnFlash(); + } + + @Override + public void setPivotVoltage(double volts) { + pivot.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setRollerVoltage(double volts) { + roller.setVoltage(MathUtil.clamp(volts, -12, 12)); + } } diff --git a/src/main/java/frc/robot/subsystems/pivot2/Pivot.java b/src/main/java/frc/robot/subsystems/pivot2/Pivot.java index 6b7033e..bfb189e 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivot2/Pivot.java @@ -4,35 +4,34 @@ package frc.robot.subsystems.pivot2; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.PIDCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.Constants.ShooterConstants; import frc.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; /** Add your docs here. */ public class Pivot extends SubsystemBase { public PivotIO io; public PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); - private ArmFeedforward pivotFF = new ArmFeedforward(0.0, ShooterConstants.kGPivot, ShooterConstants.kVPivot, - ShooterConstants.kAPivot); - private ProfiledPIDController pivotPID = new ProfiledPIDController(0.0, 0.0, 0.0, - new TrapezoidProfile.Constraints(ShooterConstants.maxPivotVelocity, ShooterConstants.maxPivotAccel)); - private LoggedTunableNumber kPPivot = new LoggedTunableNumber("Shooter/kPPivot", ShooterConstants.kPPivot); + private ArmFeedforward pivotFF = + new ArmFeedforward( + 0.0, ShooterConstants.kGPivot, ShooterConstants.kVPivot, ShooterConstants.kAPivot); + private ProfiledPIDController pivotPID = + new ProfiledPIDController( + 0.0, + 0.0, + 0.0, + new TrapezoidProfile.Constraints( + ShooterConstants.maxPivotVelocity, ShooterConstants.maxPivotAccel)); + private LoggedTunableNumber kPPivot = + new LoggedTunableNumber("Shooter/kPPivot", ShooterConstants.kPPivot); public Pivot(PivotIO io) { this.io = io; @@ -49,16 +48,17 @@ public void periodic() { } public Command setPivotTarget(DoubleSupplier radians) { - return this.run(() -> { - double volts = pivotPID.calculate(inputs.pivotPosition.getRadians(), radians.getAsDouble()) - + pivotFF.calculate(pivotPID.getSetpoint().position, - pivotPID.getSetpoint().velocity); - - io.setPivotVoltage(volts); - inputs.pivotAppliedVolts = volts; - inputs.pivotTargetPosition = Rotation2d.fromRadians(radians.getAsDouble()); - - }); + return this.run( + () -> { + double volts = + pivotPID.calculate(inputs.pivotPosition.getRadians(), radians.getAsDouble()) + + pivotFF.calculate( + pivotPID.getSetpoint().position, pivotPID.getSetpoint().velocity); + + io.setPivotVoltage(volts); + inputs.pivotAppliedVolts = volts; + inputs.pivotTargetPosition = Rotation2d.fromRadians(radians.getAsDouble()); + }); } public Command setPivotVoltage(DoubleSupplier volts) { @@ -98,9 +98,10 @@ public boolean isStalled() { public Command runZero() { return this.run( - () -> { - io.setPivotVoltage(-1); - }).until(() -> inputs.pivotStalled).finallyDo(() -> io.resetEncoder()); + () -> { + io.setPivotVoltage(-1); + }) + .until(() -> inputs.pivotStalled) + .finallyDo(() -> io.resetEncoder()); } - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java index 38f50f0..df708e7 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java @@ -4,30 +4,28 @@ package frc.robot.subsystems.pivot2; -import org.littletonrobotics.junction.AutoLog; - -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; /** Add your docs here. */ public interface PivotIO { - @AutoLog - public static class PivotIOInputs { - public Rotation2d pivotPosition = new Rotation2d(); - public Rotation2d pivotRelativeEncoder = new Rotation2d(); - public Rotation2d pivotAbsolutePosition = new Rotation2d(); - public Rotation2d pivotTargetPosition = new Rotation2d(); - public double pivotVelocityRadPerSec = 0.0; - public double pivotAppliedVolts = 0.0; - public double pivotCurrentAmps = 0.0; - public double pivotTempCelsius = 0.0; - public double pivotOffset = 0.0; - public boolean pivotStalled = false; - } + @AutoLog + public static class PivotIOInputs { + public Rotation2d pivotPosition = new Rotation2d(); + public Rotation2d pivotRelativeEncoder = new Rotation2d(); + public Rotation2d pivotAbsolutePosition = new Rotation2d(); + public Rotation2d pivotTargetPosition = new Rotation2d(); + public double pivotVelocityRadPerSec = 0.0; + public double pivotAppliedVolts = 0.0; + public double pivotCurrentAmps = 0.0; + public double pivotTempCelsius = 0.0; + public double pivotOffset = 0.0; + public boolean pivotStalled = false; + } - public abstract void processInputs(final PivotIOInputsAutoLogged inputs); + public abstract void processInputs(final PivotIOInputsAutoLogged inputs); - public abstract void setPivotVoltage(double volts); + public abstract void setPivotVoltage(double volts); - public abstract void resetEncoder(); -} \ No newline at end of file + public abstract void resetEncoder(); +} diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIOReplay.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIOReplay.java index d490692..10472ff 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/PivotIOReplay.java +++ b/src/main/java/frc/robot/subsystems/pivot2/PivotIOReplay.java @@ -4,17 +4,15 @@ package frc.robot.subsystems.pivot2; -import edu.wpi.first.math.controller.ArmFeedforward; - /** Add your docs here. */ public class PivotIOReplay implements PivotIO { - @Override - public void processInputs(PivotIOInputsAutoLogged inputs) {} + @Override + public void processInputs(PivotIOInputsAutoLogged inputs) {} + + @Override + public void setPivotVoltage(double volts) {} - @Override - public void setPivotVoltage(double volts) {} - - @Override - public void resetEncoder() {} -} \ No newline at end of file + @Override + public void resetEncoder() {} +} diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSim.java index 906ed20..4f4b943 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSim.java @@ -1,39 +1,44 @@ package frc.robot.subsystems.pivot2; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants; -import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; public class PivotIOSim implements PivotIO { - private SingleJointedArmSim pivotSim = new SingleJointedArmSim(DCMotor.getNEO(1), ShooterConstants.pivotRatio, ShooterConstants.pivotMOI, ShooterConstants.pivotLength, ShooterConstants.down, ShooterConstants.up, true, ShooterConstants.down); - private Debouncer stallDebouncer = new Debouncer(ShooterConstants.stallTimeout, DebounceType.kRising); + private SingleJointedArmSim pivotSim = + new SingleJointedArmSim( + DCMotor.getNEO(1), + ShooterConstants.pivotRatio, + ShooterConstants.pivotMOI, + ShooterConstants.pivotLength, + ShooterConstants.down, + ShooterConstants.up, + true, + ShooterConstants.down); + private Debouncer stallDebouncer = + new Debouncer(ShooterConstants.stallTimeout, DebounceType.kRising); - @Override - public void processInputs(PivotIOInputsAutoLogged inputs) { - pivotSim.update(Constants.loopPeriodSecs); - - inputs.pivotPosition = Rotation2d.fromRadians(pivotSim.getAngleRads()); - inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getVelocityRadPerSec()); - inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps(); - inputs.pivotStalled = stallDebouncer.calculate(pivotSim.getCurrentDrawAmps() > 20); - } + @Override + public void processInputs(PivotIOInputsAutoLogged inputs) { + pivotSim.update(Constants.loopPeriodSecs); - @Override - public void setPivotVoltage(double volts) { - pivotSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); - } + inputs.pivotPosition = Rotation2d.fromRadians(pivotSim.getAngleRads()); + inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getVelocityRadPerSec()); + inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps(); + inputs.pivotStalled = stallDebouncer.calculate(pivotSim.getCurrentDrawAmps() > 20); + } - @Override - public void resetEncoder() { - } -} \ No newline at end of file + @Override + public void setPivotVoltage(double volts) { + pivotSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void resetEncoder() {} +} diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java index b2fd87c..c5c72e6 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java @@ -4,17 +4,11 @@ package frc.robot.subsystems.pivot2; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; - import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,57 +17,61 @@ /** Add your docs here. */ public class PivotIOSparkMax implements PivotIO { - private final CANSparkMax pivot = new CANSparkMax(RobotMap.Shooter.pivot, MotorType.kBrushless); - private final RelativeEncoder pivotEnc = pivot.getEncoder(); - private final ThroughboreEncoder pivotAbs; + private final CANSparkMax pivot = new CANSparkMax(RobotMap.Shooter.pivot, MotorType.kBrushless); + private final RelativeEncoder pivotEnc = pivot.getEncoder(); + private final ThroughboreEncoder pivotAbs; - private Debouncer stallDebouncer = new Debouncer(ShooterConstants.stallTimeout, DebounceType.kRising); + private Debouncer stallDebouncer = + new Debouncer(ShooterConstants.stallTimeout, DebounceType.kRising); - public PivotIOSparkMax() { - pivot.restoreFactoryDefaults(); - pivotAbs = new ThroughboreEncoder(pivot.getAbsoluteEncoder(), pivot.getAbsoluteEncoder().getPosition()); + public PivotIOSparkMax() { + pivot.restoreFactoryDefaults(); + pivotAbs = + new ThroughboreEncoder( + pivot.getAbsoluteEncoder(), pivot.getAbsoluteEncoder().getPosition()); - pivot.setSmartCurrentLimit(ShooterConstants.pivotCurrentLimit); - pivot.setIdleMode(IdleMode.kCoast); + pivot.setSmartCurrentLimit(ShooterConstants.pivotCurrentLimit); + pivot.setIdleMode(IdleMode.kCoast); - pivotAbs.abs.setInverted(true); - pivot.setInverted(true); - pivotAbs.abs.setPositionConversionFactor(ShooterConstants.pivotAbsConversion); - pivotAbs.abs.setVelocityConversionFactor(ShooterConstants.pivotAbsConversion / 60.0); + pivotAbs.abs.setInverted(true); + pivot.setInverted(true); + pivotAbs.abs.setPositionConversionFactor(ShooterConstants.pivotAbsConversion); + pivotAbs.abs.setVelocityConversionFactor(ShooterConstants.pivotAbsConversion / 60.0); - pivotEnc.setPositionConversionFactor(ShooterConstants.pivotEncConversion); - pivotEnc.setVelocityConversionFactor(ShooterConstants.pivotEncConversion / 60.0); + pivotEnc.setPositionConversionFactor(ShooterConstants.pivotEncConversion); + pivotEnc.setVelocityConversionFactor(ShooterConstants.pivotEncConversion / 60.0); - pivotEnc.setPosition(pivotAbs.abs.getPosition()); + pivotEnc.setPosition(pivotAbs.abs.getPosition()); - pivot.burnFlash(); + pivot.burnFlash(); - if ((Math.abs(pivotAbs.getPosition()) % (2 * Math.PI)) > 0.1) { - pivotAbs.setOffset(pivotAbs.getOffset() + pivotAbs.getPosition()); - } + if ((Math.abs(pivotAbs.getPosition()) % (2 * Math.PI)) > 0.1) { + pivotAbs.setOffset(pivotAbs.getOffset() + pivotAbs.getPosition()); } - - @Override - public void processInputs(PivotIOInputsAutoLogged inputs) { - inputs.pivotPosition = Rotation2d.fromRadians(pivotAbs.getPosition()); - inputs.pivotAbsolutePosition = Rotation2d.fromRadians(pivotAbs.getPosition()); - inputs.pivotRelativeEncoder = Rotation2d.fromRadians(pivotEnc.getPosition()); - inputs.pivotVelocityRadPerSec = pivotEnc.getVelocity(); - inputs.pivotAppliedVolts = pivot.getAppliedOutput() * pivot.getBusVoltage(); - inputs.pivotCurrentAmps = pivot.getOutputCurrent(); - inputs.pivotTempCelsius = pivot.getMotorTemperature(); - inputs.pivotOffset = pivotAbs.getOffset(); - inputs.pivotStalled = stallDebouncer.calculate((pivot.getOutputCurrent() > 15) && (Math.abs(pivotEnc.getVelocity()) < 0.02)); - } - - @Override - public void setPivotVoltage(double volts) { - pivot.setVoltage(MathUtil.clamp(volts, -12, 12)); - } - - @Override - public void resetEncoder() { - pivotAbs.setOffset(pivotAbs.abs.getPosition()); - } - + } + + @Override + public void processInputs(PivotIOInputsAutoLogged inputs) { + inputs.pivotPosition = Rotation2d.fromRadians(pivotAbs.getPosition()); + inputs.pivotAbsolutePosition = Rotation2d.fromRadians(pivotAbs.getPosition()); + inputs.pivotRelativeEncoder = Rotation2d.fromRadians(pivotEnc.getPosition()); + inputs.pivotVelocityRadPerSec = pivotEnc.getVelocity(); + inputs.pivotAppliedVolts = pivot.getAppliedOutput() * pivot.getBusVoltage(); + inputs.pivotCurrentAmps = pivot.getOutputCurrent(); + inputs.pivotTempCelsius = pivot.getMotorTemperature(); + inputs.pivotOffset = pivotAbs.getOffset(); + inputs.pivotStalled = + stallDebouncer.calculate( + (pivot.getOutputCurrent() > 15) && (Math.abs(pivotEnc.getVelocity()) < 0.02)); + } + + @Override + public void setPivotVoltage(double volts) { + pivot.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void resetEncoder() { + pivotAbs.setOffset(pivotAbs.abs.getPosition()); + } } diff --git a/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java b/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java index a234e57..88e5960 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java +++ b/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java @@ -3,27 +3,27 @@ import com.revrobotics.SparkAbsoluteEncoder; public class ThroughboreEncoder { - SparkAbsoluteEncoder abs; - double offset; + SparkAbsoluteEncoder abs; + double offset; - public ThroughboreEncoder(SparkAbsoluteEncoder absoluteEncoder, double absOffset) { - abs = absoluteEncoder; - offset = absOffset; - } + public ThroughboreEncoder(SparkAbsoluteEncoder absoluteEncoder, double absOffset) { + abs = absoluteEncoder; + offset = absOffset; + } - public double getPosition() { - return (abs.getPosition() - offset) % (2 * Math.PI); - } + public double getPosition() { + return (abs.getPosition() - offset) % (2 * Math.PI); + } - public double getAbsPosition() { - return abs.getPosition(); - } + public double getAbsPosition() { + return abs.getPosition(); + } - public void setOffset(double absOffset) { - offset = absOffset; - } + public void setOffset(double absOffset) { + offset = absOffset; + } - public double getOffset() { - return offset; - } + public double getOffset() { + return offset; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b4f75d8..4b40257 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,56 +4,55 @@ package frc.robot.subsystems.shooter; -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.ShooterConstants; import frc.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import org.littletonrobotics.junction.Logger; /** Add your docs here. */ public class Shooter extends SubsystemBase { - public ShooterIO io; - public ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - - private SimpleMotorFeedforward shooterFF; - - private LoggedTunableNumber kPShooter = new LoggedTunableNumber("Shooter/kPShooter"); - private LoggedTunableNumber kIShooter = new LoggedTunableNumber("Shooter/kIShooter"); - private LoggedTunableNumber kSShooter = new LoggedTunableNumber("Shooter/kSShooter"); - - - public Shooter(ShooterIO io) { - this.io = io; - - switch (Constants.currentMode) { - case REAL: - kPShooter.initDefault(ShooterConstants.kPShooterReal); - kIShooter.initDefault(ShooterConstants.kIShooterReal); - kSShooter.initDefault(ShooterConstants.kSShooterReal); - break; - case SIM: - kPShooter.initDefault(ShooterConstants.kPShooterSim); - kIShooter.initDefault(ShooterConstants.kIShooterSim); - kSShooter.initDefault(ShooterConstants.kSShooterSim); - break; - case REPLAY: - kPShooter.initDefault(ShooterConstants.kPShooterReplay); - kIShooter.initDefault(ShooterConstants.kIShooterReplay); - kSShooter.initDefault(ShooterConstants.kSShooterReplay); - break; - default: - kPShooter.initDefault(0.0); - kIShooter.initDefault(0.0); - kSShooter.initDefault(0.0); - } - io.setShooterPID(kPShooter.getAsDouble(), kIShooter.getAsDouble(), 0.0); - shooterFF = new SimpleMotorFeedforward(kSShooter.getAsDouble(), ShooterConstants.kVShooter, ShooterConstants.kAShooter); + public ShooterIO io; + public ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + + private SimpleMotorFeedforward shooterFF; + + private LoggedTunableNumber kPShooter = new LoggedTunableNumber("Shooter/kPShooter"); + private LoggedTunableNumber kIShooter = new LoggedTunableNumber("Shooter/kIShooter"); + private LoggedTunableNumber kSShooter = new LoggedTunableNumber("Shooter/kSShooter"); + + public Shooter(ShooterIO io) { + this.io = io; + + switch (Constants.currentMode) { + case REAL: + kPShooter.initDefault(ShooterConstants.kPShooterReal); + kIShooter.initDefault(ShooterConstants.kIShooterReal); + kSShooter.initDefault(ShooterConstants.kSShooterReal); + break; + case SIM: + kPShooter.initDefault(ShooterConstants.kPShooterSim); + kIShooter.initDefault(ShooterConstants.kIShooterSim); + kSShooter.initDefault(ShooterConstants.kSShooterSim); + break; + case REPLAY: + kPShooter.initDefault(ShooterConstants.kPShooterReplay); + kIShooter.initDefault(ShooterConstants.kIShooterReplay); + kSShooter.initDefault(ShooterConstants.kSShooterReplay); + break; + default: + kPShooter.initDefault(0.0); + kIShooter.initDefault(0.0); + kSShooter.initDefault(0.0); + } + io.setShooterPID(kPShooter.getAsDouble(), kIShooter.getAsDouble(), 0.0); + shooterFF = + new SimpleMotorFeedforward( + kSShooter.getAsDouble(), ShooterConstants.kVShooter, ShooterConstants.kAShooter); } public void periodic() { @@ -63,60 +62,53 @@ public void periodic() { public Command setRPM(IntSupplier rpm, double differential) { return this.run( - () -> { - io.setRPM(rpm.getAsInt(), shooterFF, differential); - inputs.leftTargetRPM = rpm.getAsInt(); - inputs.rightTargetRPM = rpm.getAsInt() * differential; - } - ); + () -> { + io.setRPM(rpm.getAsInt(), shooterFF, differential); + inputs.leftTargetRPM = rpm.getAsInt(); + inputs.rightTargetRPM = rpm.getAsInt() * differential; + }); } public Command setLeftRPM(IntSupplier rpm) { return this.run( - () -> { - io.setLeftRPM(rpm.getAsInt(), shooterFF); - inputs.leftTargetRPM = rpm.getAsInt(); - } - ); + () -> { + io.setLeftRPM(rpm.getAsInt(), shooterFF); + inputs.leftTargetRPM = rpm.getAsInt(); + }); } public Command setLeftVoltage(DoubleSupplier volts) { return this.run( - () -> { - io.setLeftVoltage(volts.getAsDouble()); - } - ); + () -> { + io.setLeftVoltage(volts.getAsDouble()); + }); } public Command setRightVoltage(DoubleSupplier volts) { return this.run( - () -> { - io.setRightVoltage(volts.getAsDouble()); - } - ); + () -> { + io.setRightVoltage(volts.getAsDouble()); + }); } public Command setRightRPM(IntSupplier rpm) { return this.run( - () -> { - io.setRightRPM(rpm.getAsInt(), shooterFF); - inputs.rightTargetRPM = rpm.getAsInt(); - } - ); + () -> { + io.setRightRPM(rpm.getAsInt(), shooterFF); + inputs.rightTargetRPM = rpm.getAsInt(); + }); } public Command stopShooter() { - return this.run( - () -> { - io.setRightVoltage(0); - io.setLeftVoltage(0); - } - ); + return this.run( + () -> { + io.setRightVoltage(0); + io.setLeftVoltage(0); + }); } public boolean atSetpoint() { - return Math.abs(inputs.leftTargetRPM - inputs.leftSpeedRPM) <= (0.05 * inputs.leftTargetRPM) && - Math.abs(inputs.rightTargetRPM - inputs.rightSpeedRPM) <= (0.05 * inputs.rightTargetRPM); -} - + return Math.abs(inputs.leftTargetRPM - inputs.leftSpeedRPM) <= (0.05 * inputs.leftTargetRPM) + && Math.abs(inputs.rightTargetRPM - inputs.rightSpeedRPM) <= (0.05 * inputs.rightTargetRPM); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 0eba80f..d0886b8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -4,39 +4,37 @@ package frc.robot.subsystems.shooter; -import org.littletonrobotics.junction.AutoLog; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import org.littletonrobotics.junction.AutoLog; /** Add your docs here. */ public interface ShooterIO { - @AutoLog - public static class ShooterIOInputs { - public double leftSpeedRPM = 0.0; - public double leftTargetRPM = 0.0; - public double leftAppliedVolts = 0.0; - public double leftCurrentAmps = 0.0; - public double leftTempCelsius = 0.0; + @AutoLog + public static class ShooterIOInputs { + public double leftSpeedRPM = 0.0; + public double leftTargetRPM = 0.0; + public double leftAppliedVolts = 0.0; + public double leftCurrentAmps = 0.0; + public double leftTempCelsius = 0.0; - public double rightSpeedRPM = 0.0; - public double rightTargetRPM = 0.0; - public double rightAppliedVolts = 0.0; - public double rightCurrentAmps = 0.0; - public double rightTempCelsius = 0.0; - } + public double rightSpeedRPM = 0.0; + public double rightTargetRPM = 0.0; + public double rightAppliedVolts = 0.0; + public double rightCurrentAmps = 0.0; + public double rightTempCelsius = 0.0; + } - public abstract void processInputs(final ShooterIOInputsAutoLogged inputs); + public abstract void processInputs(final ShooterIOInputsAutoLogged inputs); - public abstract void setLeftVoltage(double volts); + public abstract void setLeftVoltage(double volts); - public abstract void setRightVoltage(double volts); + public abstract void setRightVoltage(double volts); - public abstract void setRPM(int rpm, SimpleMotorFeedforward ff, double differential); + public abstract void setRPM(int rpm, SimpleMotorFeedforward ff, double differential); - public abstract void setLeftRPM(int rpm, SimpleMotorFeedforward ff); + public abstract void setLeftRPM(int rpm, SimpleMotorFeedforward ff); - public abstract void setRightRPM(int rpm, SimpleMotorFeedforward ff); + public abstract void setRightRPM(int rpm, SimpleMotorFeedforward ff); - public abstract void setShooterPID(double kP, double kI, double kD); - -} + public abstract void setShooterPID(double kP, double kI, double kD); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReplay.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReplay.java index e9a52a8..281b80f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReplay.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReplay.java @@ -9,27 +9,24 @@ /** Add your docs here. */ public class ShooterIOReplay implements ShooterIO { - @Override - public void processInputs(ShooterIOInputsAutoLogged inputs) {} + @Override + public void processInputs(ShooterIOInputsAutoLogged inputs) {} - @Override - public void setLeftVoltage(double volts) {} + @Override + public void setLeftVoltage(double volts) {} - @Override - public void setRightVoltage(double volts) {} + @Override + public void setRightVoltage(double volts) {} - @Override - public void setRPM(int rpm, SimpleMotorFeedforward ff, double differential) {} + @Override + public void setRPM(int rpm, SimpleMotorFeedforward ff, double differential) {} - @Override - public void setLeftRPM(int rpm, SimpleMotorFeedforward ff) {} + @Override + public void setLeftRPM(int rpm, SimpleMotorFeedforward ff) {} - @Override - public void setRightRPM(int rpm, SimpleMotorFeedforward ff) {} + @Override + public void setRightRPM(int rpm, SimpleMotorFeedforward ff) {} - @Override - public void setShooterPID(double kP, double kI, double kD) {} - - - -} + @Override + public void setShooterPID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index c02c811..17ab896 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -9,58 +9,55 @@ import frc.robot.Constants.ShooterConstants; public class ShooterIOSim implements ShooterIO { - private FlywheelSim leftSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, ShooterConstants.shooterMOI); - private FlywheelSim rightSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, ShooterConstants.shooterMOI); - - private PIDController leftPID = new PIDController(ShooterConstants.kPShooterSim, 0, 0); - private PIDController rightPID = new PIDController(ShooterConstants.kPShooterSim, 0, 0); - - @Override - public void processInputs(ShooterIOInputsAutoLogged inputs) { - leftSim.update(Constants.loopPeriodSecs); - rightSim.update(Constants.loopPeriodSecs); - - inputs.leftSpeedRPM = leftSim.getAngularVelocityRPM(); - inputs.leftCurrentAmps = leftSim.getCurrentDrawAmps(); - - inputs.rightSpeedRPM = rightSim.getAngularVelocityRPM(); - inputs.rightCurrentAmps = rightSim.getCurrentDrawAmps(); - } - - - @Override - public void setLeftVoltage(double volts) { - leftSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); - - } - - @Override - public void setRightVoltage(double volts) { - rightSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); - - } - - @Override - public void setRPM(int rpm, SimpleMotorFeedforward ff, double differential) { - setLeftRPM(rpm, ff); - setRightRPM((int) (rpm * differential), ff); - } - - @Override - public void setLeftRPM(int rpm, SimpleMotorFeedforward ff) { - setLeftVoltage(ff.calculate(rpm)); - } - - @Override - public void setRightRPM(int rpm, SimpleMotorFeedforward ff) { - setRightVoltage(ff.calculate(rpm)); - - } - - @Override - public void setShooterPID(double kP, double kI, double kD) { - leftPID = new PIDController(kP, kI, kD); - rightPID = new PIDController(kP, kI, kD); - } - + private FlywheelSim leftSim = + new FlywheelSim(DCMotor.getNEO(1), 1.0, ShooterConstants.shooterMOI); + private FlywheelSim rightSim = + new FlywheelSim(DCMotor.getNEO(1), 1.0, ShooterConstants.shooterMOI); + + private PIDController leftPID = new PIDController(ShooterConstants.kPShooterSim, 0, 0); + private PIDController rightPID = new PIDController(ShooterConstants.kPShooterSim, 0, 0); + + @Override + public void processInputs(ShooterIOInputsAutoLogged inputs) { + leftSim.update(Constants.loopPeriodSecs); + rightSim.update(Constants.loopPeriodSecs); + + inputs.leftSpeedRPM = leftSim.getAngularVelocityRPM(); + inputs.leftCurrentAmps = leftSim.getCurrentDrawAmps(); + + inputs.rightSpeedRPM = rightSim.getAngularVelocityRPM(); + inputs.rightCurrentAmps = rightSim.getCurrentDrawAmps(); + } + + @Override + public void setLeftVoltage(double volts) { + leftSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setRightVoltage(double volts) { + rightSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setRPM(int rpm, SimpleMotorFeedforward ff, double differential) { + setLeftRPM(rpm, ff); + setRightRPM((int) (rpm * differential), ff); + } + + @Override + public void setLeftRPM(int rpm, SimpleMotorFeedforward ff) { + setLeftVoltage(ff.calculate(rpm)); + } + + @Override + public void setRightRPM(int rpm, SimpleMotorFeedforward ff) { + setRightVoltage(ff.calculate(rpm)); + } + + @Override + public void setShooterPID(double kP, double kI, double kD) { + leftPID = new PIDController(kP, kI, kD); + rightPID = new PIDController(kP, kI, kD); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index d71489a..f10c433 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -4,100 +4,96 @@ package frc.robot.subsystems.shooter; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import frc.robot.Constants.RobotMap; /** Add your docs here. */ public class ShooterIOSparkMax implements ShooterIO { - private final CANSparkMax left = new CANSparkMax(RobotMap.Shooter.left, MotorType.kBrushless); - private final RelativeEncoder leftEnc = left.getEncoder(); - private final SparkPIDController leftPID = left.getPIDController(); - - private final CANSparkMax right = new CANSparkMax(RobotMap.Shooter.right, MotorType.kBrushless); - private final RelativeEncoder rightEnc = right.getEncoder(); - private final SparkPIDController rightPID = right.getPIDController(); - - public ShooterIOSparkMax() { - left.restoreFactoryDefaults(); - right.restoreFactoryDefaults(); - - left.setSmartCurrentLimit(40); - right.setSmartCurrentLimit(40); - - left.setIdleMode(IdleMode.kCoast); - right.setIdleMode(IdleMode.kCoast); - - left.setSmartCurrentLimit(40); - right.setSmartCurrentLimit(40); - - left.setInverted(true); - - left.burnFlash(); - right.burnFlash(); - } - - @Override - public void processInputs(ShooterIOInputsAutoLogged inputs) { - inputs.leftSpeedRPM = leftEnc.getVelocity(); - inputs.leftAppliedVolts = left.getAppliedOutput() * left.getBusVoltage(); - inputs.leftCurrentAmps = left.getOutputCurrent(); - inputs.leftTempCelsius = left.getMotorTemperature(); - - inputs.rightSpeedRPM = rightEnc.getVelocity(); - inputs.rightAppliedVolts = right.getAppliedOutput() * left.getBusVoltage(); - inputs.rightCurrentAmps = right.getOutputCurrent(); - inputs.rightTempCelsius = right.getMotorTemperature(); - } - - @Override - public void setLeftVoltage(double volts) { - left.setVoltage(MathUtil.clamp(volts, -12, 12)); - } - - @Override - public void setRightVoltage(double volts) { - right.setVoltage(MathUtil.clamp(volts, -12, 12)); - } - - public void setRPM(int rpm, SimpleMotorFeedforward ff, double differential) { - setLeftRPM(rpm, ff); - setRightRPM((int) (rpm * differential), ff); - } - - @Override - public void setLeftRPM(int rpm, SimpleMotorFeedforward ff) { - rpm = MathUtil.clamp(rpm, -5880, 5880); - leftPID.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); - - } - - @Override - public void setRightRPM(int rpm, SimpleMotorFeedforward ff) { - rpm = MathUtil.clamp(rpm, -5880, 5880); - rightPID.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); - - } - - @Override - public void setShooterPID(double kP, double kI, double kD) { - leftPID.setP(kP); - leftPID.setI(kI); - leftPID.setD(kD); - left.burnFlash(); - - rightPID.setP(kP); - rightPID.setI(kI); - rightPID.setD(kD); - right.burnFlash(); - } - + private final CANSparkMax left = new CANSparkMax(RobotMap.Shooter.left, MotorType.kBrushless); + private final RelativeEncoder leftEnc = left.getEncoder(); + private final SparkPIDController leftPID = left.getPIDController(); + + private final CANSparkMax right = new CANSparkMax(RobotMap.Shooter.right, MotorType.kBrushless); + private final RelativeEncoder rightEnc = right.getEncoder(); + private final SparkPIDController rightPID = right.getPIDController(); + + public ShooterIOSparkMax() { + left.restoreFactoryDefaults(); + right.restoreFactoryDefaults(); + + left.setSmartCurrentLimit(40); + right.setSmartCurrentLimit(40); + + left.setIdleMode(IdleMode.kCoast); + right.setIdleMode(IdleMode.kCoast); + + left.setSmartCurrentLimit(40); + right.setSmartCurrentLimit(40); + + left.setInverted(true); + + left.burnFlash(); + right.burnFlash(); + } + + @Override + public void processInputs(ShooterIOInputsAutoLogged inputs) { + inputs.leftSpeedRPM = leftEnc.getVelocity(); + inputs.leftAppliedVolts = left.getAppliedOutput() * left.getBusVoltage(); + inputs.leftCurrentAmps = left.getOutputCurrent(); + inputs.leftTempCelsius = left.getMotorTemperature(); + + inputs.rightSpeedRPM = rightEnc.getVelocity(); + inputs.rightAppliedVolts = right.getAppliedOutput() * left.getBusVoltage(); + inputs.rightCurrentAmps = right.getOutputCurrent(); + inputs.rightTempCelsius = right.getMotorTemperature(); + } + + @Override + public void setLeftVoltage(double volts) { + left.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + @Override + public void setRightVoltage(double volts) { + right.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + public void setRPM(int rpm, SimpleMotorFeedforward ff, double differential) { + setLeftRPM(rpm, ff); + setRightRPM((int) (rpm * differential), ff); + } + + @Override + public void setLeftRPM(int rpm, SimpleMotorFeedforward ff) { + rpm = MathUtil.clamp(rpm, -5880, 5880); + leftPID.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); + } + + @Override + public void setRightRPM(int rpm, SimpleMotorFeedforward ff) { + rpm = MathUtil.clamp(rpm, -5880, 5880); + rightPID.setReference(rpm, ControlType.kVelocity, 0, ff.calculate(rpm), ArbFFUnits.kVoltage); + } + + @Override + public void setShooterPID(double kP, double kI, double kD) { + leftPID.setP(kP); + leftPID.setI(kI); + leftPID.setD(kD); + left.burnFlash(); + + rightPID.setP(kP); + rightPID.setI(kI); + rightPID.setD(kD); + right.burnFlash(); + } } diff --git a/src/main/java/frc/util/LoggedTunableNumber.java b/src/main/java/frc/util/LoggedTunableNumber.java index 291710e..a679274 100644 --- a/src/main/java/frc/util/LoggedTunableNumber.java +++ b/src/main/java/frc/util/LoggedTunableNumber.java @@ -7,6 +7,7 @@ package frc.util; +import frc.robot.Constants; import java.util.Arrays; import java.util.HashMap; import java.util.Map; @@ -14,8 +15,6 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; -import frc.robot.Constants; - /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. @@ -121,4 +120,4 @@ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tun public double getAsDouble() { return get(); } -} \ No newline at end of file +}