diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5787481..c7cbf07 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.ManipulatorConstants; import frc.robot.constants.OIConstants; +import frc.robot.constants.VisionConstants; import frc.robot.subsystems.ManipulatorPivot; import frc.robot.subsystems.Vision; import frc.robot.subsystems.ClimberPivot; @@ -16,11 +17,12 @@ import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Manipulator; + +import java.util.function.IntSupplier; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -47,8 +49,11 @@ public class RobotContainer { private CommandXboxController operator; private SendableChooser autoChooser; + private Vision vision; + private IntSupplier closestTag; + public RobotContainer() { climberPivot = new ClimberPivot(); @@ -70,6 +75,8 @@ public class RobotContainer { autoChooser = AutoBuilder.buildAutoChooser(); + closestTag = drivetrain::getClosestTag; + configureButtonBindings(); //elevatorSysIDBindings(); //elevatorOnlyBindings(); @@ -157,6 +164,24 @@ public class RobotContainer { driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false)); + /* + driver.rightBumper().whileTrue( + drivetrain.goToPose( + () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], + () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], + () -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3] + ) + ); + + driver.leftBumper().whileTrue( + drivetrain.goToPose( + () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], + () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], + () -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3] + ) + ); + */ + //Operator inputs operator.povUp().onTrue( safeMoveManipulator( diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index fb5fe68..9d47b89 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -57,6 +57,12 @@ public class DrivetrainConstants { public static final boolean kGyroReversed = true; + public static final double kHeadingP = 0.0; + + public static final double kXTranslationP = 0.0; + public static final double kYTranslationP = 0.0; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM // Distance between front and back wheels on robot diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 9774d91..18c5c84 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -1,7 +1,5 @@ package frc.robot.constants; -import java.util.HashMap; - public class VisionConstants { // global coordinate map of all tags. index is the tag id. @@ -35,22 +33,30 @@ public class VisionConstants { //map of coral placing setpoints based on the tag that is on the same reef face // and the on the left or right branch of that side of the reef // - public HashMap blueReefMap = new HashMap<>(); - public HashMap redReefMap = new HashMap<>(); + public static final double[][] reefSetpointsMap = { + {}, + {}, + {}, + {}, + {}, + {}, + {4.993+12.272, 2.816, 5.272+12.272, 2.996},//6 + {5.789+12.272, 3.862, 5.789+12.272, 4.194}, + {5.275+12.272, 5.075, 4.991+12.272, 5.246}, + {3.986+12.272, 5.24, 3.701+12.272, 5.076}, + {3.183+12.272, 4.191, 3.183, 3.857}, + {3.703+12.272, 3.975, 3.982+12.272, 2.806},//11 + {}, + {}, + {}, + {}, + {}, + {3.703, 3.975, 3.982, 2.806}, + {3.183, 4.191, 3.183, 3.857}, + {3.986, 5.24, 3.701, 5.076}, + {5.275, 5.075, 4.991, 5.246}, + {5.789, 3.862, 5.789, 4.194}, + {4.993, 2.816, 5.272, 2.996} + }; - VisionConstants(){ - blueReefMap.put(17, new double[] {3.703, 3.975, 3.982, 2.806}); - blueReefMap.put(18, new double[] {3.183, 4.191, 3.183, 3.857}); - blueReefMap.put(19, new double[] {3.986, 5.24, 3.701, 5.076}); - blueReefMap.put(20, new double[] {5.275, 5.075, 4.991, 5.246}); - blueReefMap.put(21, new double[] {5.789, 3.862, 5.789, 4.194}); - blueReefMap.put(22, new double[] {4.993, 2.816, 5.272, 2.996}); - - blueReefMap.put(11, new double[] {3.703+12.272, 3.975, 3.982+12.272, 2.806}); - blueReefMap.put(10, new double[] {3.183+12.272, 4.191, 3.183, 3.857}); - blueReefMap.put(9, new double[] {3.986+12.272, 5.24, 3.701+12.272, 5.076}); - blueReefMap.put(8, new double[] {5.275+12.272, 5.075, 4.991+12.272, 5.246}); - blueReefMap.put(7, new double[] {5.789+12.272, 3.862, 5.789+12.272, 4.194}); - blueReefMap.put(6, new double[] {4.993+12.272, 2.816, 5.272+12.272, 2.996}); - } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f598ceb..52790dd 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -15,6 +15,7 @@ import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -31,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; +import frc.robot.constants.VisionConstants; public class Drivetrain extends SubsystemBase { // Create MAXSwerveModules @@ -50,6 +52,10 @@ public class Drivetrain extends SubsystemBase { public Orchestra m_orchestra = new Orchestra(); private Timer musicTimer = new Timer(); + private PIDController pidHeading; + private PIDController pidTranslationX; + private PIDController pidTranslationY; + /** Creates a new DriveSubsystem. */ public Drivetrain() { m_frontLeft = new MAXSwerveModule( @@ -90,6 +96,12 @@ public class Drivetrain extends SubsystemBase { new Pose2d() ); + pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0); + pidHeading.enableContinuousInput(-180, 180); + + pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0); + pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0); + AutoBuilder.configure( this::getPose, this::resetOdometry, @@ -152,7 +164,7 @@ public class Drivetrain extends SubsystemBase { } */ - if(musicTimer.get()>8){ + if(musicTimer.get()>4){ if (m_orchestra.isPlaying()) { m_orchestra.stop(); } @@ -245,6 +257,50 @@ public class Drivetrain extends SubsystemBase { }); } + public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){ + return run(() -> { + drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), + pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), + pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), + true); + }); + } + + public int getClosestTag(){ + + if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){ + int closestTag = 17; + double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2) + + Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2)); + + for(int i = 17; i <= 22; ++i){ + double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2) + + Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2)); + + if(distance < closestTagDist){ + closestTag = i; + closestTagDist = distance; + } + } + return closestTag; + }else{ + int closestTag = 6; + double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2) + + Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2)); + + for(int i = 6; i <= 11; ++i){ + double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2) + + Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2)); + + if(distance < closestTagDist){ + closestTag = i; + closestTagDist = distance; + } + } + return closestTag; + } + } + /** * Sets the wheels into an X formation to prevent movement. */