Merge pull request #68 from frc937/implement-belts-motors-and-intake

Implement belts motors and intake
frc937 · Feb 20, 2024 · bdaf226 · bdaf226
2 parents 6a52f55 + b2b6a78
commit bdaf226
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -48,11 +48,20 @@ public static class MailboxPneumatics {
 
   /** Constants for the Belts system. */
   public static class MailboxBelts {
-    /** The channel on the PCM for the belt motor */
-    public static final int BELT_MOTOR_ID = 0;
+    /** The CAN ID for the upper belt motor */
+    public static final int UPPER_BELT_MOTOR_ID = 0;
 
-    /** The speed for the belt motor. */
+    /** The CAN ID for the lower belt motor */
+    public static final int LOWER_BELT_MOTOR_ID = 0;
+
+    /** The speed for the belt motors. */
     public static final double BELT_MOTOR_SPEED = 1;
+
+    /** Inversion state of the upper belt motor. */
+    public static final boolean UPPER_BELT_MOTOR_INVERTED = false;
+
+    /** Inversion state of the belts follower motor. */
+    public static final boolean BELTS_FOLLOWER_INVERSE_STATE = false;
   }
 
   /** Constants that are relating to the controllers. */
@@ -105,15 +114,24 @@ public static class Auto {
 
   /** Constants for the Intake System */
   public static class Intake {
-    /** Motor id of the Intake motor. */
-    public static final int INTAKE_MOTOR_ID = 0;
+    /** Motor id of the Lower Intake motor. */
+    public static final int LOWER_INTAKE_MOTOR_ID = 0;
+
+    /** Motor id of the Upper Intake motor. */
+    public static final int UPPER_INTAKE_MOTOR_ID = 0;
+
+    /** Inversion state of the upper intake motor. */
+    public static final boolean UPPER_INTAKE_MOTOR_INVERTED = false;
 
     // It was me, DIO!
     /** DIO Port ID for the Intake limit switch. */
     public static final int INTAKE_LIMIT_SWITCH_DIO_PORT = 0;
 
     /** Speed we want to run the Intake at. */
     public static final double INTAKE_MOTOR_SPEED = 1;
+
+    /** Inversion state for the intake follower motor. */
+    public static final boolean INTAKE_FOLLOWER_INVERSE_STATE = false;
   }
 
   /** Holds contstants for the Limelights. */

diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -20,18 +20,25 @@
 /** The intake of the robot. */
 public class Intake extends SubsystemBase {
 
-  private CANSparkMax intake;
+  private CANSparkMax intakeLower;
+  private CANSparkMax intakeUpper;
   private DigitalInput limitSwitch;
 
   /** Creates a new Intake. */
   public Intake() {
-    this.intake = new CANSparkMax(Constants.Intake.INTAKE_MOTOR_ID, MotorType.kBrushless);
+    this.intakeLower =
+        new CANSparkMax(Constants.Intake.LOWER_INTAKE_MOTOR_ID, MotorType.kBrushless);
+    this.intakeUpper =
+        new CANSparkMax(Constants.Intake.UPPER_INTAKE_MOTOR_ID, MotorType.kBrushless);
     this.limitSwitch = new DigitalInput(Constants.Intake.INTAKE_LIMIT_SWITCH_DIO_PORT);
+
+    intakeLower.follow(intakeUpper, Constants.Intake.INTAKE_FOLLOWER_INVERSE_STATE);
+    intakeUpper.setInverted(Constants.Intake.UPPER_INTAKE_MOTOR_INVERTED);
   }
 
   /** Runs the intake motors. */
   public void runIntake() {
-    intake.set(Constants.Intake.INTAKE_MOTOR_SPEED);
+    intakeUpper.set(Constants.Intake.INTAKE_MOTOR_SPEED);
   }
 
   /**
@@ -46,7 +53,7 @@ public boolean getLimitSwitch() {
 
   /** Stops the intake motors. */
   public void stop() {
-    intake.set(0);
+    intakeUpper.set(0);
   }
 
   @Override

diff --git a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java
@@ -18,21 +18,28 @@
 
 /** Subsystem for the mailbox belt. */
 public class MailboxBelts extends SubsystemBase {
-  private CANSparkMax beltMotor;
+  private CANSparkMax upperBeltMotor;
+  private CANSparkMax lowerBeltMotor;
 
   /** Constructer for MailboxBelts subsystem */
   public MailboxBelts() {
-    beltMotor = new CANSparkMax(Constants.MailboxBelts.BELT_MOTOR_ID, MotorType.kBrushless);
+    upperBeltMotor =
+        new CANSparkMax(Constants.MailboxBelts.UPPER_BELT_MOTOR_ID, MotorType.kBrushless);
+    lowerBeltMotor =
+        new CANSparkMax(Constants.MailboxBelts.LOWER_BELT_MOTOR_ID, MotorType.kBrushless);
+
+    lowerBeltMotor.follow(upperBeltMotor, Constants.MailboxBelts.BELTS_FOLLOWER_INVERSE_STATE);
+    upperBeltMotor.setInverted(Constants.MailboxBelts.UPPER_BELT_MOTOR_INVERTED);
   }
 
   /** Runs the belt. */
   public void runBelts() {
-    beltMotor.set(Constants.MailboxBelts.BELT_MOTOR_SPEED);
+    upperBeltMotor.set(Constants.MailboxBelts.BELT_MOTOR_SPEED);
   }
 
   /** Stops the belt. */
   public void stop() {
-    beltMotor.set(0);
+    upperBeltMotor.set(0);
   }
 
   @Override