diff --git a/build.gradle b/build.gradle index 5b29be2..ecbb547 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" -} + id "edu.wpi.first.GradleRIO" version "2024.2.1" +} java { sourceCompatibility = JavaVersion.VERSION_17 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7dcc779..3638ab8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -99,7 +99,6 @@ public static final class OIConstants { public static final double kDeadband = 0.05; } - public static final class VisionConstants { public static final double kPoseErrorAcceptance = 2.0; // How much error there can be between current stimated pose and vision pose in meters } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9e476ed..dc20dda 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,19 +48,24 @@ import frc.robot.Constants.OIConstants; import frc.robot.commands.AutoCommand; import frc.robot.commands.SwerveJoystickCmd; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.PoseEstimator; +import frc.robot.subsystems.Shooter; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.Vision; +import pabeles.concurrency.IntRangeTask; public class RobotContainer { - private final Vision vision = new Vision(); - private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(vision); - private final PoseEstimator poseEstimator = new PoseEstimator(swerveSubsystem, vision, new Pose2d(2, 7, swerveSubsystem.getRotation2d())); - private AutoCommand autoComands = new AutoCommand(vision, swerveSubsystem); + private final Vision vision = new Vision(); + private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(vision); + private final PoseEstimator poseEstimator = new PoseEstimator(swerveSubsystem, vision, new Pose2d(2, 7, swerveSubsystem.getRotation2d())); + private AutoCommand autoComands = new AutoCommand(vision, swerveSubsystem); private final CommandXboxController driverJoytick = new CommandXboxController(OIConstants.kDriverControllerPort); private final Joystick buttonBox = new Joystick(1); + private Intake intakeUse = new Intake(); + private Shooter shooterUse = new Shooter(); private final SendableChooser autoChooser; @@ -73,7 +78,7 @@ public RobotContainer() { () -> -driverJoytick.getRawAxis(OIConstants.kDriverRotAxis), () -> !driverJoytick.b().getAsBoolean())); - NamedCommands.registerCommand("PathPlan", autoComands.toNote()); + // NamedCommands.registerCommand("PathPlan", autoComands.toNote()); configureButtonBindings(); @@ -87,14 +92,20 @@ public RobotContainer() { } private void configureButtonBindings() { - new JoystickButton(buttonBox, 3).onTrue(autoComands.toNote()); + // new JoystickButton(buttonBox, 1).onTrue(autoComands.toNote()); - PathPlannerPath spin = PathPlannerPath.fromPathFile("Spin"); + // PathPlannerPath spin = PathPlannerPath.fromPathFile("Spin"); driverJoytick.leftBumper().onTrue(swerveSubsystem.zeroHeadingCommand()); - driverJoytick.rightBumper().onTrue(AutoBuilder.followPath(spin)); - //new JoystickButton(buttonBox, 3).onTrue(PathPlan); - //new JoystickButton(buttonBox, 4).onTrue(autoComands.PathToPose(1, 1, 0)); + driverJoytick.leftTrigger().onTrue(intakeUse.toggle(0.1)); + driverJoytick.rightTrigger().onTrue(shooterUse.toggleSlow(0.1)); + driverJoytick.y().onTrue(intakeUse.toggle(0.7)); + // driverJoytick.rightBumper().onTrue(AutoBuilder.followPath(spin)); + new JoystickButton(buttonBox, 1).onTrue(intakeUse.toggle(0.7)); + new JoystickButton(buttonBox, 2).onTrue(shooterUse.toggleFast(0.1)); + new JoystickButton(buttonBox, 3).onTrue(shooterUse.toggleSlow(0.1)); } + // new JoystickButton(buttonBox, 4).onTrue(autoComands.PathToPose(1, 1, 0)); + // //cool spin move // new JoystickButton(buttonBox, 2).onTrue(Commands.runOnce(() -> { // //get current pose @@ -124,5 +135,7 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { swerveSubsystem.zeroHeading(); return autoChooser.getSelected(); - } + + +} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java new file mode 100644 index 0000000..96a90c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Conveyor extends SubsystemBase{ + private CANSparkMax conveyMotor1; + private CANSparkMax conveyMotor2; + private boolean isRunning; + private static final CANSparkLowLevel.MotorType kMotorType = CANSparkLowLevel.MotorType.kBrushless; + + public Conveyor() { + isRunning = false; + conveyMotor1 = new CANSparkMax(12, kMotorType); + conveyMotor2 = new CANSparkMax(13, kMotorType); + } + /** + * to be used by RobotContainer. will toggle shooter + * @param speed + * @return + */ + public Command toggle(double speed) { + return this.runOnce(() -> { isRunning = !(isRunning); makeSpeed(speed);}); + } + /** + * stops the motors + */ + public void stop() { + isRunning = false; + makeSpeed(0); + } + /** + * moves up to shooter + * @param speed + */ + public void run(double speed) { + isRunning = true; + makeSpeed(speed); + } + /** + * sets the speed of both motors, only accesable in this class + * speed shall NEVER exceed domain (-1,1) + * @param speed + */ + private void makeSpeed(double speed) { + if(isRunning) { + conveyMotor1.set(speed); + conveyMotor2.set(speed); + } + else { + conveyMotor1.set(0); + conveyMotor2.set(0); + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..11302f4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class Intake extends SubsystemBase { + private boolean isOn; + private final CANSparkMax intakeMotor; + private final CANSparkMax intakeMotor2; + private static final CANSparkLowLevel.MotorType kMotorType = CANSparkLowLevel.MotorType.kBrushless; + /** + * Constructer + * starts off + */ + public Intake() { + isOn = false; + intakeMotor = new CANSparkMax(9 , kMotorType); + intakeMotor2 = new CANSparkMax(14, kMotorType); + } + public Command toggle(double speed) { + return this.runOnce(() -> {isOn = !(isOn); setSpeed(speed);}); + } + public void turnOn(double speed) { + isOn = true; + setSpeed(speed); + } + + public void turnOff(double speed) { + isOn = false; + setSpeed(0); + } + /** + * Speed Should NEVER exceed the domain (-1,1) + * @param speed + */ + private void setSpeed(double speed) { + if(isOn) { + intakeMotor.set(speed); + intakeMotor2.set(speed*-1); + } + else { + intakeMotor.set(0); + intakeMotor2.set(0);} + + + SmartDashboard.putBoolean("Intake", isOn); + } +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 0000000..266f31f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,85 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase{ + private CANSparkMax shootMotor1; + private CANSparkMax shootMotor2; + private boolean isRunning; + private boolean isFast; + private static final CANSparkLowLevel.MotorType kMotorType = CANSparkLowLevel.MotorType.kBrushless; + + public Shooter() { + isRunning = false; + isFast = false; + shootMotor1 = new CANSparkMax(10, kMotorType); + shootMotor2 = new CANSparkMax(11, kMotorType); + } + /** + * to be used by RobotContainer. will toggle shooter + * @param speed + * @return + */ + public Command toggleSlow(double speed) { + return this.runOnce(() -> {isFast = false; isRunning = !(isRunning); makeSpeed(speed);}); + } + /** + * to be used by RobotContainer. will also toggle shooter, but faster + * @param speed + * @return + */ + public Command toggleFast(double speed) { + return this.runOnce(() -> {isFast = true; isRunning = !(isRunning); makeSpeed(speed);}); + } + /** + * stops the motors + */ + public void stop() { + isRunning = false; + makeSpeed(0); + } + /** + * Ideal for amp + * @param speed + */ + public void runSlow(double speed) { + isRunning = true; + isFast = false; + makeSpeed(speed); + } + /** + * better for shooting upward and outward + * @param speed + */ + public void runFast(double speed) { + isRunning = true; + isFast = true; + makeSpeed(speed); + } + /** + * sets the speed of both motors, only accesable in this class + * speed shall NEVER exceed domain (-1,1) + * @param speed + */ + private void makeSpeed(double speed) { + if(isRunning) { + if(isFast) { + shootMotor1.set(speed); + shootMotor2.set(speed); + } + else { + shootMotor1.set(speed/2); + shootMotor2.set(speed/2); + } + } + else { + shootMotor1.set(0); + shootMotor2.set(0); + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 129df3c..625df1e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -153,7 +153,7 @@ public void zeroHeading() { } public Command zeroHeadingCommand() { - return this.runOnce(() -> this. gyro.reset()); + return this.runOnce(() -> this.gyro.reset()); } public double getHeading() {