From d29642b89dfe07d37d65d2bbd055a6ef029c5975 Mon Sep 17 00:00:00 2001 From: NoahLacks63 Date: Thu, 9 Jan 2025 17:53:06 +0000 Subject: [PATCH] fleshed out subsystems --- src/main/java/frc/robot/RobotContainer.java | 124 ++++++++++++++---- .../frc/robot/constants/ArmConstants.java | 6 + .../robot/constants/ElevatorConstants.java | 2 +- src/main/java/frc/robot/constants/IDs | 29 ++++ .../frc/robot/constants/IndexerConstants.java | 6 + .../robot/constants/ManipulatorConstants.java | 7 + .../java/frc/robot/constants/OIConstants.java | 1 + src/main/java/frc/robot/subsystems/Arm.java | 43 ++++++ .../frc/robot/subsystems/ClimberPivot.java | 1 - .../frc/robot/subsystems/ClimberRollers.java | 1 - .../java/frc/robot/subsystems/Elevator.java | 27 ++-- .../java/frc/robot/subsystems/Indexer.java | 37 ++++++ .../frc/robot/subsystems/Manipulator.java | 46 +++++++ 13 files changed, 291 insertions(+), 39 deletions(-) create mode 100644 src/main/java/frc/robot/constants/ArmConstants.java create mode 100644 src/main/java/frc/robot/constants/IDs create mode 100644 src/main/java/frc/robot/constants/IndexerConstants.java create mode 100644 src/main/java/frc/robot/constants/ManipulatorConstants.java create mode 100644 src/main/java/frc/robot/subsystems/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/Indexer.java create mode 100644 src/main/java/frc/robot/subsystems/Manipulator.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b4a813..e9bbe31 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,38 +5,118 @@ package frc.robot; import frc.robot.constants.OIConstants; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.ClimberPivot; +import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Manipulator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { - private DriveSubsystem drivetrain; + private Arm arm; - private CommandXboxController driver; + private ClimberPivot climberPivot; - public RobotContainer() { - drivetrain = new DriveSubsystem(); + private ClimberRollers climberRollers; - driver = new CommandXboxController(OIConstants.kDriverControllerPort); - - configureButtonBindings(); - } + private DriveSubsystem drivetrain; - private void configureButtonBindings() { - drivetrain.setDefaultCommand( - drivetrain.drive( - driver::getLeftY, - driver::getLeftX, - driver::getRightX, - () -> true - ) - ); + private Elevator elevator; - driver.start().whileTrue(drivetrain.setXCommand()); - } + private Indexer indexer; - public Command getAutonomousCommand() { - return new PrintCommand("NO AUTO DEFINED"); - } + private Manipulator manipulator; + + private CommandXboxController driver; + private CommandXboxController operator; + + public RobotContainer() { + arm = new Arm(); + + climberPivot = new ClimberPivot(); + + climberRollers = new ClimberRollers(); + + drivetrain = new DriveSubsystem(); + + elevator = new Elevator(); + + indexer = new Indexer(); + + manipulator = new Manipulator(); + + driver = new CommandXboxController(OIConstants.kDriverControllerPort); + operator = new CommandXboxController(OIConstants.kOperatorControllerPort); + + configureButtonBindings(); + } + + private void configureButtonBindings() { + arm.setDefaultCommand( + arm.goToSetpoint(0, 1) + ); + + climberPivot.setDefaultCommand( + climberPivot.goToAngle(0) + ); + + climberRollers.setDefaultCommand( + climberRollers.runRoller(0) + ); + + drivetrain.setDefaultCommand( + drivetrain.drive( + driver::getLeftY, + driver::getLeftX, + driver::getRightX, + () -> true + ) + ); + + elevator.setDefaultCommand( + elevator.goToSetpoint(0, 1) + ); + + indexer.setDefaultCommand( + indexer.runIndexer(0) + ); + + manipulator.setDefaultCommand( + manipulator.runManipulator(0) + ); + + //Driver inputs + driver.start().whileTrue( + drivetrain.setXCommand() + ); + + driver.rightTrigger().whileTrue( + manipulator.runManipulator(1) + ); + + //Operator inputs + operator.povUp().onTrue( + elevator.goToSetpoint(0, 0) + ); + + operator.povRight().onTrue( + elevator.goToSetpoint(0, 0) + ); + + operator.povLeft().onTrue( + elevator.goToSetpoint(0, 0) + ); + + operator.povDown().onTrue( + elevator.goToSetpoint(0, 0) + ); + } + + public Command getAutonomousCommand() { + return new PrintCommand("NO AUTO DEFINED"); + } } diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java new file mode 100644 index 0000000..8ab9aa3 --- /dev/null +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class ArmConstants { + public static final int kArmMotorID = 0; + public static final int kCANcoderID = 0; +} diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 58ae1a6..687edf3 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -1,7 +1,7 @@ package frc.robot.constants; public class ElevatorConstants { - public static final int kMotor1ID = 0; + public static final int kElevatorMotorID = 0; public static final double kPIDControllerP = 0; public static final double kPIDControllerI = 0; diff --git a/src/main/java/frc/robot/constants/IDs b/src/main/java/frc/robot/constants/IDs new file mode 100644 index 0000000..9900f2d --- /dev/null +++ b/src/main/java/frc/robot/constants/IDs @@ -0,0 +1,29 @@ +CAN +---------------------------- +ARM +kArmMotorID = 0 +kCANcoderID = 0 + +CLIMBER +kPivotMotorID = 0 +kRollerMotorID = 0 + +DRIVEBASE +kFrontLeftDrivingCanId = 11 +kRearLeftDrivingCanId = 13 +kFrontRightDrivingCanId = 15 +kRearRightDrivingCanId = 17 +kFrontLeftTurningCanId = 10 +kRearLeftTurningCanId = 12 +kFrontRightTurningCanId = 14 +kRearRightTurningCanId = 16 + +ELEVATOR +kElevatorMotorID = 0 + +MANIPULATOR +kManipulatorMotorID = 0 + +PWM +---------------------------- + diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java new file mode 100644 index 0000000..cdec2fe --- /dev/null +++ b/src/main/java/frc/robot/constants/IndexerConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class IndexerConstants { + public static final int kIndexerMotorID = 0; + public static final int kIndexerBeamBreakID = 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..6bdb595 --- /dev/null +++ b/src/main/java/frc/robot/constants/ManipulatorConstants.java @@ -0,0 +1,7 @@ +package frc.robot.constants; + +public class ManipulatorConstants { + public static final int kManipulatorMotorID = 0; + public static final int kCoralBeamBreakID = 0; + public static final int kAlgaeBeamBreakID = 0; +} diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index 7b86c95..02616b9 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -2,6 +2,7 @@ package frc.robot.constants; public class OIConstants { public static final int kDriverControllerPort = 0; + public static final int kOperatorControllerPort = 1; public static final double kDriveDeadband = 0.05; } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..610930a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.ArmFeedforward; +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.ArmConstants; + +public class Arm extends SubsystemBase { + private SparkMax ArmMotor; + + private CANcoder canCoder; + + private PIDController pidController; + private ArmFeedforward feedForward; + + public Arm() { + ArmMotor = new SparkMax( + ArmConstants.kArmMotorID, + MotorType.kBrushless + ); + + canCoder = new CANcoder(ArmConstants.kCANcoderID); + } + + public Command goToSetpoint(double setpoint, double timeout) { + return run(() -> { + double voltsOut = pidController.calculate( + canCoder.getPosition().getValueAsDouble(), + setpoint + ) + feedForward.calculate( + canCoder.getPosition().getValueAsDouble(), + canCoder.getVelocity().getValueAsDouble() + ); + + ArmMotor.setVoltage(voltsOut); + }).until(pidController::atSetpoint).withTimeout(timeout); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java index fa6a39e..694105f 100644 --- a/src/main/java/frc/robot/subsystems/ClimberPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -24,7 +24,6 @@ public class ClimberPivot extends SubsystemBase { ClimberPivotConstants.kPivotMotorID, MotorType.kBrushless ); - pivotMotor.setInverted(true); neoEncoder = pivotMotor.getAbsoluteEncoder(); diff --git a/src/main/java/frc/robot/subsystems/ClimberRollers.java b/src/main/java/frc/robot/subsystems/ClimberRollers.java index e57b00d..61aa276 100644 --- a/src/main/java/frc/robot/subsystems/ClimberRollers.java +++ b/src/main/java/frc/robot/subsystems/ClimberRollers.java @@ -15,7 +15,6 @@ public class ClimberRollers extends SubsystemBase { ClimberRollersConstants.kRollerMotorID, MotorType.kBrushless ); - rollerMotor.setInverted(false); } public Command runRoller(double speed) { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index cf55ac2..baf11b4 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; public class Elevator extends SubsystemBase { - private SparkMax motor1; + private SparkMax elevatorMotor1; private AbsoluteEncoder encoder; @@ -20,12 +20,12 @@ public class Elevator extends SubsystemBase { private ElevatorFeedforward feedForward; public Elevator() { - motor1 = new SparkMax( - ElevatorConstants.kMotor1ID, + elevatorMotor1 = new SparkMax( + ElevatorConstants.kElevatorMotorID, MotorType.kBrushless ); - encoder = motor1.getAbsoluteEncoder(); + encoder = elevatorMotor1.getAbsoluteEncoder(); pidController = new PIDController( ElevatorConstants.kPIDControllerP, @@ -42,19 +42,18 @@ public class Elevator extends SubsystemBase { public Command runElevator(double speed) { return run(() -> { - motor1.set(speed); + elevatorMotor1.set(speed); }); } - public Command goToSetpoint(double setpoint) { + public Command goToSetpoint(double setpoint, double timeout) { return run(() -> { - motor1.set(feedForward.calculate( - encoder.getVelocity(), - pidController.calculate( - encoder.getPosition(), - setpoint - ) - )); - }); + double voltsOut = pidController.calculate( + encoder.getPosition(), + setpoint + ) + feedForward.calculate(0); + + elevatorMotor1.setVoltage(voltsOut); + }).until(pidController::atSetpoint).withTimeout(timeout); } } diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java new file mode 100644 index 0000000..e114c63 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.IndexerConstants; + +public class Indexer extends SubsystemBase { + private SparkMax indexerMotor; + + private DigitalInput indexerBeamBreak; + + public Indexer() { + indexerMotor = new SparkMax( + IndexerConstants.kIndexerMotorID, + MotorType.kBrushless + ); + + indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID); + } + + public Command runIndexer(double speed) { + return run(() -> { + indexerMotor.set(speed); + }); + } + + public Command indexCoral(double speed) { + return run(() -> { + indexerMotor.set(speed); + }) + .until(indexerBeamBreak::get); + } +} diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java new file mode 100644 index 0000000..361a689 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ManipulatorConstants; + +public class Manipulator extends SubsystemBase { + private SparkMax manipulatorMotor; + + private DigitalInput coralBeamBreak; + private DigitalInput algaeBeamBreak; + + public Manipulator() { + manipulatorMotor = new SparkMax( + ManipulatorConstants.kManipulatorMotorID, + MotorType.kBrushless + ); + + coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID); + algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID); + } + + public Command runManipulator(double speed) { + return run(() -> { + manipulatorMotor.set(speed); + }); + } + + public Command runUntilCollected(double speed, boolean coral) { + return run(() -> { + manipulatorMotor.set(coral ? speed : speed * -1); + }) + .until(() -> coralBeamBreak.get() || algaeBeamBreak.get()); + } + + public Command indexCoral(double speed) { + return run(() -> { + manipulatorMotor.set(speed); + }) + .until(coralBeamBreak::get); + } +}