diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..4919a24 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "Joysticks": { + "window": { + "visible": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a295450..ddf82e5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,11 +47,14 @@ public enum Mode { public static final boolean DEBUG = true; public static final int ROLLER_MOTOR_ID = 1; public static final int TILT_MOTOR_ID = 2; + public static final int INTAKE_MOTOR_ID = 3; public static final double ROLLER_SPEED = 0.25; public static final double TILT_SPEED = -0.5; // Arm motor is inverted - use negative speed + public static final double INTAKE_SPEED = 0.5; public static final double SPIN_TIMEOUT = 5; public static final double TILT_TIMEOUT = 5; + public static final double INTAKE_TIMEOUT = 4; public static final double TILT_LENGTH = 0.2; public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45); public static final Rotation2d TILT_MAX_ANGLE = Rotation2d.fromDegrees(90); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e39d38a..e1c0d61 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,9 +8,12 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.intake.FlushIntake; +import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.roller.SpinRoller; import frc.robot.commands.tilt.TiltDown; import frc.robot.commands.tilt.TiltUp; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.RollerSubsystem; import frc.robot.subsystems.TiltSubsystem; import frc.robot.utils.simulation.RobotVisualizer; @@ -25,6 +28,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final RollerSubsystem rollerSubsystem; private final TiltSubsystem tiltSubsystem; + private final IntakeSubsystem intakeSubsystem; private RobotVisualizer robotVisualizer; @@ -36,15 +40,18 @@ public RobotContainer() { case REAL -> { rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo()); tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo()); + intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo()); } case REPLAY -> { rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo()); + intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo()); } case SIM -> { robotVisualizer = new RobotVisualizer(); rollerSubsystem = new RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer)); tiltSubsystem = new TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer)); + intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer)); } default -> { throw new RuntimeException("Did not specify Robot Mode"); @@ -93,6 +100,14 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Tilt Down", new TiltDown(tiltSubsystem)); + + SmartDashboard.putData( + "Spin Intake", + new SpinIntake(intakeSubsystem)); + + SmartDashboard.putData( + "Flush Intake", + new FlushIntake(intakeSubsystem)); } } diff --git a/src/main/java/frc/robot/commands/intake/FlushIntake.java b/src/main/java/frc/robot/commands/intake/FlushIntake.java new file mode 100644 index 0000000..adfdef8 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/FlushIntake.java @@ -0,0 +1,43 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +// Spins Intake +public class FlushIntake extends LoggableCommand { + private final IntakeSubsystem subsystem; + private final Timer timer; + + public FlushIntake(IntakeSubsystem subsystem) { + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + subsystem.setSpeed(-(Constants.INTAKE_SPEED)); + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + if (timer.hasElapsed(Constants.INTAKE_TIMEOUT - 3)) { + return true; + } + else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/intake/SpinIntake.java b/src/main/java/frc/robot/commands/intake/SpinIntake.java new file mode 100644 index 0000000..3d30c19 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/SpinIntake.java @@ -0,0 +1,43 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +// Spins Intake +public class SpinIntake extends LoggableCommand { + private final IntakeSubsystem subsystem; + private final Timer timer; + + public SpinIntake(IntakeSubsystem subsystem) { + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + subsystem.setSpeed(Constants.INTAKE_SPEED); + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + if (timer.hasElapsed(Constants.INTAKE_TIMEOUT)) { + return true; + } + else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/roller/SpinRoller.java b/src/main/java/frc/robot/commands/roller/SpinRoller.java index 539b234..4dc17ed 100644 --- a/src/main/java/frc/robot/commands/roller/SpinRoller.java +++ b/src/main/java/frc/robot/commands/roller/SpinRoller.java @@ -5,6 +5,7 @@ import frc.robot.subsystems.RollerSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; +// Spins roller on Algae Bye Bye public class SpinRoller extends LoggableCommand { private final RollerSubsystem subsystem; private final Timer timer; @@ -34,7 +35,8 @@ public void end(boolean interrupted) { public boolean isFinished() { if (timer.hasElapsed(Constants.SPIN_TIMEOUT)) { return true; - } else{ + } + else { return false; } } diff --git a/src/main/java/frc/robot/commands/tilt/TiltDown.java b/src/main/java/frc/robot/commands/tilt/TiltDown.java index b4653da..355359f 100644 --- a/src/main/java/frc/robot/commands/tilt/TiltDown.java +++ b/src/main/java/frc/robot/commands/tilt/TiltDown.java @@ -5,6 +5,7 @@ import frc.robot.subsystems.TiltSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; +// Tilts Algae Bye Bye arm down public class TiltDown extends LoggableCommand { private final TiltSubsystem subsystem; private final Timer timer; diff --git a/src/main/java/frc/robot/commands/tilt/TiltUp.java b/src/main/java/frc/robot/commands/tilt/TiltUp.java index ef19a1c..cb428a9 100644 --- a/src/main/java/frc/robot/commands/tilt/TiltUp.java +++ b/src/main/java/frc/robot/commands/tilt/TiltUp.java @@ -5,6 +5,7 @@ import frc.robot.subsystems.TiltSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; +// Tilts Algae Bye Bye arm up public class TiltUp extends LoggableCommand { private final TiltSubsystem subsystem; private final Timer timer; diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..cefc308 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.utils.logging.input.MotorLoggableInputs; +import frc.robot.utils.logging.io.motor.MockSparkMaxIo; +import frc.robot.utils.logging.io.motor.RealSparkMaxIo; +import frc.robot.utils.logging.io.motor.SimSparkMaxIo; +import frc.robot.utils.logging.io.motor.SparkMaxIo; +import frc.robot.utils.simulation.MotorSimulator; +import frc.robot.utils.simulation.RobotVisualizer; + +public class IntakeSubsystem extends SubsystemBase { +public static final String LOGGING_NAME = "IntakeSubsystem"; + private final SparkMaxIo io; + + public IntakeSubsystem(SparkMaxIo io) { + this.io = io; + } + + public void setSpeed(double speed) { + io.set(speed); + } + + public void stopMotors() { + io.stopMotor(); + } + + @Override + public void periodic() { + io.periodic(); + } + + public static SparkMaxIo createMockIo() { + return new MockSparkMaxIo(); + } + + public static SparkMaxIo createRealIo() { + return new RealSparkMaxIo(LOGGING_NAME, createMotor(), MotorLoggableInputs.allMetrics()); + } + + public static SparkMaxIo createSimIo(RobotVisualizer visualizer) { + SparkMax motor = createMotor(); + return new SimSparkMaxIo(LOGGING_NAME, motor, MotorLoggableInputs.allMetrics(), + new MotorSimulator(motor, visualizer.getIntakeLigament())); + } + + private static SparkMax createMotor() { + SparkMax motor = new SparkMax(Constants.INTAKE_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); + SparkMaxConfig motorConfig = new SparkMaxConfig(); + motorConfig.idleMode(SparkBaseConfig.IdleMode.kBrake); + motorConfig.smartCurrentLimit(Constants.NEO_CURRENT_LIMIT); + motor.configure( + motorConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + + return motor; + } +} diff --git a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java index dab54c8..f5d036b 100644 --- a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java +++ b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java @@ -13,8 +13,28 @@ public class RobotVisualizer { private final LoggedMechanism2d mech2d = new LoggedMechanism2d(2, Units.feetToMeters(7)); private final LoggedMechanismLigament2d tiltLigament; private final LoggedMechanismLigament2d rollerLigament; + private final LoggedMechanismLigament2d intakeLigament; + private final LoggedMechanismLigament2d intakeLigament2; public RobotVisualizer() { + LoggedMechanismRoot2d root2 = + mech2d.getRoot("Robot Root 2", Constants.DRIVE_BASE_WIDTH, Constants.INITIAL_ROBOT_HEIGHT); + + LoggedMechanismLigament2d riserLigament2 = + root2.append( + new LoggedMechanismLigament2d( + "Riser2", 0.2, 90, 5, new Color8Bit(Color.kDarkGray))); + + this.intakeLigament = + riserLigament2.append( + new LoggedMechanismLigament2d( + "Intake", 0.05, 270, 50, new Color8Bit(Color.kPurple))); + + this.intakeLigament2 = + riserLigament2.append( + new LoggedMechanismLigament2d( + "Intake 2", 0.05, 90, 50, new Color8Bit(Color.kPurple))); + LoggedMechanismRoot2d root = mech2d.getRoot("Robot Root", Constants.DRIVE_BASE_WIDTH / 2, Constants.INITIAL_ROBOT_HEIGHT); @@ -33,7 +53,7 @@ public RobotVisualizer() { this.rollerLigament = this.tiltLigament.append( new LoggedMechanismLigament2d( - "Roller", 0.05, 180, 5, new Color8Bit(Color.kGreen))); + "Roller", 0.05, 180, 50, new Color8Bit(Color.kGreen))); // changed roller size } public LoggedMechanismLigament2d getRollerLigament() { @@ -44,6 +64,14 @@ public LoggedMechanismLigament2d getTiltLigament() { return tiltLigament; } + public LoggedMechanismLigament2d getIntakeLigament() { + return intakeLigament; + } + + public LoggedMechanismLigament2d getIntakeLigament2() { + return intakeLigament2; + } + public void logMechanism() { Logger.recordOutput("Mechanism2d/", mech2d); }