diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "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..3c81045 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,6 +39,7 @@ public enum Mode { public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public static final int NEO_CURRENT_LIMIT = 20; + public static final int NEO_CURRENT_LIMIT_2 = 40; // Logging public static final long MAX_LOG_TIME_WAIT = 10; @@ -46,6 +47,7 @@ public enum Mode { public static final boolean DEBUG = true; public static final int ROLLER_MOTOR_ID = 1; + public static final int INTAKE_MOTOR_ID = 3; public static final int TILT_MOTOR_ID = 2; public static final double ROLLER_SPEED = 0.25; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e39d38a..7303e70 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.Flush; +import frc.robot.commands.Intake; 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,7 +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 +39,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 +99,14 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Tilt Down", new TiltDown(tiltSubsystem)); + + SmartDashboard.putData( + "Intake", + new Intake(intakeSubsystem)); + + SmartDashboard.putData( + "Flush", + new Flush(intakeSubsystem)); } } diff --git a/src/main/java/frc/robot/commands/Flush.java b/src/main/java/frc/robot/commands/Flush.java new file mode 100644 index 0000000..d8c62b2 --- /dev/null +++ b/src/main/java/frc/robot/commands/Flush.java @@ -0,0 +1,43 @@ +package frc.robot.commands; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class Flush extends LoggableCommand { + private final IntakeSubsystem subsystem; + private final Timer timer; + + /** + * @param subsystem + */ +public Flush(IntakeSubsystem subsystem) { + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); +} + +@Override +public void initialize() { + timer.restart(); +} + +@Override +public void execute() { + subsystem.setSpeed(Constants.ROLLER_SPEED); +} + +@Override +public void end(boolean interrupted) { + subsystem.stopMotors(); +} + +@Override +public boolean isFinished() { + if (timer.hasElapsed(Constants.SPIN_TIMEOUT)) { + return true; + } else{ + return false; + } +} +} diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java new file mode 100644 index 0000000..690e08b --- /dev/null +++ b/src/main/java/frc/robot/commands/Intake.java @@ -0,0 +1,43 @@ +package frc.robot.commands; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class Intake extends LoggableCommand { + private final IntakeSubsystem subsystem; + private final Timer timer; + + /** + * @param subsystem + */ +public Intake(IntakeSubsystem subsystem) { + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); +} + +@Override +public void initialize() { + timer.restart(); +} + +@Override +public void execute() { + subsystem.setSpeed(Constants.ROLLER_SPEED); +} + +@Override +public void end(boolean interrupted) { + subsystem.stopMotors(); +} + +@Override +public boolean isFinished() { + if (timer.hasElapsed(Constants.SPIN_TIMEOUT)) { + return true; + } else{ + return false; + } +} +} 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..d3c23a0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,69 @@ +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_2); + motor.configure( + motorConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); +//changed constructor name + + return motor; + } +} + + diff --git a/src/main/java/frc/robot/subsystems/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/RollerSubsystem.java index b18839f..d6aed39 100644 --- a/src/main/java/frc/robot/subsystems/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RollerSubsystem.java @@ -21,23 +21,23 @@ public class RollerSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "RollerSubsystem"; - private final SparkMaxIo io; + private final SparkMaxIo motor; - public RollerSubsystem(SparkMaxIo io) { - this.io = io; + public RollerSubsystem(SparkMaxIo motor) { + this.motor = motor; } public void setSpeed(double speed) { - io.set(speed); + motor.set(speed); } public void stopMotors() { - io.stopMotor(); + motor.stopMotor(); } @Override public void periodic() { - io.periodic(); + motor.periodic(); } public static SparkMaxIo createMockIo() { @@ -63,7 +63,8 @@ private static SparkMax createMotor() { motorConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); +//changed constructor name - return motor; + return motor; } } diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java b/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java index 9236397..d445743 100644 --- a/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java +++ b/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java @@ -8,7 +8,7 @@ public class LoggableSequentialCommandGroup extends SequentialCommandGroup imple private String basicName = getClass().getSimpleName(); private Command parent = new BlankCommand(); - public LoggableSequentialCommandGroup(T... commands) { + public LoggableSequentialCommandGroup(T...commands) { ProxyCommand[] proxyCommands = new ProxyCommand[commands.length]; for (int i = 0; i < commands.length; i++) { commands[i].setParent(this); diff --git a/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java b/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java index bc39c5a..14bb115 100644 --- a/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java +++ b/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java @@ -1,6 +1,5 @@ package frc.robot.utils.logging.io; -import frc.robot.utils.logging.io.BaseIo; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.inputs.LoggableInputs; diff --git a/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java index 8402c27..aa68678 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/SimSparkMaxIo.java @@ -4,7 +4,6 @@ import frc.robot.Constants; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.simulation.Simulator; -import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; /** * IO Implementation for a simulated SparkMax IO controller. diff --git a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java index dab54c8..27cf5d4 100644 --- a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java +++ b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java @@ -1,18 +1,20 @@ package frc.robot.utils.simulation; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.Constants; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import frc.robot.Constants; + 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; public RobotVisualizer() { LoggedMechanismRoot2d root = @@ -25,15 +27,24 @@ public RobotVisualizer() { this.tiltLigament = riserLigament.append( new LoggedMechanismLigament2d( - "Tilt", - 0.5, - 90.0, - 4, - new Color8Bit(Color.kCornflowerBlue))); + "Tilt", 0.5, 90.0, 4, new Color8Bit(Color.kCornflowerBlue))); this.rollerLigament = this.tiltLigament.append( new LoggedMechanismLigament2d( - "Roller", 0.05, 180, 5, new Color8Bit(Color.kGreen))); + "Roller", 67, 2670, 67, new Color8Bit(Color.kGreen))); + + LoggedMechanismRoot2d root2 = + mech2d.getRoot("Robot Root 2", Constants.DRIVE_BASE_WIDTH / 2, Constants.INITIAL_ROBOT_HEIGHT); + + LoggedMechanismLigament2d riserLigament2 = + root2.append( + new LoggedMechanismLigament2d( + "Riser2", 0.45, 100, 6, new Color8Bit(Color.kLightGray))); + + this.intakeLigament = + riserLigament2.append( + new LoggedMechanismLigament2d( + "Intake", 25, 2550, 25, new Color8Bit(Color.kBlack))); } public LoggedMechanismLigament2d getRollerLigament() { @@ -44,6 +55,10 @@ public LoggedMechanismLigament2d getTiltLigament() { return tiltLigament; } + public LoggedMechanismLigament2d getIntakeLigament() { + return intakeLigament; + } + public void logMechanism() { Logger.recordOutput("Mechanism2d/", mech2d); }