Merge pull request #8 from OakvilleDynamics/subsystem/intake

Subsystem/intake

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

@@ -59,10 +59,22 @@ public static class OperatorConstants {
     public static final int DRIVER_CONTROLLER = 0;
     public static final int COPILOT_CONTROLLER = 1;
 
+    // TODO: Add button binds for driver controllers
+
     // Joystick Deadband
     public static final double LEFT_X_DEADBAND = 0.01;
     public static final double LEFT_Y_DEADBAND = 0.01;
     public static final double RIGHT_X_DEADBAND = 0.01;
     public static final double TURN_CONSTANT = 6;
   }
+
+  public static class MechanismConstants {
+
+    // Intake Motors
+    public static final int INTAKE_MOTOR_1 = 20;
+    public static final int INTAKE_MOTOR_2 = 21;
+    public static final boolean INTAKE_MOTOR_1_INVERTED = true;
+    public static final boolean INTAKE_MOTOR_2_INVERTED = false;
+    public static final double INTAKE_MOTOR_SPEED = 1;
+  }
 }

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

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake;
+
+public class IntakeCommand extends Command {
+  private final Intake m_intakeSubsystem;
+  // controller
+  // TODO: Change this to the correct controller
+  private final Joystick IntakeJoystick = new Joystick(1);
+
+  public IntakeCommand(Intake subsystem) {
+    m_intakeSubsystem = subsystem;
+    addRequirements(m_intakeSubsystem);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    // TODO: Change this to the correct button
+    if (IntakeJoystick.getRawButton(3) == true) {
+      m_intakeSubsystem.enableIntake();
+      // TODO: Change this to the correct button
+    } else if (IntakeJoystick.getRawButton(4) == true) {
+      m_intakeSubsystem.reverseIntake();
+      System.out.println("Intake Moving in Reverse");
+    } else {
+      m_intakeSubsystem.disableIntake();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_intakeSubsystem.disableIntake();
+    m_intakeSubsystem.enableIntake();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.MechanismConstants;
+
+public class Intake extends SubsystemBase {
+
+  private final CANSparkMax intakeMotor1 =
+      new CANSparkMax(MechanismConstants.INTAKE_MOTOR_1, CANSparkLowLevel.MotorType.kBrushless);
+  private final CANSparkMax intakeMotor2 =
+      new CANSparkMax(MechanismConstants.INTAKE_MOTOR_2, CANSparkLowLevel.MotorType.kBrushless);
+
+  /** Creates a new Intake. */
+  public Intake() {
+    intakeMotor1.setInverted(MechanismConstants.INTAKE_MOTOR_1_INVERTED);
+    intakeMotor2.setInverted(MechanismConstants.INTAKE_MOTOR_2_INVERTED);
+  }
+
+  /** Sets the intake motors to 100% power. */
+  public void enableIntake() {
+    intakeMotor1.set(MechanismConstants.INTAKE_MOTOR_SPEED);
+    intakeMotor2.set(MechanismConstants.INTAKE_MOTOR_SPEED);
+  }
+
+  /** Sets the intake motors to 0% power. */
+  public void disableIntake() {
+    intakeMotor1.set(0);
+    intakeMotor2.set(0);
+  }
+
+  /** Sets the intake motors to -100% power. (Reverse direction) */
+  public void reverseIntake() {
+    intakeMotor1.set(-MechanismConstants.INTAKE_MOTOR_SPEED);
+    intakeMotor2.set(-MechanismConstants.INTAKE_MOTOR_SPEED);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}