diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a390c3f..a82210e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,7 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.REAL; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0d2574c..539bed7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,7 +18,6 @@ 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.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; @@ -29,12 +28,14 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOReal; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.pivot.Pivot; +import frc.robot.subsystems.pivot.PivotIO; +import frc.robot.subsystems.pivot.PivotIOReal; import frc.robot.subsystems.pivot.PivotIOSim; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -45,6 +46,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final Intake intake; private final Pivot pivot; // Controller @@ -52,8 +54,6 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -67,13 +67,8 @@ public RobotContainer() { new ModuleIOTalonFX(1), new ModuleIOTalonFX(2), new ModuleIOTalonFX(3)); - // drive = new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); - // flywheel = new Flywheel(new FlywheelIOTalonFX()); + intake = Intake.initialize(new IntakeIOReal()); + pivot = Pivot.initialize(new PivotIOReal()); break; case SIM: @@ -85,8 +80,8 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + intake = Intake.initialize(new IntakeIOSim()); pivot = Pivot.initialize(new PivotIOSim()); - Intake.initialize(new IntakeIOSim()); break; @@ -99,19 +94,12 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + intake = Intake.initialize(new IntakeIO() {}); + pivot = Pivot.initialize(new PivotIO() {}); // change? - pivot = Pivot.initialize(new PivotIOSim()); break; } - // This can go away - - // Set up auto routines - // NamedCommands.registerCommand( - // "Run Flywheel", - // Commands.startEnd( - // () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel) - // .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set up SysId routines @@ -126,19 +114,6 @@ public RobotContainer() { autoChooser.addOption( "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - // autoChooser.addOption( - // "Flywheel SysId (Quasistatic Forward)", - // flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - // autoChooser.addOption( - // "Flywheel SysId (Quasistatic Reverse)", - // flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - // autoChooser.addOption( - // "Flywheel SysId (Dynamic Forward)", - // flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - // autoChooser.addOption( - // "Flywheel SysId (Dynamic Reverse)", - // flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - // Configure the button bindings configureButtonBindings(); } @@ -157,35 +132,27 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> -controller.getRightX())); - // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - - controller.leftBumper().whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + controller.rightBumper() + .onTrue(Commands.run(() -> intake.setIntakeSpeed(0.75))) + .onFalse(Commands.run(() -> intake.setIntakeSpeed(0))); - controller.leftTrigger().whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - - controller.rightBumper().whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); - - controller.rightTrigger().whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + controller.leftBumper() + .onTrue(Commands.run(() -> intake.setIntakeSpeed(-0.75))) + .onFalse(Commands.run(() -> intake.setIntakeSpeed(0))); // controller - // .b() - // .onTrue( - // Commands.runOnce( - // () -> - // drive.setPose( - // new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - // drive) - // .ignoringDisable(true)); + // .leftBumper() + // .whileTrue(Commands.startEnd(() -> pivot.runVoltage(3), () -> pivot.runVoltage(0), + // pivot)); + // controller - // .a() - // .whileTrue( - // Commands.startEnd( - // () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); + // .rightBumper() + // .whileTrue(Commands.startEnd(() -> pivot.runVoltage(-3), () -> pivot.runVoltage(0), + // pivot)); controller.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 35499d2..99f37c9 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.Timer; 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; @@ -50,8 +51,8 @@ public boolean noteNotInIntake() { // .. } public Command intake() { - return startEnd(() -> setIntakeSpeed(1), () -> setIntakeSpeed(0)) - .onlyWhile(() -> noteNotInIntake()); + return Commands.startEnd(() -> io.setMotorSpeed(1), () -> io.setMotorSpeed(0)); + // .onlyWhile(() -> noteNotInIntake()); // return (startEnd(() -> setIntakeSpeed(1), () -> setIntakeSpeed(0)).onlyWhile(() -> // noteNotInIntake())).alongWith(startEnd(() -> setIntakeSpeed(0.1), () -> // setIntakeSpeed(0)).onlyIf(() -> noteNotInIntake() == true).onlyWhile(() -> noteNotInIntake() @@ -63,7 +64,7 @@ public Command spike() { } */ public Command reverse() { - return startEnd(() -> setIntakeSpeed(-1), () -> setIntakeSpeed(0)); + return startEnd(() -> io.setMotorSpeed(-1), () -> io.setMotorSpeed(0)); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index deb74f1..1e3efba 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -2,6 +2,6 @@ public class IntakeConstants { public static final int CanID = 5; // .. - public static final int currentLimit = 40; // vortex + public static final int currentLimit = 60; // vortex public static final int current = 10; // .. } diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java index b8a18f1..538e186 100644 --- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivot/Pivot.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.subsystems.intake.Intake; - import org.littletonrobotics.junction.Logger; public class Pivot extends SubsystemBase { @@ -34,9 +33,6 @@ private Pivot(PivotIO io) { ffModel = new SimpleMotorFeedforward(0.0, 0.0); break; } - - setDefaultCommand(Pivot.getInstance().currDetPivot()); - } public static Pivot getInstance() { @@ -52,11 +48,10 @@ public static Pivot initialize(PivotIO io) { public Command currDetPivot() { return Commands.sequence( - Commands.waitUntil(() -> Intake.getInstance().noteNotInIntake()), - Commands.runOnce(() -> Pivot.getInstance().setGoal(Math.PI / 6)), - Commands.waitUntil(() -> !Intake.getInstance().noteNotInIntake()), - Commands.runOnce(() -> Pivot.getInstance().setGoal(0)) - ); + Commands.waitUntil(() -> Intake.getInstance().noteNotInIntake()), + Commands.runOnce(() -> Pivot.getInstance().setGoal(Math.PI / 6)), + Commands.waitUntil(() -> !Intake.getInstance().noteNotInIntake()), + Commands.runOnce(() -> Pivot.getInstance().setGoal(0))); } @Override diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index a8436cc..7f35187 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -11,6 +11,7 @@ public class PivotIOReal implements PivotIO { private CANSparkFlex pivotMotor = new CANSparkFlex(11, MotorType.kBrushless); private RelativeEncoder pivotEncoderRight = pivotMotor.getEncoder(); private PIDController pid = new PIDController(1.0, 0.0, 0.0); + private double gearing = 36; private boolean closedLoop = false; private double ffVolts = 0.0; @@ -29,8 +30,8 @@ public PivotIOReal() { @Override public void updateInputs(PivotIOInputs inputs) { - inputs.positionRad = pivotEncoderRight.getPosition() * 2 * Math.PI; - inputs.velocityRadPerSec = pivotEncoderRight.getVelocity() * 2 * Math.PI / 60; + inputs.positionRad = (pivotEncoderRight.getPosition() * 2 * Math.PI) / gearing; + inputs.velocityRadPerSec = (pivotEncoderRight.getVelocity() * 2 * Math.PI / 60) / gearing; inputs.appliedVolts = appliedVolts; inputs.goalAngle = pid.getSetpoint();