Merge pull request #16 from frc-4931/Intake

Intake

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

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
       }

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<Command> 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();
-  }
+
+
+}
 }

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);
+        }
+    }
+
+}

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);
+    }
+}

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);
+        }
+    }
+
+}

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() {