diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9fa45e3..af39c6a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,11 +7,14 @@ package frc4388.robot; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.RobotUnits; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -168,5 +171,8 @@ public final class Constants { // public static final double MID_TARGET_HEIGHT = 34.0; public static final double MID_TAPE_HEIGHT = 24.0; + + public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index fbdf90c..5978788 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,20 +7,6 @@ package frc4388.robot; -import java.lang.System; -import java.lang.reflect.Array; -import java.util.Arrays; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.can.MotControllerJNI; - -import java.io.File; -import java.io.IOException; -import java.io.PrintWriter; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -28,10 +14,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; -import frc4388.robot.subsystems.Location; -import frc4388.robot.subsystems.Apriltags.Tag; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f724ec0..cd728ce 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,26 +7,34 @@ package frc4388.robot; +import java.util.function.Consumer; + +import edu.wpi.first.math.geometry.Translation2d; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.CANSparkMax.FaultID; - import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.AutoBalance; -import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.commands.JoystickRecorder; -import frc4388.robot.commands.LimeAlign; -import frc4388.robot.commands.PlaybackChooser; +import frc4388.robot.commands.BooleanCommand; +import frc4388.robot.commands.Arm.PivotCommand; +import frc4388.robot.commands.Arm.TeleCommand; +import frc4388.robot.commands.Autos.AutoBalance; +import frc4388.robot.commands.Autos.PlaybackChooser; +import frc4388.robot.commands.Placement.AprilRotAlign; +import frc4388.robot.commands.Placement.LimeAlign; +import frc4388.robot.commands.Swerve.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -52,7 +60,7 @@ public class RobotContainer { public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin); - public final Limelight m_limeLight = new Limelight(); + public final Limelight m_robotLimeLight = new Limelight(); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -60,9 +68,7 @@ public class RobotContainer { /* Autos */ public SendableChooser chooser = new SendableChooser<>(); - - private Command noAuto = new InstantCommand(); - + // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); @@ -74,6 +80,92 @@ public class RobotContainer { // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private PlaybackChooser playbackChooser; + + /* Commands */ + private Command emptyCommand = new InstantCommand(); + private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight); + + private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135)); + + private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); + + private boolean readyForPlacement = false; + private Boolean isPole = null; + + private SequentialCommandGroup alignToPole = + new SequentialCommandGroup( + new RotateToAngle(m_robotSwerveDrive, 0.0), + new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04), + new RotateToAngle(m_robotSwerveDrive, 0.0), + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); + + // private SequentialCommandGroup alignToShelf = + // new SequentialCommandGroup( + // new RotateToAngle(m_robotSwerveDrive, 0.0), + // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), + // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), + // new RotateToAngle(m_robotSwerveDrive, 0.0), + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); + + private SequentialCommandGroup alignToShelf = + new SequentialCommandGroup( + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), + new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); + + public SequentialCommandGroup place = null; + + private Consumer queuePlacement = (scg) -> { + place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null)); + }; + + // TODO: find actual values + private SequentialCommandGroup placeCubeHigh = + new SequentialCommandGroup( + // new InstantCommand(() -> System.out.println("Placing cone high")), + new PivotCommand(m_robotArm, 64 + 135), + new InstantCommand(() -> m_robotArm.setRotVel(0)), + new TeleCommand(m_robotArm, 95642), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 70 + 135), + new TeleCommand(m_robotArm, 32866), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeLow = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -95,21 +187,32 @@ public class RobotContainer { .withName("Arm DefaultCommand")); // * Auto Commands - // chooser.setDefaultOption("NoAuto", noAuto); + chooser.setDefaultOption("NoAuto", emptyCommand); + chooser.addOption("alignToPole", alignToPole); + chooser.addOption("alignToShelf", alignToShelf); + + chooser.addOption("placeConeHigh", placeConeHigh); + chooser.addOption("placeConeMid", placeConeMid); + chooser.addOption("placeCubeHigh", placeCubeHigh); + chooser.addOption("placeCubeMid", placeCubeMid); + chooser.addOption("placeLow", placeLow); // chooser.addOption("Blue1Path", blue1Path); // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); // chooser.addOption("Red1Path", red1Path); // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); - - // chooser.addOption("Taxi", taxi); playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) .buildDisplay(); } + // here be dragons - enter if you dare + private static class TapState { + public boolean gearTapped = true; + public long gearTime = 0; + } /** * Use this method to define your button->command mappings. Buttons can be @@ -120,23 +223,38 @@ public class RobotContainer { private void configureButtonBindings() { // * Driver Buttons new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) + // because closure reasons - ask Daniel Thomas + // final TapState tap = new TapState(); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())); + // .onTrue(new InstantCommand(() -> { + // tap.gearTapped = true; + // tap.gearTime = System.currentTimeMillis(); + // })) + // .whileTrue(new RunCommand(() -> { + // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) { + // m_robotSwerveDrive.setToTurbo(); + // tap.gearTapped = false; + // } + // })) + // .onFalse(new InstantCommand(() -> { + // if (tap.gearTapped) + // m_robotSwerveDrive.setToFast(); + // else + // m_robotSwerveDrive.setToSlow(); + // })); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); - // // .onFalse() - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(interruptCommand.asProxy()); // final // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, // () -> getDeadbandedDriverController().getLeftX(), @@ -148,25 +266,26 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // .onFalse(new InstantCommand()); // * Operator Buttons - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); - // .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + + // align (pole) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final + .onTrue(alignToPole) + .onFalse(interruptCommand.asProxy()); + + // align (shelf) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final + .onTrue(alignToShelf) + .onFalse(interruptCommand.asProxy()); + // toggle claw + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final + .onTrue(toggleClaw.asProxy()); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight)); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight)); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + // kill soft limits + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); // SmartDashboard.putBoolean("isn't SparkMAX", ); @@ -175,11 +294,39 @@ public class RobotContainer { .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin())) .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin())); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); + //Arm to Home + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(armToHome.asProxy()); + + //Interupt Button + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(interruptCommand.asProxy()); + + + // place high + new POVButton(getDeadbandedOperatorController(), 0) + .onTrue(new ConditionalCommand( + new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), + emptyCommand.asProxy(), + () -> readyForPlacement == true) + ); + + // place mid + new POVButton(getDeadbandedOperatorController(), 270) + .onTrue(new ConditionalCommand( + new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), + emptyCommand.asProxy(), + () -> readyForPlacement == true) + ); + + // place low + new POVButton(getDeadbandedOperatorController(), 180) + .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true)); + + // confirm + new POVButton(getDeadbandedOperatorController(), 90) + .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null)); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); } /** diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java similarity index 90% rename from src/main/java/frc4388/robot/commands/PivotCommand.java rename to src/main/java/frc4388/robot/commands/Arm/PivotCommand.java index a84690d..279e1b0 100644 --- a/src/main/java/frc4388/robot/commands/PivotCommand.java +++ b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java @@ -2,9 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.Arm; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.Arm; public class PivotCommand extends PelvicInflammatoryDisease { diff --git a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java new file mode 100644 index 0000000..a5a6e9f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Arm; + +public class TeleCommand extends CommandBase { + private final Arm arm; + private final double target; + private boolean goIn; + + /** Creates a new ArmCommand. */ + public TeleCommand(Arm arm, double target) { + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public void initialize() { + this.goIn = target < arm.getArmLength(); + } + + @Override + public void execute() { + arm.setTeleVel(goIn ? 1 : -1); + } + + @Override + public boolean isFinished() { + if (goIn) return arm.getArmLength() < target; + else return arm.getArmLength() > target; + } +} diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java similarity index 91% rename from src/main/java/frc4388/robot/commands/AutoBalance.java rename to src/main/java/frc4388/robot/commands/Autos/AutoBalance.java index 629c903..4d18c99 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java @@ -2,11 +2,12 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.Autos; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; diff --git a/src/main/java/frc4388/robot/commands/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt similarity index 100% rename from src/main/java/frc4388/robot/commands/Blue1Path.txt rename to src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java similarity index 97% rename from src/main/java/frc4388/robot/commands/PlaybackChooser.java rename to src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index c2b32e4..2438a38 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands; +package frc4388.robot.commands.Autos; import java.io.File; import java.util.ArrayList; @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.subsystems.SwerveDrive; public class PlaybackChooser { diff --git a/src/main/java/frc4388/robot/commands/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt similarity index 100% rename from src/main/java/frc4388/robot/commands/Taxi.txt rename to src/main/java/frc4388/robot/commands/Autos/Taxi.txt diff --git a/src/main/java/frc4388/robot/commands/BooleanCommand.java b/src/main/java/frc4388/robot/commands/BooleanCommand.java new file mode 100644 index 0000000..91e49b3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/BooleanCommand.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class BooleanCommand extends CommandBase { + + private Supplier onTrue; + private Supplier onFalse; + + private Supplier condition; + + private Supplier selected; + + + /** Creates a new BooleanCommand. */ + public BooleanCommand(Supplier onTrue, Supplier onFalse, Supplier condition) { + this.onTrue = onTrue; + this.onFalse = onFalse; + this.condition = condition; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (condition.get()) { + selected = onTrue; + } else { + selected = onFalse; + } + if (selected.get() != null) { + selected.get().initialize(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (selected.get() != null) { + selected.get().execute(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (selected.get() != null) { + selected.get().end(interrupted); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (selected.get() != null) { + return selected.get().isFinished(); + } else { + return true; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java new file mode 100644 index 0000000..212d979 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Placement; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class AprilRotAlign extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + /** Creates a new AprilRotAlign. */ + public AprilRotAlign(SwerveDrive drive, Limelight lime) { + super(0.1, 0.2, 0.0, 0.0, 0.0); + + this.drive = drive; + this.lime = lime; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + double err = 0.0; + + try { + err = lime.getAprilSkew(); + } catch (NullPointerException ex) {} + + return err; + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java new file mode 100644 index 0000000..e79cffc --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Placement; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveToLimeDistance extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + double targetDistance; + DoubleSupplier ds; + + /** Creates a new DriveToLimeDistance. */ + public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) { + super(0.5, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + this.lime = lime; + this.targetDistance = targetDistance; + this.ds = ds; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + return targetDistance - ds.getAsDouble(); + } + + @Override + public void runWithOutput(double output) { + System.out.println(output / Math.abs(getError())); + drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java similarity index 67% rename from src/main/java/frc4388/robot/commands/LimeAlign.java rename to src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index 533b6f0..5b2e87f 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -2,12 +2,13 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.Placement; -import org.photonvision.targeting.PhotonTrackedTarget; +import java.util.function.DoubleSupplier; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; @@ -16,11 +17,14 @@ public class LimeAlign extends PelvicInflammatoryDisease { SwerveDrive drive; Limelight lime; - public LimeAlign(SwerveDrive drive, Limelight lime) { - super(0.7, 0.1, 0.0, 0.0, 0); + DoubleSupplier ds; + + public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) { + super(0.4, 0.4, 0.0, 0.0, tolerance); this.drive = drive; this.lime = lime; + this.ds = ds; addRequirements(drive, lime); } @@ -30,7 +34,8 @@ public class LimeAlign extends PelvicInflammatoryDisease { double err = 0.0; try { - err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); + System.out.println(ds.getAsDouble()); + err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); } catch (NullPointerException ex) {} return err; @@ -38,13 +43,6 @@ public class LimeAlign extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { - - if (output > 0) { - output += 0.6; - } else if (output < 0) { - output -= 0.6; - } - drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); } } diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java similarity index 99% rename from src/main/java/frc4388/robot/commands/JoystickPlayback.java rename to src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 0f496a9..56e8d43 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.Swerve; import java.io.File; import java.io.FileNotFoundException; diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java similarity index 98% rename from src/main/java/frc4388/robot/commands/JoystickRecorder.java rename to src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index eea72b4..84608cc 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.Swerve; import java.io.File; import java.io.IOException; diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java new file mode 100644 index 0000000..8cf630f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Swerve; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.SwerveDrive; + +public class RotateToAngle extends PelvicInflammatoryDisease { + + SwerveDrive drive; + double targetAngle; + + /** Creates a new RotateToAngle. */ + public RotateToAngle(SwerveDrive drive, double targetAngle) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + this.targetAngle = targetAngle; + + addRequirements(drive); + } + + @Override + public double getError() { + return targetAngle - drive.getGyroAngle(); + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java deleted file mode 100644 index 02c8c27..0000000 --- a/src/main/java/frc4388/robot/commands/TeleCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands; - -import frc4388.robot.Constants.ArmConstants; -import frc4388.robot.subsystems.Arm; - -public class TeleCommand extends PelvicInflammatoryDisease { - private final Arm arm; - private final double target; - - /** Creates a new ArmCommand. */ - public TeleCommand(Arm arm, double target) { - super(0.6, 0, 0, 0, 0); - this.arm = arm; - this.target = target; - addRequirements(arm); - } - - @Override - public double getError() { - return (arm.getArmLength() - target) / - (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); - } - - @Override - public void runWithOutput(double output) { - arm.setTeleVel(output); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 06fab10..94081df 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,28 +1,20 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import frc4388.robot.Constants.ArmConstants; -import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.DeferredBlock; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; public WPI_TalonFX m_pivot; private CANCoder m_pivotEncoder; - private boolean m_debug; // Moves arm to distance [dist] then returns new ang public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) { @@ -48,6 +40,8 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { + if (vel > 1) vel = 1; + var degrees = Math.abs(getArmRotation()) - 135; SmartDashboard.putNumber("arm degrees", degrees); SmartDashboard.putNumber("arm rot vel", vel); @@ -56,6 +50,8 @@ public class Arm extends SubsystemBase { m_pivot.set(ControlMode.PercentOutput, 0); } else if (degrees > 90 && vel > 0) { m_pivot.set(ControlMode.PercentOutput, .15 * vel); + } else if (degrees < 25 && vel < 0) { + m_pivot.set(ControlMode.PercentOutput, .15 * vel); } else { m_pivot.set(ControlMode.PercentOutput, .3 * vel); } @@ -86,7 +82,7 @@ public class Arm extends SubsystemBase { } public double getArmLength() { - return m_tele.getSelectedSensorPosition(); + return m_tele.getSelectedSensorPosition() - tele_soft; } public double getArmRotation() { @@ -120,9 +116,10 @@ public class Arm extends SubsystemBase { } boolean tele_softLimit = false; + double tele_soft = 0; public void resetTeleSoftLimit() { if (!tele_softLimit) { - var tele_soft = m_tele.getSelectedSensorPosition(); + tele_soft = m_tele.getSelectedSensorPosition(); m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); m_tele.configReverseSoftLimitThreshold(tele_soft); m_tele.configForwardSoftLimitEnable(true); @@ -140,7 +137,6 @@ public class Arm extends SubsystemBase { @Override public void periodic() { - double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135); if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { var tele_soft = m_tele.getSelectedSensorPosition(); @@ -154,17 +150,10 @@ public class Arm extends SubsystemBase { } // double x = Math.cos(Math.toRadians(degrees)); + SmartDashboard.putNumber("arm length", getArmLength()); } - boolean soft_limits = true; public void killSoftLimits() { resetTeleSoftLimit(); - var pivot_soft = m_pivot.getSelectedSensorPosition(); - var tele_soft = m_tele.getSelectedSensorPosition(); - - m_pivot.configForwardSoftLimitEnable(!soft_limits); - m_pivot.configReverseSoftLimitEnable(!soft_limits); - - soft_limits = !soft_limits; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index d7f3b44..985f70a 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,4 +1,5 @@ package frc4388.robot.subsystems; + import java.util.Timer; import java.util.TimerTask; @@ -8,6 +9,7 @@ import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Claw extends SubsystemBase { + private final PWM m_leftMotor; private final PWM m_rightMotor; private final CANSparkMax m_spinnyspin; diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index c5f3c50..9d1289b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -4,23 +4,33 @@ package frc4388.robot.subsystems; +import java.io.IOException; import java.util.ArrayList; import java.util.List; +import java.util.Optional; -import org.opencv.core.Point; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.AbhiIsADumbass; public class Limelight extends SubsystemBase { + private PhotonCamera cam; + private PhotonPoseEstimator photonPoseEstimator; private boolean lightOn; @@ -28,8 +38,6 @@ public class Limelight extends SubsystemBase { public Limelight() { cam = new PhotonCamera(VisionConstants.NAME); cam.setDriverMode(false); - - setToLimePipeline(); } public void setLEDs(boolean on) { @@ -48,44 +56,15 @@ public class Limelight extends SubsystemBase { public void setToLimePipeline() { cam.setPipelineIndex(1); + setLEDs(true); } public void setToAprilPipeline() { cam.setPipelineIndex(0); + setLEDs(false); } - public ArrayList getTargetPoints() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = new ArrayList<>(2); - - for(PhotonTrackedTarget target : result.getTargets()) { - List corners = target.getDetectedCorners(); - - double sumX = 0.0; - double sumY = 0.0; - double mx = 0.0; - double my = 0.0; - - for (TargetCorner c : corners) { - sumX += c.x; - sumY += c.y; - } - - mx = sumX / 4.0; - my = sumY / 4.0; - - points.add(new Point(mx, my)); - } - - return points; - } - - public PhotonTrackedTarget getFirstTargetPoint() { + public PhotonTrackedTarget getAprilPoint() { if (!cam.isConnected()) return null; PhotonPipelineResult result = cam.getLatestResult(); @@ -95,40 +74,92 @@ public class Limelight extends SubsystemBase { return result.getBestTarget(); } - private double getPointAngle(Point point) { - return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS); + private List getAprilCorners() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget().getDetectedCorners(); } - public double getHorizontalDistanceToTarget(boolean high) { - ArrayList targetPoints = getTargetPoints(); - if (targetPoints == null) return -1; + public double getAprilSkew() { + List corners = getAprilCorners(); + ArrayList bottomSide = getAprilBottomSide(corners); - // Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); - // Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); + if (bottomSide == null) return 0; - PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint; - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; + TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); + TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - double theta = 35.0 + tapePoint.getPitch(); - - double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; - - double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta)); - - return horizontalDistanceToTarget; + return bottomLeft.y - bottomRight.y; } - int ctr = 0; + private ArrayList getAprilBottomSide(List box) { + if (box == null) return null; - @Override - public void periodic() { - // This method will be called once per scheduler run + ArrayList bottomSide = new ArrayList<>(); + + TargetCorner l1 = new TargetCorner(-1, -1); + TargetCorner l2 = new TargetCorner(-1, -1); - if (ctr % 50 == 0) { - SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false)); + for (TargetCorner c : box) { + if (c.y > l1.y) l1 = c; } - ctr++; + for (TargetCorner c : box) { + if (c.y == l1.y) continue; + if (c.y > l2.y) l2 = c; + } + bottomSide.add(l1); + bottomSide.add(l2); + + return bottomSide; } + + public double getDistanceToApril() { + PhotonTrackedTarget aprilPoint = getAprilPoint(); + if (aprilPoint == null) return -1; + + double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + aprilPoint.getPitch(); + + double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); + return distanceToApril; + } + + public PhotonTrackedTarget getLowestTape() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + ArrayList points = (ArrayList) result.getTargets(); + + PhotonTrackedTarget lowest = points.get(0); + for (PhotonTrackedTarget point : points) { + if (point.getPitch() < lowest.getPitch()) { + lowest = point; + } + } + + return lowest; + } + + public double getDistanceToTape() { + PhotonTrackedTarget tapePoint = getLowestTape(); + if (tapePoint == null) return -1; + + double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + tapePoint.getPitch(); + + double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); + return distanceToTape; + } + + @Override + public void periodic() {} } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f65dd00..055230b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,16 +4,12 @@ package frc4388.robot.subsystems; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -77,10 +73,10 @@ public class SwerveDrive extends SubsystemBase { // Use the left joystick to set speed. Apply a cubic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); @@ -101,6 +97,19 @@ public class SwerveDrive extends SubsystemBase { } } + public boolean rotateToTarget(double angle) { + double currentAngle = getGyroAngle(); + double error = angle - currentAngle; + + driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); + + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } + + return false; + } + public double getGyroAngle() { return gyro.getAngle(); } @@ -122,7 +131,8 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { - // This method will be called once per scheduler run + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); } public void shiftDown() { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 3731849..371f621 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -3,7 +3,6 @@ package frc4388.robot.subsystems; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index d339841..4577a2e 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -4,7 +4,6 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; -import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } @@ -21,11 +20,6 @@ public class DeadbandedXboxController extends XboxController { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - // if (OIConstants.SKEW_STICKS && magnitude >= 1) { - // System.out.println("if statement running"); - // return translation2d.div(magnitude); - // } - if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index e263633..8c2fe88 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -1,7 +1,6 @@ package frc4388.utility.controller; import edu.wpi.first.wpilibj2.command.button.Button; -import frc4388.utility.controller.XboxController; /** * Mapping for the Xbox controller triggers to allow triggers to be defined as