From 5e876a984c67e5e3977d0fd39c21c162a1e17f81 Mon Sep 17 00:00:00 2001 From: Ping Date: Wed, 17 Jan 2024 17:45:34 -0500 Subject: [PATCH 1/4] test Adding comment to ensure I can make changes --- .../frc2024/subsystems/drive/commands/IntakeCommand.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java index c0e28096..0ab647f0 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java @@ -42,5 +42,6 @@ public boolean isFinished() { public void end(boolean interrupted) { // TODO Auto-generated method stub SmartDashboard.putNumber("Intakecommand/intakespeed", Double.NaN); + // TODO Testing to ensure I can make changes } } \ No newline at end of file From 27c7d5a6445291bea568f3fb5d8569b03f3a2368 Mon Sep 17 00:00:00 2001 From: Ping Date: Mon, 22 Jan 2024 20:09:40 -0500 Subject: [PATCH 2/4] Created Basic Intake Subsystem Changed the IntakeCommand file with the information in last year's version. Added IntakeConstants file just to place contastant although can delete later. --- .../drive/commands/IntakeCommand.java | 124 ++++++++++++------ .../drive/commands/IntakeConstants.java | 7 + 2 files changed, 91 insertions(+), 40 deletions(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java index 0ab647f0..2222d969 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java @@ -1,47 +1,91 @@ package org.jmhsrobotics.frc2024.subsystems.drive.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; +import com.revrobotics.CANSparkMax; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; -import edu.wpi.first.wpilibj.Timer; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj2.command.Command; public class IntakeCommand extends Command { - private DriveSubsystem drive; - private double speed; - private Timer timer; - - public IntakeCommand(DriveSubsystem drive, double speed) { - this.drive = drive; - this.speed = speed; - this.timer = new Timer(); - addRequirements(this.drive); - } - - @Override - public void initialize() { - // TODO Auto-generated method stub - this.timer.reset(); - this.timer.start(); - } - - @Override - public void execute() { - // TODO Auto-generated method stub - SmartDashboard.putNumber("Intakecommand/intakespeed", this.speed); - } - - @Override - public boolean isFinished() { - // TODO Auto-generated method stub - return timer.hasElapsed(5); - } - - @Override - public void end(boolean interrupted) { - // TODO Auto-generated method stub - SmartDashboard.putNumber("Intakecommand/intakespeed", Double.NaN); - // TODO Testing to ensure I can make changes - } -} \ No newline at end of file + protected CANSparkMax innerIntakeMotor; + protected Solenoid intakeArmsVirtualSolenoidA; + protected Solenoid intakeArmsVirtualSolenoidB; + protected CANSparkMax intakeArmsMotor; + protected DigitalInput intakeSwitch; + + // protected boolean isClimbing; + public IntakeCommand(DriveSubsystem driveSubsystem, int i) { + innerIntakeMotor = new CANSparkMax(IntakeConstants.innerIntakeMotor, MotorType.kBrushless); + innerIntakeMotor.restoreFactoryDefaults(); + innerIntakeMotor.setInverted(true); + innerIntakeMotor.setIdleMode(IdleMode.kBrake); + + innerIntakeMotor.setSmartCurrentLimit(35); + + intakeArmsVirtualSolenoidA = new Solenoid(PneumaticsModuleType.CTREPCM, 3); + intakeArmsVirtualSolenoidB = new Solenoid(PneumaticsModuleType.CTREPCM, 4); + intakeArmsVirtualSolenoidA.set(false); + intakeArmsVirtualSolenoidB.set(false); + + intakeArmsMotor = new CANSparkMax(IntakeConstants.intakeArmsMotor, MotorType.kBrushless); + intakeArmsMotor.restoreFactoryDefaults(); + intakeArmsMotor.setInverted(true); + + intakeArmsMotor.setSmartCurrentLimit(35); + + intakeSwitch = new DigitalInput(IntakeConstants.intakeSwitch); + } + public void enableInnerIntakeMotor() { + innerIntakeMotor.set(1); + } + public void reverseInnerIntakeMotor() { + innerIntakeMotor.set(-1); + } + public void disableInnerIntakeMotor() { + innerIntakeMotor.set(0); + } + public void setInnerIntakeMotor(double speed) { + this.innerIntakeMotor.set(speed); + } + + // implementing 836's idea + public void extendIntakeArms() { + intakeArmsVirtualSolenoidA.set(true); + intakeArmsVirtualSolenoidB.set(true); + } + + // implementing 836's idea + public void retractIntakeArms() { + intakeArmsVirtualSolenoidA.set(false); + intakeArmsVirtualSolenoidB.set(false); + } + + // implementing 836's idea + public void floatIntakeArms() { + intakeArmsVirtualSolenoidA.set(false); + intakeArmsVirtualSolenoidB.set(true); + } + + public void enableIntakeArmsMotor() { + intakeArmsMotor.set(-0.6); + } + public void disableIntakeArmsMotor() { + intakeArmsMotor.set(0); + } + public void reverseIntakeArmsMotor() { + intakeArmsMotor.set(0.6); + } + public double extendedIntakeArmsSpeed() { + return intakeArmsMotor.get(); + } + + public boolean getIntakeSwitch() { + return intakeSwitch.get(); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java new file mode 100644 index 00000000..060c334d --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java @@ -0,0 +1,7 @@ +package org.jmhsrobotics.frc2024.subsystems.drive.commands; + +public final class IntakeConstants { + static int innerIntakeMotor = 1; + static int intakeArmsMotor = 1; + static int intakeSwitch = 1; +} From bd8294944f77b9bf67eabb7d701a3b19ed353964 Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 27 Jan 2024 01:36:38 +0000 Subject: [PATCH 3/4] Basic sim support code cleanup. --- .../java/org/jmhsrobotics/frc2024/Robot.java | 3 - .../jmhsrobotics/frc2024/RobotContainer.java | 2 - .../drive/commands/IntakeCommand.java | 91 ------------------- .../drive/commands/IntakeConstants.java | 7 -- .../subsystems/intake/IntakeSubsystem.java | 31 +++++++ 5 files changed, 31 insertions(+), 103 deletions(-) delete mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java delete mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java diff --git a/src/main/java/org/jmhsrobotics/frc2024/Robot.java b/src/main/java/org/jmhsrobotics/frc2024/Robot.java index 7faab217..ac9d523b 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/Robot.java +++ b/src/main/java/org/jmhsrobotics/frc2024/Robot.java @@ -6,7 +6,6 @@ import java.util.List; -import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand; import org.jmhsrobotics.warcore.util.BuildDataLogger; import com.pathplanner.lib.path.GoalEndState; @@ -21,7 +20,6 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import monologue.Logged; @@ -35,7 +33,6 @@ public class Robot extends TimedRobot implements Logged { @Override public void robotInit() { m_robotContainer = new RobotContainer(); - SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5)); setupLogs(); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 8df589b6..e40f0f8a 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -10,7 +10,6 @@ import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; -import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; import com.pathplanner.lib.auto.AutoBuilder; @@ -62,7 +61,6 @@ private void configurePathPlanner() { new HolonomicPathFollowerConfig(new PIDConstants(.5, 0, 0), new PIDConstants(1.5, 0, 0), Constants.SwerveConstants.kMaxSpeedMetersPerSecond, .5, new ReplanningConfig()), this::getAllianceFlipState, driveSubsystem); - NamedCommands.registerCommand("Intake", new IntakeCommand(driveSubsystem, 5)); NamedCommands.registerCommand("Wait", new WaitCommand(30)); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java deleted file mode 100644 index 2222d969..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeCommand.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.drive.commands; - -import com.revrobotics.CANSparkMax; - -import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeCommand extends Command { - protected CANSparkMax innerIntakeMotor; - protected Solenoid intakeArmsVirtualSolenoidA; - protected Solenoid intakeArmsVirtualSolenoidB; - protected CANSparkMax intakeArmsMotor; - protected DigitalInput intakeSwitch; - - // protected boolean isClimbing; - public IntakeCommand(DriveSubsystem driveSubsystem, int i) { - innerIntakeMotor = new CANSparkMax(IntakeConstants.innerIntakeMotor, MotorType.kBrushless); - innerIntakeMotor.restoreFactoryDefaults(); - innerIntakeMotor.setInverted(true); - innerIntakeMotor.setIdleMode(IdleMode.kBrake); - - innerIntakeMotor.setSmartCurrentLimit(35); - - intakeArmsVirtualSolenoidA = new Solenoid(PneumaticsModuleType.CTREPCM, 3); - intakeArmsVirtualSolenoidB = new Solenoid(PneumaticsModuleType.CTREPCM, 4); - intakeArmsVirtualSolenoidA.set(false); - intakeArmsVirtualSolenoidB.set(false); - - intakeArmsMotor = new CANSparkMax(IntakeConstants.intakeArmsMotor, MotorType.kBrushless); - intakeArmsMotor.restoreFactoryDefaults(); - intakeArmsMotor.setInverted(true); - - intakeArmsMotor.setSmartCurrentLimit(35); - - intakeSwitch = new DigitalInput(IntakeConstants.intakeSwitch); - } - public void enableInnerIntakeMotor() { - innerIntakeMotor.set(1); - } - public void reverseInnerIntakeMotor() { - innerIntakeMotor.set(-1); - } - public void disableInnerIntakeMotor() { - innerIntakeMotor.set(0); - } - public void setInnerIntakeMotor(double speed) { - this.innerIntakeMotor.set(speed); - } - - // implementing 836's idea - public void extendIntakeArms() { - intakeArmsVirtualSolenoidA.set(true); - intakeArmsVirtualSolenoidB.set(true); - } - - // implementing 836's idea - public void retractIntakeArms() { - intakeArmsVirtualSolenoidA.set(false); - intakeArmsVirtualSolenoidB.set(false); - } - - // implementing 836's idea - public void floatIntakeArms() { - intakeArmsVirtualSolenoidA.set(false); - intakeArmsVirtualSolenoidB.set(true); - } - - public void enableIntakeArmsMotor() { - intakeArmsMotor.set(-0.6); - } - public void disableIntakeArmsMotor() { - intakeArmsMotor.set(0); - } - public void reverseIntakeArmsMotor() { - intakeArmsMotor.set(0.6); - } - public double extendedIntakeArmsSpeed() { - return intakeArmsMotor.get(); - } - - public boolean getIntakeSwitch() { - return intakeSwitch.get(); - } -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java deleted file mode 100644 index 060c334d..00000000 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/IntakeConstants.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.jmhsrobotics.frc2024.subsystems.drive.commands; - -public final class IntakeConstants { - static int innerIntakeMotor = 1; - static int intakeArmsMotor = 1; - static int intakeSwitch = 1; -} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java new file mode 100644 index 00000000..c0d13816 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java @@ -0,0 +1,31 @@ +package org.jmhsrobotics.frc2024.subsystems.intake; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class IntakeSubsystem extends SubsystemBase { + private CANSparkMax intakeMotor; + private DigitalInput intakeSwitch; + + public IntakeSubsystem() { + intakeMotor = new CANSparkMax(32, MotorType.kBrushless); // TODO: move to constants file + intakeMotor.setInverted(true); + intakeMotor.setIdleMode(IdleMode.kBrake); + + intakeMotor.setSmartCurrentLimit(35); + + intakeSwitch = new DigitalInput(2); // TODO: move to constants file + } + + public void set(double speed) { + intakeMotor.set(speed); + } + + public boolean hasNote() { + return intakeSwitch.get(); + } +} From dded129b9ca794232c602eedc4db850bb05054b8 Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 27 Jan 2024 13:29:37 +0000 Subject: [PATCH 4/4] Add Simulation support to intake --- .../org/jmhsrobotics/frc2024/Constants.java | 20 +++++++++ .../jmhsrobotics/frc2024/RobotContainer.java | 4 +- .../subsystems/intake/IntakeSubsystem.java | 43 ++++++++++++++++++- 3 files changed, 64 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/Constants.java b/src/main/java/org/jmhsrobotics/frc2024/Constants.java index a3ebf711..6c5b92a0 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/Constants.java +++ b/src/main/java/org/jmhsrobotics/frc2024/Constants.java @@ -7,6 +7,26 @@ import edu.wpi.first.math.util.Units; public class Constants { + + /** + * CAN id assignments + */ + public static class CAN { + public static final int kIntakeId = 32; + + }; + /** + * Digital IO ports + */ + public static class DIO { + public static final int kIntakeSwitch = 2; + } + /** + * PWM output ports + */ + public static class PWM { + public static final int kLedStrip = 9; + } public static class SwerveConstants { // SPARK MAX CAN IDs public static final int kFrontLeftDrivingCanId = 10; diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index e40f0f8a..850ce75d 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -10,6 +10,7 @@ import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand; import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; +import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; import com.pathplanner.lib.auto.AutoBuilder; @@ -30,13 +31,14 @@ public class RobotContainer implements Logged { - private ControlBoard control = new CompControl(); + public ControlBoard control = new CompControl(); // Subsystems private final DriveSubsystem driveSubsystem = new DriveSubsystem(); private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem); private final LEDSubsystem ledSubsystem = new LEDSubsystem(); + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); private final SendableChooser autoChooser; diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java index c0d13816..52f3fdf0 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java @@ -1,10 +1,18 @@ package org.jmhsrobotics.frc2024.subsystems.intake; +import org.jmhsrobotics.frc2024.Constants; +import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper; + import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.DIOSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { @@ -12,13 +20,23 @@ public class IntakeSubsystem extends SubsystemBase { private DigitalInput intakeSwitch; public IntakeSubsystem() { - intakeMotor = new CANSparkMax(32, MotorType.kBrushless); // TODO: move to constants file + intakeMotor = new CANSparkMax(Constants.CAN.kIntakeId, MotorType.kBrushless); intakeMotor.setInverted(true); intakeMotor.setIdleMode(IdleMode.kBrake); intakeMotor.setSmartCurrentLimit(35); - intakeSwitch = new DigitalInput(2); // TODO: move to constants file + intakeSwitch = new DigitalInput(Constants.DIO.kIntakeSwitch); + if (RobotBase.isSimulation()) { + simInit(); + } + } + + @Override + public void periodic() { + SmartDashboard.putNumber("intake/velocityRPM", intakeMotor.getEncoder().getVelocity()); + SmartDashboard.putNumber("intake/currentDrawAmps", intakeMotor.getOutputCurrent()); + SmartDashboard.putBoolean("intake/hasNote", this.hasNote()); } public void set(double speed) { @@ -28,4 +46,25 @@ public void set(double speed) { public boolean hasNote() { return intakeSwitch.get(); } + + private DIOSim intakeSwitchSim; + private DCMotorSim intakeSim; + private RevEncoderSimWrapper intakeEncSim; + + public void simInit() { + intakeSwitchSim = new DIOSim(Constants.DIO.kIntakeSwitch); + intakeSim = new DCMotorSim(DCMotor.getNEO(1), 1, 0.3); + intakeEncSim = RevEncoderSimWrapper.create(intakeMotor); + } + + @Override + public void simulationPeriodic() { + double intakeVolts = intakeMotor.get() * 12; + intakeSim.setInput(intakeVolts); + intakeSim.update(0.2); + intakeSwitchSim.setValue(true); // TODO placeholder. + intakeEncSim.setDistance(intakeSim.getAngularPositionRotations()); + intakeEncSim.setVelocity(intakeSim.getAngularVelocityRPM()); + + } }