From 93f6cd45f8a3d7b082ac94c53019fa17998164aa Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Fri, 9 Feb 2024 19:42:51 -0600 Subject: [PATCH] added controls for limelights and made sure the speaker aiming code worked --- src/main/java/frc/robot/RobotContainer.java | 1 + .../frc/robot/commands/AimRobotMoving.java | 11 +++- src/main/java/frc/robot/commands/Drive.java | 4 +- .../java/frc/robot/settings/Constants.java | 2 +- .../robot/subsystems/DrivetrainSubsystem.java | 66 +++++++++++++------ 5 files changed, 59 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aae82958..f97294d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -251,6 +251,7 @@ private void configureBindings() { new Trigger(operatorController::getSquareButton).onTrue(new ClimberPullDown(climber)); } SmartDashboard.putData("set offsets", new InstantCommand(driveTrain::setEncoderOffsets)); + SmartDashboard.putData(new InstantCommand(driveTrain::forceUpdateOdometryWithVision)); // //Intake bindings // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); diff --git a/src/main/java/frc/robot/commands/AimRobotMoving.java b/src/main/java/frc/robot/commands/AimRobotMoving.java index 4c2208c1..13877fdb 100644 --- a/src/main/java/frc/robot/commands/AimRobotMoving.java +++ b/src/main/java/frc/robot/commands/AimRobotMoving.java @@ -2,7 +2,10 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.DriveConstants; @@ -31,6 +34,7 @@ public class AimRobotMoving extends Command { BooleanSupplier run; DoubleSupplier rotationSupplier; double rotationSpeed; + double allianceOffset; public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run){ m_drivetrain = drivetrain; @@ -66,11 +70,16 @@ public void execute() { } else { rotationSpeed = speedController.calculate(currentHeading); } + if(DriverStation.getAlliance().get() == Alliance.Red) { + allianceOffset = Math.PI; + } else { + allianceOffset = 0; + } m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds( translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, rotationSpeed, - m_drivetrain.getGyroscopeRotation())); + new Rotation2d(m_drivetrain.getGyroscopeRotation().getRadians()+allianceOffset))); // m_drivetrain.drive(new ChassisSpeeds( // translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, // translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 97b598ef..2cd3ab9c 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -42,14 +42,14 @@ public void execute() { drivetrain.drive(new ChassisSpeeds( translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert, translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert, - rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert + rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND )); } else { drivetrain.drive( ChassisSpeeds.fromFieldRelativeSpeeds( translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert, translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert, - rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert, + rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND, drivetrain.getGyroscopeRotation() ) ); diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 6b2cfdbf..dadf2bbe 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -406,7 +406,7 @@ public final class Field{ public final class Vision{ public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-april2"; - public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-april3"; + public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-april"; public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural"; diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index cf8546a8..a404ef28 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -125,6 +125,10 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) { PathPlannerLogging.setLogActivePathCallback((poses) -> m_field.getObject("path").setPoses(poses)); SmartDashboard.putData("Field", m_field); SmartDashboard.putData("resetOdometry", new InstantCommand(() -> this.resetOdometry())); + SmartDashboard.putBoolean("use limelight", false); + SmartDashboard.putBoolean("trust limelight", false); + SmartDashboard.putBoolean("force use limelight", false); + SmartDashboard.putBoolean("use 2 limelights", false); modules = new SwerveModule[4]; lastAngles = new Rotation2d[] {new Rotation2d(), new Rotation2d(), new Rotation2d(), new Rotation2d()}; // manually make empty angles to avoid null errors. @@ -168,7 +172,11 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) { * 'forwards' direction. */ public void zeroGyroscope() { + if(DriverStation.getAlliance().get() == Alliance.Red) { + pigeon.setYaw(180); + } else { pigeon.setYaw(0); //TODO make sure this is right for both alliances + } odometer.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d(getPose().getTranslation(), new Rotation2d())); } public void zeroGyroscope(double angleDeg) { @@ -331,11 +339,12 @@ public double calculateSpeakerAngleMoving(){ deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y); speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2)); SmartDashboard.putNumber("dist to speakre", speakerDist); - + + Rotation2d unadjustedAngle = Rotation2d.fromDegrees(Math.asin(deltaX/speakerDist)); shootingTime = speakerDist/shootingSpeed; //calculates how long the note will take to reach the target currentXSpeed = this.getChassisSpeeds().vxMetersPerSecond; currentYSpeed = this.getChassisSpeeds().vyMetersPerSecond; - targetOffset = new Translation2d(currentXSpeed*shootingTime*OFFSET_MULTIPLIER, currentYSpeed*shootingTime*OFFSET_MULTIPLIER); + targetOffset = new Translation2d(currentXSpeed*shootingTime*OFFSET_MULTIPLIER*unadjustedAngle.getRadians(), currentYSpeed*shootingTime*OFFSET_MULTIPLIER); //line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime //next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed @@ -402,31 +411,46 @@ public Pose2d getAverageBotPose(LimelightValues ll2, LimelightValues ll3) { return new Pose2d(new Translation2d(averageX, averageY), averageRotation); } public void forceUpdateOdometryWithVision() { - LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME); - LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME); - if (ll2.isResultValid && !ll3.isResultValid) {updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());} - if (!ll2.isResultValid && ll3.isResultValid) {updateOdometryWithVision(ll3.getBotPoseBlue(), ll3.gettimestamp());} - if (ll2.isResultValid && ll3.isResultValid) {updateOdometryWithVision(getAverageBotPose(ll2, ll3), ll3.gettimestamp());} + if(SmartDashboard.getBoolean("use limelight", false) && SmartDashboard.getBoolean("use 2 limelights", false)) { + LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME); + LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME); + if (ll2.isResultValid && !ll3.isResultValid) {updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());} + if (!ll2.isResultValid && ll3.isResultValid) {updateOdometryWithVision(ll3.getBotPoseBlue(), ll3.gettimestamp());} + if (ll2.isResultValid && ll3.isResultValid) {updateOdometryWithVision(getAverageBotPose(ll2, ll3), ll3.gettimestamp());} + } } @Override public void periodic() { SmartDashboard.putString("alliance:", DriverStation.getAlliance().get().toString()); updateOdometry(); AngleShooterSubsystem.setDTPose(getPose()); - AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds()); - if (SmartDashboard.getBoolean("use limelight", false)) { - LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME); - LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME); - Boolean isLL2VisionValid = ll2.isResultValid; - Boolean isLL3VisionValid = ll3.isResultValid; - Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition()); - Boolean isLL3VisionTrustworthy = isLL3VisionValid && ll3.isPoseTrustworthy(odometer.getEstimatedPosition()); - SmartDashboard.putBoolean("LL2visionValid", isLL2VisionTrustworthy); - SmartDashboard.putBoolean("LL3visionValid", isLL3VisionTrustworthy); - if (SmartDashboard.getBoolean("trust limelight", false)) { - if (isLL2VisionTrustworthy && !isLL3VisionTrustworthy) {updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());} - if (!isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(ll3.getBotPoseBlue(), ll3.gettimestamp());} - if (isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(getAverageBotPose(ll2, ll3), ll3.gettimestamp());} + AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds()); + if(SmartDashboard.getBoolean("force use limelight", false)) { + forceUpdateOdometryWithVision(); + } else { + if (SmartDashboard.getBoolean("use limelight", false)) { + if(SmartDashboard.getBoolean("use 2 limelights", false)) { + LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME); + LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME); + Boolean isLL2VisionValid = ll2.isResultValid; + Boolean isLL3VisionValid = ll3.isResultValid; + Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition()); + Boolean isLL3VisionTrustworthy = isLL3VisionValid && ll3.isPoseTrustworthy(odometer.getEstimatedPosition()); + SmartDashboard.putBoolean("LL2visionValid", isLL2VisionTrustworthy); + SmartDashboard.putBoolean("LL3visionValid", isLL3VisionTrustworthy); + if (SmartDashboard.getBoolean("trust limelight", false)) { + if (isLL2VisionTrustworthy && !isLL3VisionTrustworthy) {updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp());} + if (!isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(ll3.getBotPoseBlue(), ll3.gettimestamp());} + if (isLL2VisionTrustworthy && isLL3VisionTrustworthy) {updateOdometryWithVision(getAverageBotPose(ll2, ll3), ll3.gettimestamp());} + } + } else { + LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME); + Boolean isLL2VisionValid = ll2.isResultValid; + Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition()); + if (isLL2VisionTrustworthy) { + updateOdometryWithVision(ll2.getBotPoseBlue(), ll2.gettimestamp()); + } + } } } m_field.setRobotPose(odometer.getEstimatedPosition());