Intake subsystem

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/frc/robot/Constants.java

@@ -7,11 +7,15 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.lib.util.COTSFalconSwerveConstants;
 import frc.lib.util.SwerveModuleConstants;
 
 public final class Constants {
     public static final double stickDeadband = 0.1;
+    public static final XboxController driverControls = new XboxController(0);
+    public static final XboxController operatorControls = new XboxController(1);
 
     public static final class Swerve {
         public static final int pigeonID = 14;
@@ -26,6 +30,7 @@ public static final class Swerve {
         public static final double wheelBase = Units.inchesToMeters(21.25); //TODO: This must be tuned to specific robot
         public static final double wheelCircumference = chosenModule.wheelCircumference;
 
+
         /* Swerve Kinematics 
          * No need to ever change this unless you are not doing a traditional rectangular/square 4 module swerve */
          public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(
@@ -146,4 +151,19 @@ public static final class AutoConstants { //TODO: The below constants are used i
             new TrapezoidProfile.Constraints(
                 kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
     }
+
+    public static final class IntakeConstants {
+        public static final int INTAKE_MOTOR = 13;
+        public static final double INTAKE_SPEED = 0.3;
+        public static Boolean intakeRunning = false;
+        public static final JoystickButton CONT_INTAKE_RUN = new JoystickButton(operatorControls, XboxController.Button.kRightBumper.value);
+        public static final JoystickButton CONT_INTAKE_RUN_REV = new JoystickButton(operatorControls, XboxController.Button.kLeftBumper.value);
+        public static final int coneSensorID = 0;
+        public static final int cubeSensorID = 0;
+    }
+
+    public static final class WristConstants {
+        public static final int WRIST_MOTOR = 17;
+
+    }
 }

src/main/java/frc/robot/Main.java

@@ -21,5 +21,6 @@ private Main() {}
    */
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);
+
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -3,10 +3,11 @@
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
+import frc.robot.Constants.IntakeConstants;
 import frc.robot.autos.*;
 import frc.robot.commands.*;
 import frc.robot.subsystems.*;
@@ -32,6 +33,7 @@ public class RobotContainer {
 
     /* Subsystems */
     private final Swerve s_Swerve = new Swerve();
+    private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
 
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -58,7 +60,35 @@ public RobotContainer() {
      */
     private void configureButtonBindings() {
         /* Driver Buttons */
+
+
+        /*Zero Gyro */
         zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro()));
+
+        // if(IntakeConstants.intakeRunning == false) { 
+
+        //    /*Run Intake- FORWARD */
+
+        //    IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.run()));
+
+        //    /*Run Intake- REVERSE */
+
+        //    IntakeConstants.CONT_INTAKE_RUN_REV.onTrue(new InstantCommand(() -> m_IntakeSubsystem.runReverse()));
+
+        // } else if (IntakeConstants.intakeRunning == true) {
+
+        //    /*Stop Intake- Both ways */
+
+        //    IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.stop()));
+        //    IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.stop()));
+
+        // }
+         IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.run()));
+         IntakeConstants.CONT_INTAKE_RUN.onFalse(new InstantCommand(() -> m_IntakeSubsystem.stop()));
+
+         IntakeConstants.CONT_INTAKE_RUN_REV.onTrue(new InstantCommand(() -> m_IntakeSubsystem.runReverse()));
+         IntakeConstants.CONT_INTAKE_RUN_REV.onFalse(new InstantCommand(() -> m_IntakeSubsystem.stop()));
+
     }
 
     /**

src/main/java/frc/robot/commands/WristCommand.java

@@ -1,9 +1,15 @@
 package frc.robot.commands;
-import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.WristConstants;
 
-public class WristSubsystem extends SubsystemBase{
-    CANSparkMax motor = new CANSparkMax(17, MotorType.kBrushless);
+public class WristCommand extends CommandBase{
+    CANSparkMax motor = new CANSparkMax(WristConstants.WRIST_MOTOR, MotorType.kBrushless);
     public void positive () {
         motor.set(1);
      }    

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -1,6 +1,57 @@
 package frc.robot.subsystems;
+
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+
+import edu.wpi.first.wpilibj.DigitalInput;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
 
-public class IntakeSubsystem extends SubsystemBase{
+public class IntakeSubsystem extends SubsystemBase {
+    DigitalInput coneSensor = new DigitalInput(IntakeConstants.coneSensorID);
+    DigitalInput cubeSensor = new DigitalInput(IntakeConstants.cubeSensorID);
 
+    private boolean debug = false;
+
+    private CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR, MotorType.kBrushless);
+    private double motorSpeed = 0;
+
+    // private static final Spark m_intakeRoller = new Spark(INTAKE_ROLLER_CAN_NUM
+
+    public IntakeSubsystem() {
+    }
+
+    public void setDebug(boolean debug) {
+        this.debug = debug;
+    }
+
+    public void run(){
+        motorSpeed = IntakeConstants.INTAKE_SPEED;
+        IntakeConstants.intakeRunning = true;
+    }
+
+    public void runReverse() {
+        motorSpeed = -(IntakeConstants.INTAKE_SPEED);
+        IntakeConstants.intakeRunning = true;
+    }
+
+    public void stop(){
+        motorSpeed = 0;
+        IntakeConstants.intakeRunning = false;
+    }
+
+    @Override
+    public void periodic() {
+        intakeMotor.set(motorSpeed);
+    }
+
+    public boolean isConePresent(){
+       return coneSensor.get();
+    }
+    public boolean isCubePresent(){
+        return cubeSensor.get();
+     }
 }