diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f1478df..4605020 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..995114f --- /dev/null +++ b/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; + } +} 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..7d20f0f --- /dev/null +++ b/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 + } +}