[OLD] v2

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

@@ -27,6 +27,7 @@ public final static class SECONDARY_JOY {
       public static final int RUN_INTAKE_NECK_FEEDER_SHOOTER_BTN_ID = 7;
     }
   }
+
   public final static class CAN_IDS {
     public static final int LEFT_FRONT_FALCON_ID = 0;
     public static final int LEFT_REAR_FALCON_ID = 1;

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

@@ -5,7 +5,6 @@
 import edu.wpi.first.wpilibj2.command.*;
 
 import org.team4159.frc.robot.commands.arm.ToggleArm;
-import org.team4159.frc.robot.commands.arm.ZeroArm;
 import org.team4159.frc.robot.subsystems.*;
 
 import static org.team4159.frc.robot.Constants.*;
@@ -57,18 +56,18 @@ private void configureButtonBindings() {
       .whenPressed(new ToggleArm(arm));
 
     new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.RUN_NECK_BTN_ID)
-      .whenPressed(new InstantCommand(neck::neck))
+      .whenPressed(new InstantCommand(neck::feedCell))
       .whenReleased(new InstantCommand(neck::stop));
 
     new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.RUN_FEEDER_BTN_ID)
-      .whenPressed(new InstantCommand(feeder::feed, feeder))
+      .whenPressed(new InstantCommand(feeder::feedCell, feeder))
       .whenReleased(new InstantCommand(feeder::stop, feeder));
 
     new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.RUN_INTAKE_NECK_FEEDER_SHOOTER_BTN_ID)
       .whenPressed(new ParallelCommandGroup(
         new InstantCommand(intake::intakeCell, intake),
-        new InstantCommand(feeder::feed, feeder),
-        new InstantCommand(neck::neck),
+        new InstantCommand(feeder::feedCell, feeder),
+        new InstantCommand(neck::feedCell),
         new InstantCommand(() -> shooter.setRawSpeed(1))))
       .whenReleased(new ParallelCommandGroup(
         new InstantCommand(intake::stopIntaking, intake),

src/main/java/org/team4159/frc/robot/subsystems/Arm.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 
+import org.team4159.lib.hardware.util.ControllerUtil;
 
 import static org.team4159.frc.robot.Constants.*;
 
@@ -16,15 +17,6 @@ public class Arm extends PIDSubsystem {
   private DigitalInput arm_limit_switch;
   private Encoder arm_encoder;
 
-  private CANSparkMax configureSparkMax(CANSparkMax spark) {
-    spark.restoreFactoryDefaults();
-    //spark.setSmartCurrentLimit(40);
-    spark.setIdleMode(CANSparkMax.IdleMode.kBrake);
-    spark.burnFlash();
-
-    return spark;
-  }
-
   public Arm() {
     super(new PIDController(
       ARM_CONSTANTS.kP,
@@ -39,7 +31,7 @@ public Arm() {
       ARM_CONSTANTS.ENCODER_ENCODING_TYPE
     );
 
-    arm_spark = configureSparkMax(new CANSparkMax(CAN_IDS.ARM_SPARK_ID, CANSparkMax.MotorType.kBrushless));
+    arm_spark = ControllerUtil.configureSparkMax(new CANSparkMax(CAN_IDS.ARM_SPARK_ID, CANSparkMax.MotorType.kBrushless), CANSparkMax.IdleMode.kBrake);
 
     arm_spark.setInverted(true);
 

src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java

@@ -4,14 +4,13 @@
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import org.team4159.lib.control.signal.DriveSignal;
 import org.team4159.lib.control.signal.filters.LowPassFilterSource;
+import org.team4159.lib.hardware.util.ControllerUtil;
 
 import static org.team4159.frc.robot.Constants.*;
 
@@ -25,18 +24,11 @@ public class Drivetrain extends SubsystemBase {
 
   private boolean is_oriented_forward = true;
 
-  private TalonFX configureTalonFX(TalonFX talonSRX) {
-    talonSRX.configFactoryDefault();
-    talonSRX.setNeutralMode(NeutralMode.Coast);
-
-    return talonSRX;
-  }
-
   public Drivetrain() {
-    left_front_falcon = configureTalonFX(new WPI_TalonFX(CAN_IDS.LEFT_FRONT_FALCON_ID));
-    left_rear_falcon = configureTalonFX(new WPI_TalonFX(CAN_IDS.LEFT_REAR_FALCON_ID));
-    right_front_falcon = configureTalonFX(new WPI_TalonFX(CAN_IDS.RIGHT_FRONT_FALCON_ID));
-    right_rear_falcon = configureTalonFX(new WPI_TalonFX(CAN_IDS.RIGHT_REAR_TALON_ID));
+    left_front_falcon = ControllerUtil.configureTalonFX(new WPI_TalonFX(CAN_IDS.LEFT_FRONT_FALCON_ID));
+    left_rear_falcon = ControllerUtil.configureTalonFX(new WPI_TalonFX(CAN_IDS.LEFT_REAR_FALCON_ID));
+    right_front_falcon = ControllerUtil.configureTalonFX(new WPI_TalonFX(CAN_IDS.RIGHT_FRONT_FALCON_ID));
+    right_rear_falcon = ControllerUtil.configureTalonFX(new WPI_TalonFX(CAN_IDS.RIGHT_REAR_TALON_ID));
 
     left_front_falcon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
     right_front_falcon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);

src/main/java/org/team4159/frc/robot/subsystems/Feeder.java

@@ -6,23 +6,18 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
+import org.team4159.lib.hardware.util.ControllerUtil;
+
 import static org.team4159.frc.robot.Constants.*;
 
 public class Feeder extends SubsystemBase {
-  public TalonSRX feeder_talon_1;
-
-  private TalonSRX configureTalonSRX(TalonSRX talonSRX) {
-    talonSRX.configFactoryDefault();
-    talonSRX.setNeutralMode(NeutralMode.Brake);
-
-    return talonSRX;
-  }
+  public TalonSRX feeder_talon;
 
   public Feeder() {
-    feeder_talon_1 = configureTalonSRX(new TalonSRX(CAN_IDS.FEEDER_TALON_ONE_ID));
+    feeder_talon = ControllerUtil.configureTalonSRX(new TalonSRX(CAN_IDS.FEEDER_TALON_ONE_ID), NeutralMode.Brake);
   }
 
-  public void feed() {
+  public void feedCell() {
     setFeederSpeed(1);
   }
 
@@ -31,6 +26,6 @@ public void stop() {
   }
 
   public void setFeederSpeed(double speed) {
-    feeder_talon_1.set(ControlMode.PercentOutput, speed);
+    feeder_talon.set(ControlMode.PercentOutput, speed);
   }
 }

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

@@ -4,22 +4,15 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import org.team4159.lib.hardware.util.ControllerUtil;
+
 import static org.team4159.frc.robot.Constants.*;
 
 public class Intake extends SubsystemBase {
   private CANSparkMax intake_spark;
 
-  private CANSparkMax configureSparkMax(CANSparkMax spark) {
-    spark.restoreFactoryDefaults();
-    spark.setSmartCurrentLimit(40);
-    spark.setIdleMode(CANSparkMax.IdleMode.kCoast);
-    spark.burnFlash();
-
-    return spark;
-  }
-
   public Intake() {
-    intake_spark = configureSparkMax(new CANSparkMax(CAN_IDS.INTAKE_SPARK_ID, CANSparkMax.MotorType.kBrushless));
+    intake_spark = ControllerUtil.configureSparkMax(new CANSparkMax(CAN_IDS.INTAKE_SPARK_ID, CANSparkMax.MotorType.kBrushless));
   }
 
   public void setRawIntakeSpeed(double speed) {

src/main/java/org/team4159/frc/robot/subsystems/Neck.java

@@ -1,31 +1,27 @@
 package org.team4159.frc.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import org.team4159.frc.robot.Constants;
 
-public class Neck extends SubsystemBase {
-  private SpeedControllerGroup neck_talons;
+import org.team4159.lib.hardware.util.ControllerUtil;
 
-  private TalonSRX configureTalonSRX(TalonSRX talonSRX) {
-    talonSRX.configFactoryDefault();
-    talonSRX.setNeutralMode(NeutralMode.Brake);
+import static org.team4159.frc.robot.Constants.*;
 
-    return talonSRX;
-  }
+public class Neck extends SubsystemBase {
+  private SpeedControllerGroup neck_talons;
 
   public Neck() {
     neck_talons = new SpeedControllerGroup(
-      (WPI_TalonSRX) configureTalonSRX(new WPI_TalonSRX(Constants.CAN_IDS.NECK_TALON_ONE_ID)),
-      (WPI_TalonSRX) configureTalonSRX(new WPI_TalonSRX(Constants.CAN_IDS.NECK_TALON_TWO_ID)),
-      (WPI_TalonSRX) configureTalonSRX(new WPI_TalonSRX(Constants.CAN_IDS.NECK_TALON_THREE_ID))
+      (WPI_TalonSRX) ControllerUtil.configureTalonSRX(new WPI_TalonSRX(CAN_IDS.NECK_TALON_ONE_ID), NeutralMode.Brake),
+      (WPI_TalonSRX) ControllerUtil.configureTalonSRX(new WPI_TalonSRX(CAN_IDS.NECK_TALON_TWO_ID), NeutralMode.Brake),
+      (WPI_TalonSRX) ControllerUtil.configureTalonSRX(new WPI_TalonSRX(CAN_IDS.NECK_TALON_THREE_ID), NeutralMode.Brake)
     );
   }
 
-  public void neck() {
+  public void feedCell() {
     neck_talons.set(1);
   }
 

src/main/java/org/team4159/frc/robot/subsystems/Shooter.java

@@ -1,49 +1,33 @@
 package org.team4159.frc.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import org.team4159.lib.hardware.util.ControllerUtil;
+
 import static org.team4159.frc.robot.Constants.*;
 
 public class Shooter extends SubsystemBase {
-  private TalonSRX shooter_talon_one, shooter_talon_two;
-  private VictorSPX shooter_victor_one, shooter_victor_two;
+  private TalonSRX left_shooter_talon, right_shooter_talon;
+  private VictorSPX left_shooter_victor, right_shooter_victor;
 
   private double goal_speed = 0.0;
   private boolean enabled = false;
 
-  private TalonSRX configureTalonSRX(TalonSRX talonSRX) {
-    talonSRX.configFactoryDefault();
-    talonSRX.setNeutralMode(NeutralMode.Coast);
-
-    return talonSRX;
-  }
-
-  private VictorSPX configureVictorSPX(VictorSPX victorSPX) {
-    victorSPX.configFactoryDefault();
-    victorSPX.setNeutralMode(NeutralMode.Coast);
-
-    return victorSPX;
-  }
-
   public Shooter() {
-    shooter_talon_one = configureTalonSRX(new TalonSRX(CAN_IDS.SHOOTER_TALON_ONE_ID));
-    shooter_talon_two = configureTalonSRX(new TalonSRX(CAN_IDS.SHOOTER_TALON_TWO_ID));
+    left_shooter_talon = ControllerUtil.configureTalonSRX(new TalonSRX(CAN_IDS.SHOOTER_TALON_ONE_ID));
+    right_shooter_talon = ControllerUtil.configureTalonSRX(new TalonSRX(CAN_IDS.SHOOTER_TALON_TWO_ID));
 
-    shooter_victor_one = configureVictorSPX(new VictorSPX(CAN_IDS.SHOOTER_VICTOR_ONE_ID));
-    shooter_victor_two = configureVictorSPX(new VictorSPX(CAN_IDS.SHOOTER_VICTOR_TWO_ID));
+    left_shooter_victor = ControllerUtil.configureVictorSPX(new VictorSPX(CAN_IDS.SHOOTER_VICTOR_ONE_ID));
+    right_shooter_victor = ControllerUtil.configureVictorSPX(new VictorSPX(CAN_IDS.SHOOTER_VICTOR_TWO_ID));
 
-    shooter_talon_two.follow(shooter_talon_one);
-    shooter_victor_one.follow(shooter_talon_one);
-    shooter_victor_two.follow(shooter_victor_two);
+    right_shooter_talon.follow(left_shooter_talon);
+    left_shooter_victor.follow(left_shooter_talon);
+    right_shooter_victor.follow(right_shooter_victor);
 
     SmartDashboard.putNumber("target_shooter_speed", 0);
     SmartDashboard.putNumber("shooter_kp", SHOOTER_CONSTANTS.kP);
@@ -67,31 +51,31 @@ public void periodic() {
   }
 
   public void setRawSpeed(double percent) {
-    shooter_talon_one.set(ControlMode.PercentOutput, percent);
+    left_shooter_talon.set(ControlMode.PercentOutput, percent);
   }
 
   public void setTargetSpeed(double speed) {
-    shooter_talon_one.set(ControlMode.MotionMagic, goal_speed);
+    left_shooter_talon.set(ControlMode.MotionMagic, goal_speed);
   }
 
   public void setP(double kP) {
-    shooter_talon_one.config_kP(0, kP);
+    left_shooter_talon.config_kP(0, kP);
   }
 
   public void setI(double kI) {
-    shooter_talon_one.config_kI(0, kI);
+    left_shooter_talon.config_kI(0, kI);
   }
 
   public void setD(double kD) {
-    shooter_talon_one.config_kD(0, kD);
+    left_shooter_talon.config_kD(0, kD);
   }
 
   public void setGoal(double goal) {
     goal_speed = goal;
   }
 
   public double getPosition() {
-    return (shooter_talon_one.getSelectedSensorVelocity() + shooter_talon_two.getSelectedSensorVelocity()) / 2.0;
+    return (left_shooter_talon.getSelectedSensorVelocity() + right_shooter_talon.getSelectedSensorVelocity()) / 2.0;
   }
 
   public void enable() {

src/main/java/org/team4159/lib/hardware/util/ControllerUtil.java

@@ -0,0 +1,61 @@
+package org.team4159.lib.hardware.util;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+import com.revrobotics.CANSparkMax;
+
+public class ControllerUtil {
+  public static final int kSparkMaxDefaultCurrentLimit = 40;
+
+  public static TalonFX configureTalonFX(TalonFX talonFX) {
+    return configureTalonFX(talonFX, NeutralMode.Coast);
+  }
+
+  public static TalonFX configureTalonFX(TalonFX talonFX, NeutralMode mode) {
+    talonFX.configFactoryDefault();
+    talonFX.setNeutralMode(mode);
+
+    return talonFX;
+  }
+
+  public static TalonSRX configureTalonSRX(TalonSRX talonSRX) {
+    return configureTalonSRX(talonSRX, NeutralMode.Coast);
+  }
+
+  public static TalonSRX configureTalonSRX(TalonSRX talonSRX, NeutralMode mode) {
+    talonSRX.configFactoryDefault();
+    talonSRX.setNeutralMode(mode);
+
+    return talonSRX;
+  }
+
+  public static VictorSPX configureVictorSPX(VictorSPX victorSPX) {
+    return configureVictorSPX(victorSPX, NeutralMode.Coast);
+  }
+
+  public static VictorSPX configureVictorSPX(VictorSPX victorSPX, NeutralMode mode) {
+    victorSPX.configFactoryDefault();
+    victorSPX.setNeutralMode(mode);
+
+    return victorSPX;
+  }
+
+  public static CANSparkMax configureSparkMax(CANSparkMax spark) {
+    return configureSparkMax(spark, CANSparkMax.IdleMode.kCoast, kSparkMaxDefaultCurrentLimit);
+  }
+
+  public static CANSparkMax configureSparkMax(CANSparkMax spark, CANSparkMax.IdleMode mode) {
+    return configureSparkMax(spark, mode, kSparkMaxDefaultCurrentLimit);
+  }
+
+  public static CANSparkMax configureSparkMax(CANSparkMax spark, CANSparkMax.IdleMode mode, int current_limit) {
+    spark.restoreFactoryDefaults();
+    spark.setSmartCurrentLimit(current_limit);
+    spark.setIdleMode(mode);
+    spark.burnFlash();
+
+    return spark;
+  }
+}

src/main/java/org/team4159/lib/hardware/Limelight.java → src/main/java/org/team4159/lib/hardware/wrapper/Limelight.java

@@ -1,4 +1,4 @@
-package org.team4159.lib.hardware;
+package org.team4159.lib.hardware.wrapper;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;