From 0f6276356771930c41c1831e61c51e0a9add27f8 Mon Sep 17 00:00:00 2001 From: Rishi Nath Date: Fri, 7 Feb 2020 22:27:57 -0800 Subject: [PATCH] format & controller util class --- .../org/team4159/frc/robot/Constants.java | 1 + .../team4159/frc/robot/RobotContainer.java | 9 ++- .../team4159/frc/robot/subsystems/Arm.java | 12 +--- .../frc/robot/subsystems/Drivetrain.java | 18 ++---- .../team4159/frc/robot/subsystems/Feeder.java | 17 ++---- .../team4159/frc/robot/subsystems/Intake.java | 13 +--- .../team4159/frc/robot/subsystems/Neck.java | 22 +++---- .../frc/robot/subsystems/Shooter.java | 52 ++++++---------- .../lib/hardware/util/ControllerUtil.java | 61 +++++++++++++++++++ .../lib/hardware/{ => wrapper}/Limelight.java | 2 +- 10 files changed, 110 insertions(+), 97 deletions(-) create mode 100644 src/main/java/org/team4159/lib/hardware/util/ControllerUtil.java rename src/main/java/org/team4159/lib/hardware/{ => wrapper}/Limelight.java (99%) diff --git a/src/main/java/org/team4159/frc/robot/Constants.java b/src/main/java/org/team4159/frc/robot/Constants.java index 7d85864..d97ba72 100644 --- a/src/main/java/org/team4159/frc/robot/Constants.java +++ b/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; diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index e05b93b..cd2d16a 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/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), diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java index eb179ec..43db1a4 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java +++ b/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); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index fc9e19d..22f3581 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/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); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java index 90f1779..07ad049 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java +++ b/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); } } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java index 81a9776..4f0f3af 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java +++ b/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) { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java index 05cc574..25c3a00 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java +++ b/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); } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index 2324e63..4b4e927 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/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,23 +51,23 @@ 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) { @@ -91,7 +75,7 @@ public void setGoal(double 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() { diff --git a/src/main/java/org/team4159/lib/hardware/util/ControllerUtil.java b/src/main/java/org/team4159/lib/hardware/util/ControllerUtil.java new file mode 100644 index 0000000..7b4407d --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/org/team4159/lib/hardware/Limelight.java b/src/main/java/org/team4159/lib/hardware/wrapper/Limelight.java similarity index 99% rename from src/main/java/org/team4159/lib/hardware/Limelight.java rename to src/main/java/org/team4159/lib/hardware/wrapper/Limelight.java index 80db105..846e4fc 100644 --- a/src/main/java/org/team4159/lib/hardware/Limelight.java +++ b/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;