diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index f1d1b2d..ae23e6d 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -4,18 +4,23 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; - +import frc.robot.subsystems.vision.Vision; public class Orchestrator { private final Indexer indexer; private final Shooter shooter; + private final Vision vision; + private final Drive drive; - public Orchestrator(Indexer indexer, Shooter shooter) { + public Orchestrator(Indexer indexer, Shooter shooter, Vision vision, Drive drive) { this.indexer = indexer; this.shooter = shooter; + this.vision = vision; + this.drive = drive; } public Command spinUp(BooleanSupplier fastMode) { @@ -51,6 +56,12 @@ public Command shootCycle(BooleanSupplier fastMode) { .repeatedly()); } + public Command rotateToTag() { + return Commands.sequence( + drive.toAprilTag(vision.getYaw()) + ).repeatedly(); + } + public Command reverseAll() { return Commands.parallel(shooter.reverse(), indexer.reverse()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5274408..6f66929 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,7 @@ public class RobotContainer { private final Vision vision; private final Indexer indexer = new Indexer(new IndexerIOSparkMax()); private final Shooter shooter = new Shooter(new ShooterIOSparkMax()); - private final Orchestrator orchestrator = new Orchestrator(indexer, shooter); + private final Orchestrator orchestrator; private final CommandXboxController driverController = new CommandXboxController(0); private boolean fastMode = false; private final Trigger fastModeTrigger = new Trigger(() -> fastMode); @@ -45,6 +45,7 @@ public RobotContainer() { // Configure the trigger bindings drive = new Drive(new DriveIOSparkMax()); vision = new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){}); + orchestrator = new Orchestrator(indexer, shooter, vision, drive); configureBindings(); } @@ -67,9 +68,9 @@ private void configureBindings() { () -> driverController.getHID() .setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0) )); - drive.setDefaultCommand(drive.arcadeDrive( - driverController::getLeftY, - driverController::getRightY)); + drive.setDefaultCommand(drive.arcadeDrive( + driverController::getLeftY, + driverController::getRightY)); driverController .rightTrigger() @@ -85,6 +86,7 @@ private void configureBindings() { .rightBumper() .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); + driverController.x().whileTrue(orchestrator.rotateToTag()); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 21b398e..adb182e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; + import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { @@ -30,4 +31,12 @@ public Command arcadeDrive(DoubleSupplier speedX, DoubleSupplier rotation) { io.setVoltageRight(speeds.right * 7); }, this); } + + public Command toAprilTag(double yaw) { + return Commands.run( + () -> { + io.rotateToTag(yaw); + }, this + ); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4d7114a..d8ffeef 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -2,6 +2,8 @@ public class DriveConstants { - + public static final double PID_P = 0.0; + public static final double PID_I = 0.0; + public static final double PID_D = 0.0; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java index 71aa206..7f4018c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java @@ -26,6 +26,5 @@ default void setVelocityRadPerSecL(double leftVelocityRadPerSec){} default void setVelocityRadPerSecR(double rightVelocityRadPerSec){} - - + default public void rotateToTag(double yaw) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index e8a7ead..c2fadb5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -2,11 +2,12 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.PIDController; +import java.lang.Math; public class DriveIOSparkMax implements DriveIO{ private final SparkMax leftLeader; @@ -17,6 +18,7 @@ public class DriveIOSparkMax implements DriveIO{ private final RelativeEncoder leftLeaderEncoder; private final RelativeEncoder rightLeaderEncoder; + private final PIDController pidController; public DriveIOSparkMax(){ leftLeader = new SparkMax(3, SparkLowLevel.MotorType.kBrushless); @@ -51,6 +53,9 @@ public DriveIOSparkMax(){ .voltageCompensation(12) .smartCurrentLimit(60) .follow(5, false); + + pidController = new PIDController(DriveConstants.PID_P, DriveConstants.PID_I, DriveConstants.PID_D); + leftLeader.configure(LeftLeaderConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); leftFollower.configure(LeftFollowerConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); rightLeader.configure(RightLeaderConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); @@ -88,4 +93,12 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ rightLeader.set(rightVelocityRadPerSec); } + @Override + public void rotateToTag(double yaw) { + var newVelocity = pidController.calculate(yaw, 0); + newVelocity = Math.max(-1, newVelocity); + newVelocity = Math.min(1, newVelocity); + leftLeader.set(newVelocity); + rightLeader.set(-newVelocity); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 70c2267..e250efb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -5,6 +5,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 75d853d..06c50df 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger;