diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6d6448..a87b13d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,109 +19,111 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; public class RobotContainer { - private Drivetrain drivetrain; - private Indexer indexer; - private Intake intake; - private Shooter shooter; + private Drivetrain drivetrain; + private Indexer indexer; + private Intake intake; + private Shooter shooter; - private CommandXboxController driver; + private CommandXboxController driver; - //constructor// - public RobotContainer() { - drivetrain = new Drivetrain(); - indexer = new Indexer(); - intake = new Intake(); - shooter = new Shooter(); + //constructor// + public RobotContainer() { + drivetrain = new Drivetrain(); + indexer = new Indexer(); + intake = new Intake(); + shooter = new Shooter(); - driver = new CommandXboxController(OIConstants.kDriverUSB); + driver = new CommandXboxController(OIConstants.kDriverUSB); - configureBindings(); - configureShuffleboard(); - } + configureBindings(); + configureShuffleboard(); + } - private void configureBindings() { - //default commands// - drivetrain.setDefaultCommand( - drivetrain.driveArcade( - null, - null - ) - ); + private void configureBindings() { + //default commands// + drivetrain.setDefaultCommand( + drivetrain.driveArcade( + driver::getLeftX, + driver::getLeftY + ) + ); - indexer.setDefaultCommand(indexer.stopIndexer()); - intake.setDefaultCommand(intake.stopIntake()); - shooter.setDefaultCommand(shooter.stopShooter()); + indexer.setDefaultCommand(indexer.stopIndexer()); + intake.setDefaultCommand(intake.stopIntake()); + shooter.setDefaultCommand(shooter.runShooter(driver::getRightTriggerAxis)); - //driver inputs// - driver.rightTrigger().whileTrue( - shooter.runShooter(driver.getRightTriggerAxis()) - ); - - driver.a().whileTrue( - intake.runIntake(1) - .alongWith(indexer.runIndexer(1)) - ); + //driver inputs// + driver.a().whileTrue( + intake.runIntake(1) + .alongWith(indexer.runIndexer(() -> 1)) + ); - driver.b().whileTrue( - intake.runIntake(-1) - .alongWith(indexer.runIndexer(-1)) - ); + driver.b().whileTrue( + intake.runIntake(-1) + .alongWith(indexer.runIndexer(() -> -1)) + ); - driver.x().onTrue(smartShooter()); - } + driver.x().onTrue(smartShooter()); + } - //configures shuffleboard// - private void configureShuffleboard() { - ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName); + //configures shuffleboard// + private void configureShuffleboard() { + ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName); - robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoder) - .withPosition(0, 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView); + robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoderPos) + .withPosition(0, 0) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); - robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoder) - .withPosition(2, 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView); + robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoderPos) + .withPosition(2, 0) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); - robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle) - .withPosition(0, 1) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kGyro); + robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle) + .withPosition(0, 1) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kGyro); - robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoder) - .withPosition(4, 0) - .withSize(2, 2) - .withWidget(BuiltInWidgets.kDial) - .withProperties(Map.of( - "Max", ShooterConstants.kShooterFireSpeed) - ); + robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoderRate) + .withPosition(4, 0) + .withSize(2, 2) + .withWidget(BuiltInWidgets.kDial) + .withProperties(Map.of( + "Max", ShooterConstants.kShooterFireSpeed) + ); - robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoder) - .withPosition(6, 0) - .withSize(2, 2) - .withWidget(BuiltInWidgets.kDial) - .withProperties(Map.of( - "Max", ShooterConstants.kShooterFireSpeed) - ); - } + robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoderRate) + .withPosition(6, 0) + .withSize(2, 2) + .withWidget(BuiltInWidgets.kDial) + .withProperties(Map.of( + "Max", ShooterConstants.kShooterFireSpeed) + ); + } - public Command getAutonomousCommand() { - return drivetrain.moveDistance(1.5, 5) - .andThen(drivetrain.goToAngle(45, 2)) - .andThen(shooter.runAutoShooter()) - .until(shooter::shooterAtSpeed) - .andThen(shooter.runAutoShooter()) - .alongWith(indexer.runIndexer(1)) - .withTimeout(2); - } + //moves, rotates, runs auto shooter// + public Command getAutonomousCommand() { + drivetrain.resetGyro(); + drivetrain.resetDriveEncoders(); - //smart commands// - private Command smartShooter() { - return shooter.runAutoShooter() - .until(shooter::shooterAtSpeed) - .andThen(shooter.runAutoShooter()) - .alongWith(indexer.runIndexer(1)) - .withTimeout(2); - } + return drivetrain.moveDistance(1.5, 5) + .andThen( + drivetrain.goToAngle( + 45, + 2 + ), + smartShooter() + ); + } + + //smart commands// + private Command smartShooter() { + return shooter.runAutoShooter() + .until(shooter::shooterAtSpeed) + .andThen( + shooter.runAutoShooter(), + indexer.runIndexer(() -> 1) + ).withTimeout(2); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index ca840e0..2dc0019 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -25,4 +25,5 @@ public class ShooterConstants { public static final int kFFG = 0; public static final int kFFV = 0; + public static final int kBangBangScale = 8; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index fb7b044..066750e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.DrivetrainConstants; public class Drivetrain extends SubsystemBase { - //creates instance variables// private CANSparkMax leftFront; private CANSparkMax leftRear; @@ -27,12 +26,12 @@ public class Drivetrain extends SubsystemBase { private ADXRS450_Gyro gyro; private PIDController turnController; - private PIDController distanceController; + private PIDController rightDistanceController; + private PIDController leftDistanceController; private DifferentialDrive drive; public Drivetrain() { - //creates instances of each motor// leftFront = new CANSparkMax(DrivetrainConstants.kLeftFrontCAN, MotorType.kBrushless); leftRear = new CANSparkMax(DrivetrainConstants.kLeftRearCAN, MotorType.kBrushless); @@ -57,28 +56,47 @@ public class Drivetrain extends SubsystemBase { ); turnController.setTolerance(DrivetrainConstants.kTurnControllerTolerance); - distanceController = new PIDController( + rightDistanceController = new PIDController( DrivetrainConstants.kDistanceControllerP, DrivetrainConstants.kDistanceControllerI, DrivetrainConstants.kDistanceControllerD ); - distanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance); + rightDistanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance); + + leftDistanceController = new PIDController( + DrivetrainConstants.kDistanceControllerP, + DrivetrainConstants.kDistanceControllerI, + DrivetrainConstants.kDistanceControllerD + ); + leftDistanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance); drive = new DifferentialDrive(leftFront, rightFront); } - //getters/ + //gyro methods// public double getGyroAngle() { return gyro.getAngle() * (DrivetrainConstants.kInvertGyro ? -1 : 1); } - public double getLeftEncoder() { + public void resetGyro() { + gyro.reset(); + return; + } + + //encoder methods// + public double getLeftEncoderPos() { return leftFrontEncoder.getPosition(); } - public double getRightEncoder() { + public double getRightEncoderPos() { return rightFrontEncoder.getPosition(); } + + public void resetDriveEncoders() { + leftFrontEncoder.setPosition(0); + rightFrontEncoder.setPosition(0); + return; + } //method to turn drivetrain a certain angle// public Command goToAngle(double angle, double timeout) { @@ -94,12 +112,16 @@ public class Drivetrain extends SubsystemBase { //method to move drivetrain a certain distance// public Command moveDistance(double distance, double timeout) { return run(() -> { - double output = distanceController.calculate( + double outputL = rightDistanceController.calculate( (leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2, distance ); - drive.arcadeDrive(output, 0.0); - }).until(distanceController::atSetpoint).withTimeout(timeout); + double outputR = rightDistanceController.calculate( + (leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2, + distance + ); + drive.tankDrive(outputL, outputR); + }).until(rightDistanceController::atSetpoint).withTimeout(timeout); } //methods that take 2 doubles and convert them to motor commands// diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index a159006..9a3278c 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import edu.wpi.first.wpilibj2.command.Command; @@ -15,9 +17,9 @@ public class Indexer extends SubsystemBase { } //indexer commands// - public Command runIndexer(double speed) { + public Command runIndexer(DoubleSupplier speed) { return run(() -> { - motor1.set(speed); + motor1.set(speed.getAsDouble()); }); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0e142a4..94bddda 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import edu.wpi.first.math.controller.BangBangController; @@ -14,6 +16,7 @@ public class Shooter extends SubsystemBase { private WPI_VictorSPX rearMotor; private BangBangController bangBangController; + private SimpleMotorFeedforward motorFeedforward; private Encoder frontMotorEncoder; @@ -50,33 +53,33 @@ public class Shooter extends SubsystemBase { } //getters// - public double getFrontEncoder() { + public double getFrontEncoderRate() { return frontMotorEncoder.getRate(); } - public double getRearEncoder() { + public double getRearEncoderRate() { return rearMotorEncoder.getRate(); } public boolean shooterAtSpeed() { - return (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2 >= ShooterConstants.kShooterFireSpeed; + return bangBangController.atSetpoint(); } - //shooter commands// + //shooter run/stop commands// public Command runAutoShooter() { return run(() -> { - double output = bangBangController.calculate( + double output = (bangBangController.calculate( (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2, - ShooterConstants.kShooterFireSpeed + ShooterConstants.kShooterFireSpeed) * ShooterConstants.kBangBangScale ) + motorFeedforward.calculate(ShooterConstants.kShooterFireSpeed); frontMotor.setVoltage(output); }); } - public Command runShooter(double speed) { + public Command runShooter(DoubleSupplier speed) { return run(() -> { - frontMotor.set(speed); + frontMotor.set(speed.getAsDouble()); }); }