From 3f3cb145294b74ded13845f2fef6ce844abe4a6a Mon Sep 17 00:00:00 2001 From: NoahLacks63 Date: Tue, 7 Jan 2025 22:25:08 +0000 Subject: [PATCH] First subsystems added --- .../constants/ClimberPivotConstants.java | 8 +++ .../constants/ClimberRollersConstants.java | 5 ++ .../robot/constants/ElevatorConstants.java | 13 ++++ .../robot/constants/ManipulatorConstants.java | 5 ++ .../frc/robot/constants/SensorsConstants.java | 5 ++ .../frc/robot/subsystems/ClimberPivot.java | 51 ++++++++++++++++ .../frc/robot/subsystems/ClimberRollers.java | 26 ++++++++ .../java/frc/robot/subsystems/Elevator.java | 60 +++++++++++++++++++ .../java/frc/robot/subsystems/Sensors.java | 17 ++++++ 9 files changed, 190 insertions(+) create mode 100644 src/main/java/frc/robot/constants/ClimberPivotConstants.java create mode 100644 src/main/java/frc/robot/constants/ClimberRollersConstants.java create mode 100644 src/main/java/frc/robot/constants/ElevatorConstants.java create mode 100644 src/main/java/frc/robot/constants/ManipulatorConstants.java create mode 100644 src/main/java/frc/robot/constants/SensorsConstants.java create mode 100644 src/main/java/frc/robot/subsystems/ClimberPivot.java create mode 100644 src/main/java/frc/robot/subsystems/ClimberRollers.java create mode 100644 src/main/java/frc/robot/subsystems/Elevator.java create mode 100644 src/main/java/frc/robot/subsystems/Sensors.java diff --git a/src/main/java/frc/robot/constants/ClimberPivotConstants.java b/src/main/java/frc/robot/constants/ClimberPivotConstants.java new file mode 100644 index 0000000..d8733d5 --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberPivotConstants.java @@ -0,0 +1,8 @@ +package frc.robot.constants; + +public class ClimberPivotConstants { + public static final int kPivotMotorID = 0; + public static final double kPIDControllerP = 0; + public static final double kPIDControllerI = 0; + public static final double kPIDControllerD = 0; +} diff --git a/src/main/java/frc/robot/constants/ClimberRollersConstants.java b/src/main/java/frc/robot/constants/ClimberRollersConstants.java new file mode 100644 index 0000000..60b7404 --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberRollersConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class ClimberRollersConstants { + public static final int kRollerMotorID = 0; +} diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java new file mode 100644 index 0000000..58ae1a6 --- /dev/null +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -0,0 +1,13 @@ +package frc.robot.constants; + +public class ElevatorConstants { + public static final int kMotor1ID = 0; + + public static final double kPIDControllerP = 0; + public static final double kPIDControllerI = 0; + public static final double kPIDControllerD = 0; + + public static final double kFeedForwardS = 0; + public static final double kFeedForwardG = 0; + public static final double kFeedForwardV = 0; +} diff --git a/src/main/java/frc/robot/constants/ManipulatorConstants.java b/src/main/java/frc/robot/constants/ManipulatorConstants.java new file mode 100644 index 0000000..79be5b0 --- /dev/null +++ b/src/main/java/frc/robot/constants/ManipulatorConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class ManipulatorConstants { + +} diff --git a/src/main/java/frc/robot/constants/SensorsConstants.java b/src/main/java/frc/robot/constants/SensorsConstants.java new file mode 100644 index 0000000..d2da857 --- /dev/null +++ b/src/main/java/frc/robot/constants/SensorsConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class SensorsConstants { + public static final int kClimberLimitSwitchID = 0; +} diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java new file mode 100644 index 0000000..123457a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ClimberPivotConstants; + +public class ClimberPivot extends SubsystemBase { + private SparkMax pivotMotor; + + private AbsoluteEncoder neoEncoder; + + private PIDController pidController; + + public ClimberPivot() { + pivotMotor = new SparkMax( + ClimberPivotConstants.kPivotMotorID, + MotorType.kBrushless + ); + pivotMotor.setInverted(true); + + neoEncoder = pivotMotor.getAbsoluteEncoder(); + + pidController = new PIDController( + ClimberPivotConstants.kPIDControllerP, + ClimberPivotConstants.kPIDControllerI, + ClimberPivotConstants.kPIDControllerD + ); + } + + public Command runPivot(double speed) { + return run(() -> { + pivotMotor.set(speed); + }); + } + + public Command goToAngle(double setpoint) { + return run(() -> { + pivotMotor.set( + pidController.calculate( + neoEncoder.getPosition(), + setpoint + ) + ); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberRollers.java b/src/main/java/frc/robot/subsystems/ClimberRollers.java new file mode 100644 index 0000000..e57b00d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberRollers.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ClimberRollersConstants; + +public class ClimberRollers extends SubsystemBase { + private SparkMax rollerMotor; + + public ClimberRollers() { + rollerMotor = new SparkMax( + ClimberRollersConstants.kRollerMotorID, + MotorType.kBrushless + ); + rollerMotor.setInverted(false); + } + + public Command runRoller(double speed) { + return run(() -> { + rollerMotor.set(speed); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..cf55ac2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ElevatorConstants; + +public class Elevator extends SubsystemBase { + private SparkMax motor1; + + private AbsoluteEncoder encoder; + + private PIDController pidController; + + private ElevatorFeedforward feedForward; + + public Elevator() { + motor1 = new SparkMax( + ElevatorConstants.kMotor1ID, + MotorType.kBrushless + ); + + encoder = motor1.getAbsoluteEncoder(); + + pidController = new PIDController( + ElevatorConstants.kPIDControllerP, + ElevatorConstants.kPIDControllerI, + ElevatorConstants.kPIDControllerD + ); + + feedForward = new ElevatorFeedforward( + ElevatorConstants.kFeedForwardS, + ElevatorConstants.kFeedForwardG, + ElevatorConstants.kFeedForwardV + ); + } + + public Command runElevator(double speed) { + return run(() -> { + motor1.set(speed); + }); + } + + public Command goToSetpoint(double setpoint) { + return run(() -> { + motor1.set(feedForward.calculate( + encoder.getVelocity(), + pidController.calculate( + encoder.getPosition(), + setpoint + ) + )); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/Sensors.java b/src/main/java/frc/robot/subsystems/Sensors.java new file mode 100644 index 0000000..9128054 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Sensors.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.SensorsConstants; + +public class Sensors extends SubsystemBase { + private DigitalInput climberLimitSwitch; + + public Sensors() { + climberLimitSwitch = new DigitalInput(SensorsConstants.kClimberLimitSwitchID); + } + + public boolean getClimberLimitSwitch() { + return climberLimitSwitch.get(); + } +}