diff --git a/src/main/java/frc/team5115/Constants.java b/src/main/java/frc/team5115/Constants.java index 8e308c3..87df295 100644 --- a/src/main/java/frc/team5115/Constants.java +++ b/src/main/java/frc/team5115/Constants.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115; import edu.wpi.first.math.geometry.Rotation2d; @@ -20,14 +7,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ public final class Constants { private static final boolean isReplay = false; public static final Mode currentMode = @@ -48,9 +27,9 @@ public static enum Mode { public static final byte SNOWBLOWER_MOTOR_ID = 21; public static final byte ARM_LEFT_MOTOR_ID = 3; public static final byte ARM_RIGHT_MOTOR_ID = 33; - public static final byte SHOOTER_RIGHT_MOTOR_ID = 27; - public static final byte SHOOTER_LEFT_MOTOR_ID = 20; - public static final byte SHOTER_AUX_MOTOR_ID = 35; + public static final byte FEEDER_RIGHT_MOTOR_ID = 27; + public static final byte FEEDER_LEFT_MOTOR_ID = 20; + public static final byte SHOOTER_MOTOR_ID = 35; public static final byte CLIMBER_LEFT_MOTOR_ID = 30; public static final byte CLIMBER_RIGHT_MOTOR_ID = 31; diff --git a/src/main/java/frc/team5115/Main.java b/src/main/java/frc/team5115/Main.java index 23d3522..f6a7ca8 100644 --- a/src/main/java/frc/team5115/Main.java +++ b/src/main/java/frc/team5115/Main.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115; import edu.wpi.first.wpilibj.RobotBase; diff --git a/src/main/java/frc/team5115/Robot.java b/src/main/java/frc/team5115/Robot.java index 5f927d1..cf41f23 100644 --- a/src/main/java/frc/team5115/Robot.java +++ b/src/main/java/frc/team5115/Robot.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115; import edu.wpi.first.wpilibj2.command.Command; @@ -104,6 +91,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + robotContainer.robotPeriodic(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/team5115/RobotContainer.java b/src/main/java/frc/team5115/RobotContainer.java index 228599e..33d58d3 100644 --- a/src/main/java/frc/team5115/RobotContainer.java +++ b/src/main/java/frc/team5115/RobotContainer.java @@ -1,28 +1,20 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.team5115.commands.AutoCommands; import frc.team5115.commands.DriveCommands; +import frc.team5115.subsystems.amper.Amper; +import frc.team5115.subsystems.amper.AmperIO; +import frc.team5115.subsystems.amper.AmperIOSim; +import frc.team5115.subsystems.amper.AmperIOSparkMax; import frc.team5115.subsystems.arm.Arm; import frc.team5115.subsystems.arm.ArmIO; import frc.team5115.subsystems.arm.ArmIOSim; @@ -33,6 +25,14 @@ import frc.team5115.subsystems.drive.ModuleIO; import frc.team5115.subsystems.drive.ModuleIOSim; import frc.team5115.subsystems.drive.ModuleIOSparkMax; +import frc.team5115.subsystems.feeder.Feeder; +import frc.team5115.subsystems.feeder.FeederIO; +import frc.team5115.subsystems.feeder.FeederIOSim; +import frc.team5115.subsystems.feeder.FeederIOSparkMax; +import frc.team5115.subsystems.intake.Intake; +import frc.team5115.subsystems.intake.IntakeIO; +import frc.team5115.subsystems.intake.IntakeIOSim; +import frc.team5115.subsystems.intake.IntakeIOSparkMax; import frc.team5115.subsystems.shooter.Shooter; import frc.team5115.subsystems.shooter.ShooterIO; import frc.team5115.subsystems.shooter.ShooterIOSim; @@ -51,23 +51,33 @@ public class RobotContainer { private final GyroIO gyro; private final Drivetrain drivetrain; private final PhotonVision vision; - private final Shooter shooter; private final Arm arm; + private final Amper amper; + private final Intake intake; + private final Feeder feeder; + private final Shooter shooter; // Controller private final CommandXboxController joyDrive = new CommandXboxController(0); + private final CommandXboxController joyManip = new CommandXboxController(1); // Dashboard inputs private final LoggedDashboardChooser autoChooser; + // Shuffleboard + private final GenericEntry noteDetectedEntry; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations gyro = new GyroIONavx(); - shooter = new Shooter(new ShooterIOSparkMax()); arm = new Arm(new ArmIOSparkMax()); + amper = new Amper(new AmperIOSparkMax()); + intake = new Intake(new IntakeIOSparkMax()); + feeder = new Feeder(new FeederIOSparkMax()); + shooter = new Shooter(new ShooterIOSparkMax()); drivetrain = new Drivetrain( gyro, @@ -75,35 +85,46 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); - vision = new PhotonVision(drivetrain); + // vision = new PhotonVision(drivetrain); + vision = null; + noteDetectedEntry = + Shuffleboard.getTab("SmartDashboard").add("Has note?", false).getEntry(); break; case SIM: // Sim robot, instantiate physics sim IO implementations gyro = new GyroIO() {}; - shooter = new Shooter(new ShooterIOSim()); arm = new Arm(new ArmIOSim()); + amper = new Amper(new AmperIOSim()); + intake = new Intake(new IntakeIOSim()); + feeder = new Feeder(new FeederIOSim()); + shooter = new Shooter(new ShooterIOSim()); drivetrain = new Drivetrain( gyro, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); vision = null; + noteDetectedEntry = null; break; default: // Replayed robot, disable IO implementations gyro = new GyroIO() {}; - shooter = new Shooter(new ShooterIO() {}); arm = new Arm(new ArmIO() {}); + amper = new Amper(new AmperIO() {}); + intake = new Intake(new IntakeIO() {}); + feeder = new Feeder(new FeederIO() {}); + shooter = new Shooter(new ShooterIO() {}); drivetrain = new Drivetrain( gyro, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); vision = null; + noteDetectedEntry = null; break; } // Register auto commands for pathplanner // PhotonVision is passed in here to prevent warnings, i.e. "unused variable: vision" - AutoCommands.registerCommands(shooter, arm, drivetrain, vision); + registerCommands(drivetrain, vision, arm, amper, intake, feeder, shooter); // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -117,33 +138,26 @@ public RobotContainer() { // drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); // autoChooser.addOption( // "Drive SysId (Dynamic Forward)", - // drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); // autoChooser.addOption( // "Drive SysId (Dynamic Reverse)", - // drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - + // drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // autoChooser.addOption( - // "Shooter Aux SysId (Quasistatic Forward)", + // "Shooter SysId (Quasistatic Forward)", // shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); // autoChooser.addOption( - // "Shooter Aux SysId (Quasistatic Reverse)", + // "Shooter SysId (Quasistatic Reverse)", // shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); // autoChooser.addOption( - // "Shooter Aux SysId (Dynamic Forward)", + // "Shooter SysId (Dynamic Forward)", // shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); // autoChooser.addOption( - // "Shooter Aux SysId (Dynamic Reverse)", + // "Shooter SysId (Dynamic Reverse)", // shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - // 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() { drivetrain.setDefaultCommand( DriveCommands.joystickDrive( @@ -155,17 +169,63 @@ private void configureButtonBindings() { // joyDrive.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); joyDrive.start().onTrue(resetFieldOrientation()); - joyDrive.back().onTrue(shooter.vomit()).onFalse(shooter.stop()); - joyDrive.a().onTrue(DriveCommands.intakeUntilNote(shooter, arm)); - joyDrive.y().onTrue(DriveCommands.stowArm(shooter, arm)); - joyDrive + joyManip + .back() + .onTrue(DriveCommands.vomit(intake, feeder, shooter)) + .onFalse(DriveCommands.forceStop(intake, feeder, shooter)); + + joyManip.a().onTrue(DriveCommands.intakeUntilNote(arm, intake, feeder)); + + joyManip + .y() + .onTrue( + Commands.parallel(arm.stow(), shooter.stop(), feeder.stop(), intake.stop()) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + joyManip .b() - .onTrue(DriveCommands.prepareShoot(shooter, arm, 15, true)) - .onFalse(DriveCommands.triggerShoot(shooter)); - joyDrive + .onTrue(DriveCommands.prepareShoot(arm, intake, feeder, shooter, 15, 5000)) + .onFalse( + DriveCommands.feed(intake, feeder) + .andThen(shooter.stop()) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + + joyManip .x() - .onTrue(DriveCommands.prepareAmp(shooter, arm)) - .onFalse(DriveCommands.triggerAmp(shooter, arm)); + .onTrue(DriveCommands.prepareAmp(arm, amper, intake, feeder)) + .onFalse(DriveCommands.triggerAmp(arm, amper, intake, feeder)); + } + + public void robotPeriodic() { + if (noteDetectedEntry != null) { + noteDetectedEntry.setBoolean(feeder.noteDetected()); + } + } + + /** + * Registers commands for pathplanner to use in autos + * + * @param shooter the shooter subsystem + * @param arm the arm subsystem + * @param drivetrain the drivetrain subsytem (not currently used) + * @param photonVision the photonvision subsystem (not currently used) + */ + public static void registerCommands( + Drivetrain drivetrain, + PhotonVision vision, + Arm arm, + Amper amper, + Intake intake, + Feeder feeder, + Shooter shooter) { + NamedCommands.registerCommand( + "InitialShoot", + DriveCommands.prepareShoot(arm, intake, feeder, shooter, 15, 5000) + .andThen(DriveCommands.feed(intake, feeder))); + NamedCommands.registerCommand("Intake", DriveCommands.intakeUntilNote(arm, intake, feeder)); + NamedCommands.registerCommand("Shoot", DriveCommands.feed(intake, feeder)); + NamedCommands.registerCommand( + "PrepareClose", DriveCommands.prepareShoot(arm, intake, feeder, shooter, 15, 5000)); } /** diff --git a/src/main/java/frc/team5115/commands/AutoCommands.java b/src/main/java/frc/team5115/commands/AutoCommands.java deleted file mode 100644 index 43f11bf..0000000 --- a/src/main/java/frc/team5115/commands/AutoCommands.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.team5115.commands; - -import com.pathplanner.lib.auto.NamedCommands; -import frc.team5115.subsystems.arm.Arm; -import frc.team5115.subsystems.drive.Drivetrain; -import frc.team5115.subsystems.shooter.Shooter; -import frc.team5115.subsystems.vision.PhotonVision; - -public class AutoCommands { - private AutoCommands() {} - - /** - * Registers commands for pathplanner to use in autos - * - * @param shooter the shooter subsystem - * @param arm the arm subsystem - * @param drivetrain the drivetrain subsytem (not currently used) - * @param photonVision the photonvision subsystem (not currently used) - */ - public static void registerCommands( - Shooter shooter, Arm arm, Drivetrain drivetrain, PhotonVision photonVision) { - NamedCommands.registerCommand( - "InitialShoot", - DriveCommands.prepareShoot(shooter, arm, 15, false) - .andThen(DriveCommands.triggerShoot(shooter))); - NamedCommands.registerCommand("Intake", DriveCommands.intakeUntilNote(shooter, arm)); - NamedCommands.registerCommand("Shoot", DriveCommands.triggerShoot(shooter)); - NamedCommands.registerCommand( - "PrepareClose", DriveCommands.prepareShoot(shooter, arm, 15, false)); - } -} diff --git a/src/main/java/frc/team5115/commands/DriveCommands.java b/src/main/java/frc/team5115/commands/DriveCommands.java index 5f4c699..16617a3 100644 --- a/src/main/java/frc/team5115/commands/DriveCommands.java +++ b/src/main/java/frc/team5115/commands/DriveCommands.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.commands; import edu.wpi.first.math.MathUtil; @@ -23,8 +10,11 @@ import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import frc.team5115.Constants.SwerveConstants; +import frc.team5115.subsystems.amper.Amper; import frc.team5115.subsystems.arm.Arm; import frc.team5115.subsystems.drive.Drivetrain; +import frc.team5115.subsystems.feeder.Feeder; +import frc.team5115.subsystems.intake.Intake; import frc.team5115.subsystems.shooter.Shooter; import java.util.function.DoubleSupplier; @@ -33,73 +23,79 @@ public class DriveCommands { private DriveCommands() {} - public static Command intakeUntilNote(Shooter shooter, Arm arm) { + public static Command intakeUntilNote(Arm arm, Intake intake, Feeder feeder) { return Commands.sequence( arm.goToAngle(Rotation2d.fromDegrees(0), 1), - shooter.intake(), - shooter.centerNote(), - shooter.waitForDetectionState(true, 20), + intake.intake(), + feeder.centerNote(), + feeder.waitForDetectionState(true, 20), Commands.waitSeconds(0.25), - shooter.stopIntake(), - shooter.stopSides()) + intake.stop(), + feeder.stop()) .withInterruptBehavior(InterruptionBehavior.kCancelSelf); } - public static Command prepareAmp(Shooter shooter, Arm arm) { + public static Command prepareAmp(Arm arm, Amper amper, Intake intake, Feeder feeder) { return Commands.sequence( arm.goToAngle(Rotation2d.fromDegrees(103.5), 1), - new SpinAmper(shooter, new Rotation2d(3.25)).withTimeout(5), - shooter.setIntakeSpeed(1), - shooter.setSideSpeeds(0.25), - shooter.stopAux(), + amper.spinToAngle(new Rotation2d(3.25)), + intake.setSpeed(1), + feeder.setSpeeds(0.25), Commands.waitSeconds(0.8), - shooter.stopSides(), - shooter.setIntakeSpeed(-0.9)) + feeder.stop(), + intake.setSpeed(-0.9)) .withInterruptBehavior(InterruptionBehavior.kCancelSelf); } - public static Command triggerAmp(Shooter shooter, Arm arm) { + public static Command triggerAmp(Arm arm, Amper amper, Intake intake, Feeder feeder) { return Commands.sequence( - shooter.setIntakeSpeed(-0.9), - shooter.setSideSpeeds(-1), - shooter.setAuxSpeed(-1), - shooter.waitForDetectionState(true, 1), - shooter.waitForDetectionState(false, 1), + intake.setSpeed(-0.9), + feeder.setSpeeds(-1), + feeder.waitForDetectionState(true, 1), + feeder.waitForDetectionState(false, 1), Commands.waitSeconds(0.22), - shooter.stopIntake(), - shooter.stopSides(), - shooter.stopAux(), + intake.stop(), + feeder.stop(), Commands.waitSeconds(0.5), - new SpinAmper(shooter, new Rotation2d(0.2)) - .alongWith(stowArm(shooter, arm)) - .withTimeout(5)) + amper.spinToAngle(new Rotation2d(0.2)).alongWith(arm.stow())) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); } - public static Command prepareShoot(Shooter shooter, Arm arm, double angle, boolean neverExit) { + public static Command prepareShoot( + Arm arm, Intake intake, Feeder feeder, Shooter shooter, double angle, double rpm) { return Commands.parallel( - arm.goToAngle(Rotation2d.fromDegrees(angle), 1), - shooter.stop(), // we use this one because it doesn't require shooter subsystem - new SpinUpShooter(shooter, 5000, neverExit)) + intake.stop(), + feeder.stop(), + shooter.spinToSpeed(rpm), + arm.goToAngle(Rotation2d.fromDegrees(angle), 1)) .withInterruptBehavior(InterruptionBehavior.kCancelSelf); } - public static Command triggerShoot(Shooter shooter) { + public static Command feed(Intake intake, Feeder feeder) { return Commands.sequence( - shooter.setSideSpeeds(+1), - Commands.waitSeconds(0.5), - shooter.stopIntake(), - shooter.stopSides(), - shooter.stopAux()) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + feeder.setSpeeds(+1), + intake.setSpeed(+1), + Commands.waitSeconds(1), + feeder.stop(), + intake.stop()); } - public static Command stowArm(Shooter shooter, Arm arm) { - return Commands.sequence( - shooter.stopIntake(), - shooter.stopSides(), - shooter.stopAux(), - arm.goToAngle(Rotation2d.fromDegrees(75.0), 0.7)); + public static Command vomit(Intake intake, Feeder feeder, Shooter shooter) { + return Commands.runOnce( + () -> { + intake.vomit(); + feeder.vomit(); + shooter.vomit(); + }); + } + + public static Command forceStop(Intake intake, Feeder feeder, Shooter shooter) { + return Commands.runOnce( + () -> { + intake.forceStop(); + feeder.forceStop(); + shooter.forceStop(); + }); } /** diff --git a/src/main/java/frc/team5115/commands/SpinAmper.java b/src/main/java/frc/team5115/commands/SpinAmper.java deleted file mode 100644 index f55d036..0000000 --- a/src/main/java/frc/team5115/commands/SpinAmper.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.team5115.commands; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.team5115.subsystems.shooter.Shooter; - -public class SpinAmper extends Command { - private final Shooter shooter; - private final Rotation2d setpoint; - private final double pid_tolerance; - private double pid; - - public SpinAmper(Shooter shooter, Rotation2d setpoint) { - // addRequirements(shooter); - this.shooter = shooter; - this.setpoint = setpoint; - pid_tolerance = 0.05; - } - - @Override - public void execute() { - pid = shooter.spinAmper(setpoint); - } - - @Override - public boolean isFinished() { - return Math.abs(pid) < pid_tolerance; - } - - @Override - public void end(boolean interrupted) { - shooter.setAmperSpeed(0); - } -} diff --git a/src/main/java/frc/team5115/commands/SpinUpShooter.java b/src/main/java/frc/team5115/commands/SpinUpShooter.java deleted file mode 100644 index c4155da..0000000 --- a/src/main/java/frc/team5115/commands/SpinUpShooter.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.team5115.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.team5115.subsystems.shooter.Shooter; - -public class SpinUpShooter extends Command { - private final Shooter shooter; - private final double rpm; - private final boolean neverExit; - private boolean atSpeed; - - public SpinUpShooter(Shooter shooter, double rpm, boolean neverExit) { - addRequirements(shooter); - this.shooter = shooter; - this.rpm = rpm; - this.neverExit = neverExit; - } - - @Override - public void execute() { - atSpeed = shooter.spinAuxByPid(rpm); - } - - @Override - public boolean isFinished() { - return atSpeed && !neverExit; - } -} diff --git a/src/main/java/frc/team5115/subsystems/amper/Amper.java b/src/main/java/frc/team5115/subsystems/amper/Amper.java new file mode 100644 index 0000000..33b192f --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/amper/Amper.java @@ -0,0 +1,57 @@ +package frc.team5115.subsystems.amper; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team5115.Constants; +import org.littletonrobotics.junction.Logger; + +public class Amper extends SubsystemBase { + private static final double PID_OFFSET = 90.0; // degrees + private final AmperIO io; + private final AmperIOInputsAutoLogged inputs = new AmperIOInputsAutoLogged(); + private final PIDController pid; + + public Amper(AmperIO io) { + this.io = io; + + switch (Constants.currentMode) { + case REAL: + case REPLAY: + pid = new PIDController(0.004, 0, 0); + break; + case SIM: + pid = new PIDController(0.005, 0, 0); + break; + default: + pid = new PIDController(0, 0, 0); + break; + } + + pid.setTolerance(2); + pid.setSetpoint(new Rotation2d(0.2).getDegrees() - PID_OFFSET); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Amper", inputs); + + final double pidOutput = pid.calculate(inputs.position.getDegrees() - PID_OFFSET); + io.setPercent(pidOutput); + Logger.recordOutput("Amper/Setpoint Degrees", pid.getSetpoint() + PID_OFFSET); + Logger.recordOutput("Amper/At Setpoint?", pid.atSetpoint()); + } + + public Command spinToAngle(Rotation2d setpoint) { + return Commands.runOnce(() -> pid.setSetpoint(setpoint.getDegrees() - PID_OFFSET), this) + .andThen(Commands.waitUntil(() -> pid.atSetpoint())) + .withTimeout(5); + } + + public void setSpeed(double percent) { + io.setPercent(percent); + } +} diff --git a/src/main/java/frc/team5115/subsystems/amper/AmperIO.java b/src/main/java/frc/team5115/subsystems/amper/AmperIO.java new file mode 100644 index 0000000..36f85d1 --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/amper/AmperIO.java @@ -0,0 +1,22 @@ +package frc.team5115.subsystems.amper; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface AmperIO { + @AutoLog + public static class AmperIOInputs { + public Rotation2d position = new Rotation2d(); + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(AmperIOInputs inputs) {} + + /** Run the intake motor at the specified voltage. */ + public default void setVoltage(double volts) {} + + /** Run the amper motor at the specified percentage. */ + public default void setPercent(double percent) {} +} diff --git a/src/main/java/frc/team5115/subsystems/amper/AmperIOSim.java b/src/main/java/frc/team5115/subsystems/amper/AmperIOSim.java new file mode 100644 index 0000000..b65151e --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/amper/AmperIOSim.java @@ -0,0 +1,36 @@ +package frc.team5115.subsystems.amper; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.team5115.Constants; + +public class AmperIOSim implements AmperIO { + private final DCMotorSim sim; + private double appliedVolts; + + public AmperIOSim() { + sim = new DCMotorSim(new DCMotor(+12.0, +7.909, +24.0, +5.0, +10.472, +1), +1.0, 0.0002); + } + + @Override + public void updateInputs(AmperIOInputs inputs) { + sim.update(Constants.LOOP_PERIOD_SECS); + inputs.position = Rotation2d.fromRadians(sim.getAngularPositionRad()); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = Math.abs(sim.getCurrentDrawAmps()); + } + + @Override + public void setVoltage(double volts) { + appliedVolts = MathUtil.clamp(volts, -12.0, +12.0); + sim.setInputVoltage(appliedVolts); + } + + @Override + public void setPercent(double percent) { + appliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); + sim.setInputVoltage(appliedVolts); + } +} diff --git a/src/main/java/frc/team5115/subsystems/amper/AmperIOSparkMax.java b/src/main/java/frc/team5115/subsystems/amper/AmperIOSparkMax.java new file mode 100644 index 0000000..8f00ecc --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/amper/AmperIOSparkMax.java @@ -0,0 +1,36 @@ +package frc.team5115.subsystems.amper; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.team5115.Constants; + +public class AmperIOSparkMax implements AmperIO { + private final CANSparkMax motor; + private final AbsoluteEncoder encoder; + + public AmperIOSparkMax() { + motor = new CANSparkMax(Constants.SNOWBLOWER_MOTOR_ID, MotorType.kBrushed); + encoder = motor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + encoder.setPositionConversionFactor(360); + } + + @Override + public void updateInputs(AmperIOInputs inputs) { + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); + inputs.position = Rotation2d.fromDegrees(encoder.getPosition()); + } + + @Override + public void setPercent(double percent) { + motor.set(percent); + } + + @Override + public void setVoltage(double volts) { + motor.setVoltage(volts); + } +} diff --git a/src/main/java/frc/team5115/subsystems/arm/Arm.java b/src/main/java/frc/team5115/subsystems/arm/Arm.java index 7501141..8d37ff1 100644 --- a/src/main/java/frc/team5115/subsystems/arm/Arm.java +++ b/src/main/java/frc/team5115/subsystems/arm/Arm.java @@ -13,8 +13,8 @@ public class Arm extends SubsystemBase { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private final ArmFeedforward armFF; - private final PIDController armPID; + private final ArmFeedforward feedforward; + private final PIDController pid; public Arm(ArmIO io) { this.io = io; @@ -22,34 +22,36 @@ public Arm(ArmIO io) { switch (Constants.currentMode) { case REAL: case REPLAY: - armFF = new ArmFeedforward(0.3, 0.35, 0.13509, 0.048686); - armPID = new PIDController(0.425, 0.0, 0.0); + feedforward = new ArmFeedforward(0.3, 0.35, 0.13509, 0.048686); + pid = new PIDController(0.425, 0.0, 0.0); break; case SIM: - armFF = new ArmFeedforward(0.0, 0.35, 0.1351, 0.0); - armPID = new PIDController(0.5, 0.0, 0.0); + feedforward = new ArmFeedforward(0.0, 0.35, 0.1351, 0.0); + pid = new PIDController(0.5, 0.0, 0.0); break; default: - armFF = new ArmFeedforward(0.0, 0.0, 0, 0.0); - armPID = new PIDController(0.0, 0.0, 0.0); + feedforward = new ArmFeedforward(0.0, 0.0, 0, 0.0); + pid = new PIDController(0.0, 0.0, 0.0); break; } - armPID.setTolerance(5); - armPID.setSetpoint(75.0); + pid.setTolerance(5); + pid.setSetpoint(75.0); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Arm", inputs); + Logger.recordOutput("Arm/Setpoint Degrees", pid.getSetpoint()); + Logger.recordOutput("Arm/At Setpoint?", pid.atSetpoint()); // Update the pids and feedforward - final double speed = armPID.calculate(inputs.armAngle.getDegrees()); - double voltage = armFF.calculate(inputs.armAngle.getRadians(), speed); + final double speed = pid.calculate(inputs.armAngle.getDegrees()); + double voltage = feedforward.calculate(inputs.armAngle.getRadians(), speed); voltage = MathUtil.clamp(voltage, -10, +10); - if (Math.abs(voltage) < 2 * armFF.ks) { + if (Math.abs(voltage) < 2 * feedforward.ks) { voltage = 0; } @@ -59,12 +61,16 @@ public void periodic() { public Command goToAngle(Rotation2d setpoint, double timeout) { return Commands.runOnce( () -> { - armPID.setSetpoint(setpoint.getDegrees()); + pid.setSetpoint(setpoint.getDegrees()); }) - .andThen(Commands.waitUntil(() -> armPID.atSetpoint())) + .andThen(Commands.waitUntil(() -> pid.atSetpoint())) .withTimeout(timeout); } + public Command stow() { + return goToAngle(Rotation2d.fromDegrees(75.0), 0.7); + } + public void stop() { io.setArmVoltage(0); } diff --git a/src/main/java/frc/team5115/subsystems/drive/Drivetrain.java b/src/main/java/frc/team5115/subsystems/drive/Drivetrain.java index 56d0781..dca3c57 100644 --- a/src/main/java/frc/team5115/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/team5115/subsystems/drive/Drivetrain.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.subsystems.drive; import static edu.wpi.first.units.Units.*; diff --git a/src/main/java/frc/team5115/subsystems/drive/GyroIO.java b/src/main/java/frc/team5115/subsystems/drive/GyroIO.java index 8abccb1..0c8ba77 100644 --- a/src/main/java/frc/team5115/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/team5115/subsystems/drive/GyroIO.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/team5115/subsystems/drive/Module.java b/src/main/java/frc/team5115/subsystems/drive/Module.java index c23953d..bb0ff1e 100644 --- a/src/main/java/frc/team5115/subsystems/drive/Module.java +++ b/src/main/java/frc/team5115/subsystems/drive/Module.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.subsystems.drive; import edu.wpi.first.math.controller.PIDController; @@ -138,7 +125,11 @@ public void setBrakeMode(boolean enabled) { /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { - return inputs.turnAbsolutePosition; + if (index==0) { + return inputs.turnAbsolutePosition.plus(Rotation2d.fromDegrees(180)); + } else { + return inputs.turnAbsolutePosition; + } } /** Returns the current drive position of the module in meters. */ diff --git a/src/main/java/frc/team5115/subsystems/drive/ModuleIO.java b/src/main/java/frc/team5115/subsystems/drive/ModuleIO.java index 3e5916e..96fe9a0 100644 --- a/src/main/java/frc/team5115/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/team5115/subsystems/drive/ModuleIO.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/team5115/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/team5115/subsystems/drive/ModuleIOSim.java index eb7b1f3..ddcc44c 100644 --- a/src/main/java/frc/team5115/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/team5115/subsystems/drive/ModuleIOSim.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.subsystems.drive; import edu.wpi.first.math.MathUtil; diff --git a/src/main/java/frc/team5115/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/team5115/subsystems/drive/ModuleIOSparkMax.java index f8f8694..2d31a85 100644 --- a/src/main/java/frc/team5115/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/team5115/subsystems/drive/ModuleIOSparkMax.java @@ -1,16 +1,3 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package frc.team5115.subsystems.drive; import com.revrobotics.AbsoluteEncoder; @@ -23,18 +10,6 @@ import edu.wpi.first.math.util.Units; import frc.team5115.Constants.SwerveConstants; -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ public class ModuleIOSparkMax implements ModuleIO { private final CANSparkMax driveSparkMax; private final CANSparkMax turnSparkMax; diff --git a/src/main/java/frc/team5115/subsystems/feeder/Feeder.java b/src/main/java/frc/team5115/subsystems/feeder/Feeder.java new file mode 100644 index 0000000..78e0244 --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/feeder/Feeder.java @@ -0,0 +1,60 @@ +package frc.team5115.subsystems.feeder; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Feeder extends SubsystemBase { + private final FeederIO io; + private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); + + public Feeder(FeederIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Feeder", inputs); + } + + public Command waitForDetectionState(boolean state, double timeout) { + return Commands.waitUntil(() -> inputs.noteDetected == state).withTimeout(timeout); + } + + public Command stop() { + return setSpeeds(+0); + } + + public Command centerNote() { + return setSpeeds(+0.08); + } + + public Command setSpeeds(double percent) { + return setSpeeds(percent, percent); + } + + public Command setSpeeds(double leftPercent, double rightPercent) { + return Commands.runOnce( + () -> { + io.setLeftPercent(leftPercent); + io.setRightPercent(rightPercent); + }, + this); + } + + public void vomit() { + io.setLeftPercent(-0.9); + io.setRightPercent(-0.9); + } + + public void forceStop() { + io.setLeftPercent(0); + io.setRightPercent(0); + } + + public boolean noteDetected() { + return inputs.noteDetected; + } +} diff --git a/src/main/java/frc/team5115/subsystems/feeder/FeederIO.java b/src/main/java/frc/team5115/subsystems/feeder/FeederIO.java new file mode 100644 index 0000000..5e1459d --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/feeder/FeederIO.java @@ -0,0 +1,33 @@ +package frc.team5115.subsystems.feeder; + +import org.littletonrobotics.junction.AutoLog; + +public interface FeederIO { + @AutoLog + public static class FeederIOInputs { + public double leftVelocityRPM = 0.0; + public double leftAppliedVolts = 0.0; + public double leftCurrentAmps = 0.0; + + public double rightVelocityRPM = 0.0; + public double rightAppliedVolts = 0.0; + public double rightCurrentAmps = 0.0; + + public boolean noteDetected = false; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FeederIOInputs inputs) {} + + /** Run the left motor at the specified voltage. */ + public default void setLeftVoltage(double volts) {} + + /** Run the left motor at the specified percentage. */ + public default void setLeftPercent(double percent) {} + + /** Run the right motor at the specified voltage. */ + public default void setRightVoltage(double volts) {} + + /** Run the right motor at the specified percentage. */ + public default void setRightPercent(double percent) {} +} diff --git a/src/main/java/frc/team5115/subsystems/feeder/FeederIOSim.java b/src/main/java/frc/team5115/subsystems/feeder/FeederIOSim.java new file mode 100644 index 0000000..ad375da --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/feeder/FeederIOSim.java @@ -0,0 +1,60 @@ +package frc.team5115.subsystems.feeder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.team5115.Constants; + +public class FeederIOSim implements FeederIO { + + private final FlywheelSim leftSim; + private final FlywheelSim rightSim; + + private double leftAppliedVolts; + private double rightAppliedVolts; + + public FeederIOSim() { + leftSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.0002); + rightSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.0002); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + leftSim.update(Constants.LOOP_PERIOD_SECS); + rightSim.update(Constants.LOOP_PERIOD_SECS); + + inputs.leftVelocityRPM = leftSim.getAngularVelocityRPM(); + inputs.leftAppliedVolts = leftAppliedVolts; + inputs.leftCurrentAmps = Math.abs(leftSim.getCurrentDrawAmps()); + + inputs.rightVelocityRPM = rightSim.getAngularVelocityRPM(); + inputs.rightAppliedVolts = rightAppliedVolts; + inputs.rightCurrentAmps = Math.abs(rightSim.getCurrentDrawAmps()); + + inputs.noteDetected = false; + } + + @Override + public void setRightVoltage(double volts) { + rightAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); + rightSim.setInputVoltage(rightAppliedVolts); + } + + @Override + public void setLeftVoltage(double volts) { + leftAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); + leftSim.setInputVoltage(leftAppliedVolts); + } + + @Override + public void setLeftPercent(double percent) { + leftAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); + leftSim.setInputVoltage(leftAppliedVolts); + } + + @Override + public void setRightPercent(double percent) { + rightAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); + rightSim.setInputVoltage(rightAppliedVolts); + } +} diff --git a/src/main/java/frc/team5115/subsystems/feeder/FeederIOSparkMax.java b/src/main/java/frc/team5115/subsystems/feeder/FeederIOSparkMax.java new file mode 100644 index 0000000..7e1918b --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/feeder/FeederIOSparkMax.java @@ -0,0 +1,77 @@ +package frc.team5115.subsystems.feeder; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.team5115.Constants; + +public class FeederIOSparkMax implements FeederIO { + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + + private final RelativeEncoder leftEncoder; + private final RelativeEncoder rightEncoder; + + private final DigitalInput sensor; + + public FeederIOSparkMax() { + sensor = new DigitalInput(Constants.SHOOTER_SENSOR_ID); + + leftMotor = new CANSparkMax(Constants.FEEDER_LEFT_MOTOR_ID, MotorType.kBrushless); + rightMotor = new CANSparkMax(Constants.FEEDER_RIGHT_MOTOR_ID, MotorType.kBrushless); + + leftEncoder = leftMotor.getEncoder(); + rightEncoder = rightMotor.getEncoder(); + + leftMotor.restoreFactoryDefaults(); + rightMotor.restoreFactoryDefaults(); + + leftMotor.setClosedLoopRampRate(0.1); + leftMotor.setInverted(false); + leftMotor.setIdleMode(IdleMode.kBrake); + leftMotor.setSmartCurrentLimit(40); + + rightMotor.setClosedLoopRampRate(0.1); + rightMotor.setInverted(true); + rightMotor.setIdleMode(IdleMode.kBrake); + rightMotor.setSmartCurrentLimit(40); + + leftMotor.burnFlash(); + rightMotor.burnFlash(); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + inputs.leftVelocityRPM = leftEncoder.getVelocity(); + inputs.leftAppliedVolts = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage(); + inputs.leftCurrentAmps = leftMotor.getOutputCurrent(); + + inputs.rightVelocityRPM = rightEncoder.getVelocity(); + inputs.rightAppliedVolts = rightMotor.getAppliedOutput() * rightMotor.getBusVoltage(); + inputs.rightCurrentAmps = rightMotor.getOutputCurrent(); + + inputs.noteDetected = !sensor.get(); + } + + @Override + public void setLeftPercent(double percent) { + leftMotor.set(percent); + } + + @Override + public void setLeftVoltage(double volts) { + leftMotor.setVoltage(volts); + } + + @Override + public void setRightPercent(double percent) { + rightMotor.set(percent); + } + + @Override + public void setRightVoltage(double volts) { + rightMotor.setVoltage(volts); + } +} diff --git a/src/main/java/frc/team5115/subsystems/intake/Intake.java b/src/main/java/frc/team5115/subsystems/intake/Intake.java new file mode 100644 index 0000000..47938be --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/intake/Intake.java @@ -0,0 +1,45 @@ +package frc.team5115.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Intake extends SubsystemBase { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + public Intake(IntakeIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + } + + public Command setSpeed(double percent) { + return Commands.runOnce(() -> io.setPercent(percent), this); + } + + public Command intake() { + return setSpeed(+1); + } + + public Command outtake() { + return setSpeed(-1); + } + + public Command stop() { + return setSpeed(+0); + } + + public void vomit() { + io.setPercent(-1); + } + + public void forceStop() { + io.setPercent(0); + } +} diff --git a/src/main/java/frc/team5115/subsystems/intake/IntakeIO.java b/src/main/java/frc/team5115/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..967c9e2 --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/intake/IntakeIO.java @@ -0,0 +1,21 @@ +package frc.team5115.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double velocityRPM = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(IntakeIOInputs inputs) {} + + /** Run the intake motor at the specified voltage. */ + public default void setVoltage(double volts) {} + + /** Run the intake motor at the specified percentage. */ + public default void setPercent(double percent) {} +} diff --git a/src/main/java/frc/team5115/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/team5115/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..bd6236c --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,35 @@ +package frc.team5115.subsystems.intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.team5115.Constants; + +public class IntakeIOSim implements IntakeIO { + private final DCMotorSim sim; + private double appliedVolts; + + public IntakeIOSim() { + sim = new DCMotorSim(DCMotor.getNEO(1), 1.0, 0.0002); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + sim.update(Constants.LOOP_PERIOD_SECS); + inputs.velocityRPM = sim.getAngularVelocityRPM(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = Math.abs(sim.getCurrentDrawAmps()); + } + + @Override + public void setVoltage(double volts) { + appliedVolts = MathUtil.clamp(volts, -12.0, +12.0); + sim.setInputVoltage(appliedVolts); + } + + @Override + public void setPercent(double percent) { + appliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); + sim.setInputVoltage(appliedVolts); + } +} diff --git a/src/main/java/frc/team5115/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/team5115/subsystems/intake/IntakeIOSparkMax.java new file mode 100644 index 0000000..22af29f --- /dev/null +++ b/src/main/java/frc/team5115/subsystems/intake/IntakeIOSparkMax.java @@ -0,0 +1,36 @@ +package frc.team5115.subsystems.intake; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import frc.team5115.Constants; + +public class IntakeIOSparkMax implements IntakeIO { + private final CANSparkMax motor; + private final RelativeEncoder encoder; + + public IntakeIOSparkMax() { + motor = new CANSparkMax(Constants.INTAKE_MOTOR_ID, MotorType.kBrushless); + encoder = motor.getEncoder(); + motor.setSmartCurrentLimit(60, 80); + motor.setIdleMode(IdleMode.kCoast); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.velocityRPM = encoder.getVelocity(); + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); + } + + @Override + public void setPercent(double percent) { + motor.set(percent); + } + + @Override + public void setVoltage(double volts) { + motor.setVoltage(volts); + } +} diff --git a/src/main/java/frc/team5115/subsystems/shooter/Shooter.java b/src/main/java/frc/team5115/subsystems/shooter/Shooter.java index 00952ec..9867c4c 100644 --- a/src/main/java/frc/team5115/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/team5115/subsystems/shooter/Shooter.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -13,34 +12,32 @@ public class Shooter extends SubsystemBase { private final ShooterIO io; private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - private final SimpleMotorFeedforward auxFF; - private final PIDController auxPID; - private final PIDController amperPID; + private final SimpleMotorFeedforward feedforward; + private final PIDController pid; private final SysIdRoutine sysID; + private double setpointRPM; + public Shooter(ShooterIO io) { this.io = io; switch (Constants.currentMode) { case REAL: case REPLAY: - auxFF = new SimpleMotorFeedforward(0.17484, 0.00223, 0.00030957); - auxPID = new PIDController(4.1686E-05, 0, 0); - amperPID = new PIDController(0.004, 0, 0); + feedforward = new SimpleMotorFeedforward(0.17484, 0.00223, 0.00030957); + pid = new PIDController(4.1686E-05, 0, 0); break; case SIM: - auxFF = new SimpleMotorFeedforward(0, 0.123, 0.03); - auxPID = new PIDController(1, 0, 0); - amperPID = new PIDController(0.005, 0, 0); + feedforward = new SimpleMotorFeedforward(0, 2.10E-3, 0.03); + pid = new PIDController(0.006, 0, 0); break; default: - auxFF = new SimpleMotorFeedforward(0, 0, 0); - auxPID = new PIDController(0, 0, 0); - amperPID = new PIDController(0, 0, 0); + feedforward = new SimpleMotorFeedforward(0, 0, 0); + pid = new PIDController(0, 0, 0); break; } - auxPID.setTolerance(20); + pid.setTolerance(20); sysID = new SysIdRoutine( @@ -50,110 +47,33 @@ public Shooter(ShooterIO io) { null, (state) -> Logger.recordOutput("Shooter/SysIdState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> io.setAuxVoltage(voltage.baseUnitMagnitude()), null, this)); + (voltage) -> io.setVoltage(voltage.baseUnitMagnitude()), null, this)); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); - } - - public void setAmperSpeed(double percent) { - io.setAmperPercent(percent); - } - - public Command waitForDetectionState(boolean state, double timeout) { - return Commands.waitUntil(() -> inputs.noteDetected == state).withTimeout(timeout); - } - - public Command intake() { - return setIntakeSpeed(+1); - } - - public Command outtake() { - return setIntakeSpeed(-1); - } - - public Command stopIntake() { - return setIntakeSpeed(+0); - } - - public Command stopSides() { - return setSideSpeeds(+0); - } - - public Command stopAux() { - return setAuxSpeed(+0); - } + Logger.recordOutput("Shooter/Setpoint RPM", setpointRPM); + Logger.recordOutput("Shooter/At Setpoint?", pid.atSetpoint()); - public Command centerNote() { - return setSideSpeeds(+0.08); + final double volts = + feedforward.calculate(setpointRPM) + pid.calculate(inputs.velocityRPM, setpointRPM); + io.setVoltage(volts); } - public Command setSideSpeeds(double percent) { - return setSideSpeeds(percent, percent); - } - - public Command setAuxSpeed(double percent) { - return Commands.runOnce(() -> io.setAuxPercent(percent), this); - } - - public Command setIntakeSpeed(double percent) { - return Commands.runOnce(() -> io.setIntakePercent(percent), this); - } - - public Command setSideSpeeds(double leftPercent, double rightPercent) { - return Commands.runOnce( - () -> { - io.setLeftPercent(leftPercent); - io.setRightPercent(rightPercent); - }, - this); - } - - public double spinAmper(Rotation2d setpoint) { - final double offset = 90.0; // degrees - final double pidOutput = - amperPID.calculate( - inputs.amperPosition.getDegrees() - offset, setpoint.getDegrees() - offset); - io.setAmperPercent(pidOutput); - Logger.recordOutput("Shooter/AmperPidPercent", pidOutput); - return pidOutput; - } - - public boolean spinAuxByPid(double rpm) { - // The feedforward and PID are in RPM - final double feedforward = auxFF.calculate(rpm); - final double feedback = auxPID.calculate(inputs.auxVelocityRPM, rpm); - final double volts = feedforward + feedback; - io.setAuxVoltage(volts); - return auxPID.atSetpoint(); + public Command stop() { + return Commands.runOnce(() -> setpointRPM = 0, this); } - /** This command does NOT require the shooter subsystem so that it can override all else */ - public Command vomit() { - return Commands.runOnce( - () -> { - io.setIntakePercent(-1); - io.setLeftPercent(-0.9); - io.setRightPercent(-0.9); - io.setAuxPercent(-0.9); - }); + public Command spinToSpeed(double rpm) { + return Commands.runOnce(() -> setSetpoint(rpm), this) + .andThen(Commands.waitUntil(() -> pid.atSetpoint())); } - /** - * This command does NOT require the shooter subsystem. This means it should not be in auto - * command groups. It also does NOT stop the amper - */ - public Command stop() { - return Commands.runOnce( - () -> { - io.setIntakePercent(0); - io.setLeftPercent(0); - io.setRightPercent(0); - io.setAuxPercent(0); - }); + private void setSetpoint(double rpm) { + setpointRPM = rpm; + pid.setSetpoint(rpm); } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -163,4 +83,12 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { public Command sysIdDynamic(SysIdRoutine.Direction direction) { return sysID.dynamic(direction); } + + public void vomit() { + setSetpoint(-3000); + } + + public void forceStop() { + setSetpoint(+0); + } } diff --git a/src/main/java/frc/team5115/subsystems/shooter/ShooterIO.java b/src/main/java/frc/team5115/subsystems/shooter/ShooterIO.java index 9f00881..068924a 100644 --- a/src/main/java/frc/team5115/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/team5115/subsystems/shooter/ShooterIO.java @@ -1,65 +1,18 @@ package frc.team5115.subsystems.shooter; -import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @AutoLog public static class ShooterIOInputs { - public double leftVelocityRPM = 0.0; - public double leftAppliedVolts = 0.0; - public double leftCurrentAmps = 0.0; - - public double rightVelocityRPM = 0.0; - public double rightAppliedVolts = 0.0; - public double rightCurrentAmps = 0.0; - - public double auxVelocityRPM = 0.0; - public double auxAppliedVolts = 0.0; - public double auxCurrentAmps = 0.0; - public double auxPositionRotations = 0.0; - - public double intakeVelocityRPM = 0.0; - public double intakeAppliedVolts = 0.0; - public double intakeCurrentAmps = 0.0; - - public Rotation2d amperPosition = new Rotation2d(); - public double amperAppliedVolts = 0.0; - public double amperCurrentAmps = 0.0; - - public boolean noteDetected = false; + public double velocityRPM = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; } /** Updates the set of loggable inputs. */ public default void updateInputs(ShooterIOInputs inputs) {} - /** Run the left motor at the specified voltage. */ - public default void setLeftVoltage(double volts) {} - - /** Run the left motor at the specified percentage. */ - public default void setLeftPercent(double percent) {} - - /** Run the right motor at the specified voltage. */ - public default void setRightVoltage(double volts) {} - - /** Run the right motor at the specified percentage. */ - public default void setRightPercent(double percent) {} - - /** Run the aux motor at the specified voltage. */ - public default void setAuxVoltage(double volts) {} - - /** Run the aux motor at the specified percentage. */ - public default void setAuxPercent(double percent) {} - - /** Run the intake motor at the specified percentage. */ - public default void setIntakePercent(double percent) {} - - /** Run the intake motor at the specified voltage. */ - public default void setAmperVoltage(double volts) {} - - /** Run the amper motor at the specified percentage. */ - public default void setAmperPercent(double percent) {} - - /** Run the intake motor at the specified voltage. */ - public default void setIntakeVoltage(double volts) {} + /** Run the shooter motor at the specified voltage. */ + public default void setVoltage(double volts) {} } diff --git a/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSim.java index d77a2f5..63c677e 100644 --- a/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSim.java @@ -1,121 +1,29 @@ package frc.team5115.subsystems.shooter; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import frc.team5115.Constants; public class ShooterIOSim implements ShooterIO { - private final FlywheelSim leftSim; - private final FlywheelSim rightSim; - private final FlywheelSim auxSim; - private final DCMotorSim intakeSim; - private final DCMotorSim amperSim; - - private double leftAppliedVolts; - private double rightAppliedVolts; - private double auxAppliedVolts; - private double intakeAppliedVolts; - private double amperAppliedVolts; + private final FlywheelSim sim; + private double appliedVolts; public ShooterIOSim() { - leftSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.0002); - rightSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.0002); - auxSim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.0002); - intakeSim = new DCMotorSim(DCMotor.getNEO(1), 1.0, 0.0002); - amperSim = new DCMotorSim(new DCMotor(+12.0, +7.909, +24.0, +5.0, +10.472, +1), +1.0, 0.0002); + sim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.0002); } @Override public void updateInputs(ShooterIOInputs inputs) { - leftSim.update(Constants.LOOP_PERIOD_SECS); - rightSim.update(Constants.LOOP_PERIOD_SECS); - auxSim.update(Constants.LOOP_PERIOD_SECS); - intakeSim.update(Constants.LOOP_PERIOD_SECS); - amperSim.update(Constants.LOOP_PERIOD_SECS); - - inputs.leftVelocityRPM = leftSim.getAngularVelocityRPM(); - inputs.leftAppliedVolts = leftAppliedVolts; - inputs.leftCurrentAmps = Math.abs(leftSim.getCurrentDrawAmps()); - - inputs.rightVelocityRPM = rightSim.getAngularVelocityRPM(); - inputs.rightAppliedVolts = rightAppliedVolts; - inputs.rightCurrentAmps = Math.abs(rightSim.getCurrentDrawAmps()); - - inputs.auxVelocityRPM = auxSim.getAngularVelocityRPM(); - inputs.auxAppliedVolts = auxAppliedVolts; - inputs.auxCurrentAmps = Math.abs(auxSim.getCurrentDrawAmps()); - - inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM(); - inputs.intakeAppliedVolts = intakeAppliedVolts; - inputs.intakeCurrentAmps = Math.abs(intakeSim.getCurrentDrawAmps()); - - inputs.amperPosition = Rotation2d.fromRadians(amperSim.getAngularPositionRad()); - inputs.amperAppliedVolts = amperAppliedVolts; - inputs.amperCurrentAmps = Math.abs(amperSim.getCurrentDrawAmps()); - - inputs.noteDetected = false; - } - - @Override - public void setAuxVoltage(double volts) { - auxAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); - auxSim.setInputVoltage(auxAppliedVolts); - } - - @Override - public void setRightVoltage(double volts) { - rightAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); - rightSim.setInputVoltage(rightAppliedVolts); - } - - @Override - public void setLeftVoltage(double volts) { - leftAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); - leftSim.setInputVoltage(leftAppliedVolts); - } - - @Override - public void setIntakeVoltage(double volts) { - intakeAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); - intakeSim.setInputVoltage(intakeAppliedVolts); - } - - @Override - public void setAmperVoltage(double volts) { - amperAppliedVolts = MathUtil.clamp(volts, -12.0, +12.0); - amperSim.setInputVoltage(amperAppliedVolts); - } - - @Override - public void setIntakePercent(double percent) { - intakeAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); - intakeSim.setInputVoltage(intakeAppliedVolts); - } - - @Override - public void setAmperPercent(double percent) { - amperAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); - amperSim.setInputVoltage(amperAppliedVolts); - } - - @Override - public void setAuxPercent(double percent) { - auxAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); - auxSim.setInputVoltage(auxAppliedVolts); - } - - @Override - public void setLeftPercent(double percent) { - leftAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); - leftSim.setInputVoltage(leftAppliedVolts); + sim.update(Constants.LOOP_PERIOD_SECS); + inputs.velocityRPM = sim.getAngularVelocityRPM(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = Math.abs(sim.getCurrentDrawAmps()); } @Override - public void setRightPercent(double percent) { - rightAppliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0); - rightSim.setInputVoltage(rightAppliedVolts); + public void setVoltage(double volts) { + appliedVolts = MathUtil.clamp(volts, -12.0, +12.0); + sim.setInputVoltage(appliedVolts); } } diff --git a/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSparkMax.java index 2c75660..7a129c1 100644 --- a/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/team5115/subsystems/shooter/ShooterIOSparkMax.java @@ -1,149 +1,38 @@ package frc.team5115.subsystems.shooter; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DigitalInput; import frc.team5115.Constants; public class ShooterIOSparkMax implements ShooterIO { - private final CANSparkMax leftMotor; - private final CANSparkMax rightMotor; - private final CANSparkMax intakeMotor; - private final CANSparkMax auxMotor; - private final CANSparkMax amperMotor; - private final RelativeEncoder leftEncoder; - private final RelativeEncoder rightEncoder; - private final RelativeEncoder intakeEncoder; - private final RelativeEncoder auxEncoder; - private final AbsoluteEncoder amperEncoder; - - private final DigitalInput sensor; + private final CANSparkMax motor; + private final RelativeEncoder encoder; public ShooterIOSparkMax() { - sensor = new DigitalInput(Constants.SHOOTER_SENSOR_ID); - - leftMotor = new CANSparkMax(Constants.SHOOTER_LEFT_MOTOR_ID, MotorType.kBrushless); - rightMotor = new CANSparkMax(Constants.SHOOTER_RIGHT_MOTOR_ID, MotorType.kBrushless); - auxMotor = new CANSparkMax(Constants.SHOTER_AUX_MOTOR_ID, MotorType.kBrushless); - intakeMotor = new CANSparkMax(Constants.INTAKE_MOTOR_ID, MotorType.kBrushless); - amperMotor = new CANSparkMax(Constants.SNOWBLOWER_MOTOR_ID, MotorType.kBrushed); - - leftEncoder = leftMotor.getEncoder(); - rightEncoder = rightMotor.getEncoder(); - auxEncoder = auxMotor.getEncoder(); - intakeEncoder = intakeMotor.getEncoder(); - amperEncoder = amperMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - amperEncoder.setPositionConversionFactor(360); - - // Intake motor configs - intakeMotor.setSmartCurrentLimit(60, 80); - intakeMotor.setIdleMode(IdleMode.kCoast); + motor = new CANSparkMax(Constants.SHOOTER_MOTOR_ID, MotorType.kBrushless); + encoder = motor.getEncoder(); // Shooter motor configs - leftMotor.restoreFactoryDefaults(); - rightMotor.restoreFactoryDefaults(); - auxMotor.restoreFactoryDefaults(); - - leftMotor.setClosedLoopRampRate(0.1); - rightMotor.setClosedLoopRampRate(0.1); - auxMotor.setClosedLoopRampRate(0.1); - - leftMotor.setInverted(false); - rightMotor.setInverted(true); - auxMotor.setInverted(false); - - leftMotor.setIdleMode(IdleMode.kBrake); - rightMotor.setIdleMode(IdleMode.kBrake); - auxMotor.setIdleMode(IdleMode.kBrake); - - leftMotor.setSmartCurrentLimit(40); - rightMotor.setSmartCurrentLimit(40); - auxMotor.setSmartCurrentLimit(40); - - leftMotor.burnFlash(); - rightMotor.burnFlash(); - auxMotor.burnFlash(); + motor.restoreFactoryDefaults(); + motor.setClosedLoopRampRate(0.1); + motor.setInverted(false); + motor.setIdleMode(IdleMode.kBrake); + motor.setSmartCurrentLimit(40); + motor.burnFlash(); } @Override public void updateInputs(ShooterIOInputs inputs) { - inputs.leftVelocityRPM = leftEncoder.getVelocity(); - inputs.leftAppliedVolts = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage(); - inputs.leftCurrentAmps = leftMotor.getOutputCurrent(); - - inputs.rightVelocityRPM = rightEncoder.getVelocity(); - inputs.rightAppliedVolts = rightMotor.getAppliedOutput() * rightMotor.getBusVoltage(); - inputs.rightCurrentAmps = rightMotor.getOutputCurrent(); - - inputs.auxVelocityRPM = auxEncoder.getVelocity(); - inputs.auxAppliedVolts = auxMotor.getAppliedOutput() * auxMotor.getBusVoltage(); - inputs.auxCurrentAmps = auxMotor.getOutputCurrent(); - inputs.auxPositionRotations = auxEncoder.getPosition(); - - inputs.intakeVelocityRPM = intakeEncoder.getVelocity(); - inputs.intakeAppliedVolts = intakeMotor.getAppliedOutput() * intakeMotor.getBusVoltage(); - inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); - - inputs.amperAppliedVolts = amperMotor.getAppliedOutput() * amperMotor.getBusVoltage(); - inputs.amperCurrentAmps = amperMotor.getOutputCurrent(); - inputs.amperPosition = Rotation2d.fromDegrees(amperEncoder.getPosition()); - - inputs.noteDetected = !sensor.get(); - } - - @Override - public void setAmperPercent(double percent) { - amperMotor.set(percent); - } - - @Override - public void setAuxPercent(double percent) { - auxMotor.set(percent); - } - - @Override - public void setAuxVoltage(double volts) { - auxMotor.setVoltage(volts); - } - - @Override - public void setIntakePercent(double percent) { - intakeMotor.set(percent); - } - - @Override - public void setLeftPercent(double percent) { - leftMotor.set(percent); - } - - @Override - public void setLeftVoltage(double volts) { - leftMotor.setVoltage(volts); - } - - @Override - public void setRightPercent(double percent) { - rightMotor.set(percent); - } - - @Override - public void setRightVoltage(double volts) { - rightMotor.setVoltage(volts); - } - - @Override - public void setAmperVoltage(double volts) { - amperMotor.setVoltage(volts); + inputs.velocityRPM = encoder.getVelocity(); + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); } @Override - public void setIntakeVoltage(double volts) { - intakeMotor.setVoltage(volts); + public void setVoltage(double volts) { + motor.setVoltage(volts); } }