From 9669b5e8571d7be8f063647848e972da8965e98e Mon Sep 17 00:00:00 2001
From: trombonegeek27 <kendalcook6@gmail.com>
Date: Sun, 28 Jan 2024 16:32:21 -0600
Subject: [PATCH] Conveyor Done

(thanks shane i stole your code :smile: )
---
 src/main/java/frc/robot/Constants.java        |  7 +++
 .../frc/robot/commands/ConveyorCommand.java   | 49 +++++++++++++++++++
 .../java/frc/robot/subsystems/Conveyor.java   | 47 ++++++++++++++++++
 3 files changed, 103 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/ConveyorCommand.java
 create mode 100644 src/main/java/frc/robot/subsystems/Conveyor.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4605020..b19e13b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -76,5 +76,12 @@ public static class MechanismConstants {
     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;
+
+    // Conveyor Motors
+    public static final int CONVEYOR_MOTOR_1 = 22;
+    public static final int CONVEYOR_MOTOR_2 = 23;
+    public static final boolean CONVEYOR_MOTOR_1_INVERTED = true;
+    public static final boolean CONVEYOR_MOTOR_2_INVERTED = false;
+    public static final double CONVEYOR_MOTOR_SPEED = 1;
   }
 }
diff --git a/src/main/java/frc/robot/commands/ConveyorCommand.java b/src/main/java/frc/robot/commands/ConveyorCommand.java
new file mode 100644
index 0000000..6363152
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ConveyorCommand.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.Conveyor;
+
+public class ConveyorCommand extends Command {
+  private final Conveyor m_ConveyorSubsystem;
+  // controller
+  // TODO: Change this to the correct controller
+  private final Joystick ConveyorJoystick = new Joystick(1);
+
+  public ConveyorCommand(Conveyor subsystem) {
+    m_ConveyorSubsystem = subsystem;
+    addRequirements(m_ConveyorSubsystem);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    // TODO: Change this to the correct button
+    if (ConveyorJoystick.getRawButton(3) == true) {
+      m_ConveyorSubsystem.enableConveyor();
+      // TODO: Change this to the correct button
+    } else if (ConveyorJoystick.getRawButton(4) == true) {
+      m_ConveyorSubsystem.reverseConveyor();
+      System.out.println("Conveyor Moving in Reverse");
+    } else {
+      m_ConveyorSubsystem.disableConveyor();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_ConveyorSubsystem.disableConveyor();
+    m_ConveyorSubsystem.enableConveyor();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
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..1e453cf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Conveyor.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 Conveyor extends SubsystemBase {
+
+  private final CANSparkMax conveyorMotor1 =
+      new CANSparkMax(MechanismConstants.CONVEYOR_MOTOR_1, CANSparkLowLevel.MotorType.kBrushless);
+  private final CANSparkMax conveyorMotor2 =
+      new CANSparkMax(MechanismConstants.CONVEYOR_MOTOR_2, CANSparkLowLevel.MotorType.kBrushless);
+
+  /** Creates a new Conveyor. */
+  public Conveyor() {
+    conveyorMotor1.setInverted(MechanismConstants.CONVEYOR_MOTOR_1_INVERTED);
+    conveyorMotor2.setInverted(MechanismConstants.CONVEYOR_MOTOR_2_INVERTED);
+  }
+
+  /** Sets the Conveyor motors to 100% power. */
+  public void enableConveyor() {
+    conveyorMotor1.set(MechanismConstants.CONVEYOR_MOTOR_SPEED);
+    conveyorMotor2.set(MechanismConstants.CONVEYOR_MOTOR_SPEED);
+  }
+
+  /** Sets the Conveyor motors to 0% power. */
+  public void disableConveyor() {
+    conveyorMotor1.set(0);
+    conveyorMotor2.set(0);
+  }
+
+  /** Sets the Conveyor motors to -100% power. (Reverse direction) */
+  public void reverseConveyor() {
+    conveyorMotor1.set(-MechanismConstants.CONVEYOR_MOTOR_SPEED);
+    conveyorMotor2.set(-MechanismConstants.CONVEYOR_MOTOR_SPEED);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}