diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index b419cac..6a2e54d 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -5,6 +5,7 @@ import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -24,7 +25,7 @@ public class AutoAim extends Command { private DoubleSupplier _ySpeed; private PIDController _headingController = new PIDController( - Constants.PID.PP_ROTATION.kP, + 0.05, 0, 0 ); @@ -46,6 +47,7 @@ public AutoAim( _ySpeed = ySpeed; _headingController.setTolerance(2); + _headingController.enableContinuousInput(-180, 180); addRequirements(_shooter, _vision, _swerve); } @@ -66,29 +68,39 @@ public void execute() { double[] visionAngles = _vision.anglesToSpeaker(); - if (_vision.isApriltagVisible() && visionAngles != null) { - desiredShooterAngle = visionAngles[1]; - desiredSwerveHeading = visionAngles[0]; + // if (_vision.isApriltagVisible() && visionAngles != null) { + // desiredShooterAngle = visionAngles[1]; + // desiredSwerveHeading = visionAngles[0]; - SmartDashboard.putNumberArray("VISION ANGLES", visionAngles); + // SmartDashboard.putNumberArray("VISION ANGLES", visionAngles); - } else { - desiredShooterAngle = _swerve.anglesToSpeaker()[1]; - desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; - } + // } else { + // desiredShooterAngle = _swerve.anglesToSpeaker()[1]; + // desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; + // } - desiredSwerveHeading -= currentSwerveHeading; + // desiredSwerveHeading -= currentSwerveHeading; - _shooter.setAngle(desiredShooterAngle); + // SmartDashboard.putNumber("DESIRED HEADING 360", desiredSwerveHeading); - SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading); + // desiredSwerveHeading = MathUtil.angleModulus(Math.toRadians(desiredSwerveHeading)); + // desiredSwerveHeading = Math.toDegrees(desiredSwerveHeading); + + // _shooter.setAngle(desiredShooterAngle); + + // SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading); + + desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; _swerve.driveChassis( new ChassisSpeeds( _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - // _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), - 0 + MathUtil.clamp( + _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), + -Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED, + Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED + ) ) ); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 2c6809e..d02e235 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; 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.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -226,6 +227,9 @@ public void initSendable(SendableBuilder builder) { public void periodic() { publisher.set(states); + SmartDashboard.putNumber("Gyro 180/-180", getHeading().getDegrees()); + anglesToSpeaker(); + // This method will be called once per scheduler run // SmartDashboard.putNumber("Gyro", getHeading().getDegrees()); SmartDashboard.putBoolean("Field Oriented", fieldOriented); @@ -358,7 +362,7 @@ public void resetGyro() { resetPose(new_pose); } - + /** Resets pose estimator's translation of the drive to (0, 0). */ public void resetTranslation() { Pose2d new_pose = new Pose2d(0, 0, _pose.getRotation()); @@ -391,31 +395,46 @@ public Rotation2d getHeadingRaw() { /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */ public double[] anglesToSpeaker() { - int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; - Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); + // int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; + // Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); - double xDifference = tagPose.getX() - _pose.getX(); - double yDifference = tagPose.getY() - _pose.getY(); + // Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY()); + // Translation2d botTranslation = _pose.getTranslation(); - double distanceToRobot = Math.sqrt(Math.pow(xDifference, 2) + Math.pow(yDifference, 2)); + // Rotation2d moveAngle = tagTranslation.minus(botTranslation).getAngle(); - double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; + // double[] a = {1, 2}; - double angleY = Math.atan(zDifference / distanceToRobot); + // SmartDashboard.putNumber("MOVE ANGLE", moveAngle.getDegrees()); + double[] a = {1, 2}; - Rotation2d currentRotation = getHeading(); - double angleX = 0; + return a; - if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){ - angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); - } - else{ - angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); - } + // double xDifference = tagPose.getX() - _pose.getX(); + // double yDifference = tagPose.getY() - _pose.getY(); + + // Constants.FIELD_CONSTANTS.SPEAKER_POSE.minus(new Pose2d()); + + // double distanceToRobot = Math.sqrt(Math.pow(xDifference, 2) + Math.pow(yDifference, 2)); + + // double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; + + // double angleY = Math.atan(zDifference / distanceToRobot); + + + // Rotation2d currentRotation = getHeading(); + // double angleX = 0; + + // if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){ + // angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + // } + // else{ + // angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + // } - double[] angles = {angleX, angleY}; + // double[] angles = {angleX, angleY}; - return angles; + // return angles; } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index c004377..782776c 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -100,7 +100,7 @@ public boolean isApriltagVisible() { public boolean isApriltagVisible(int ID) { if (!isApriltagVisible()) return false; - return (_limelight.getTag(ID) != null); + return _limelight.getTag(ID) != null; } /** diff --git a/src/main/java/frc/robot/utils/LimelightHelper.java b/src/main/java/frc/robot/utils/LimelightHelper.java index 1af35ca..4ff0fb8 100644 --- a/src/main/java/frc/robot/utils/LimelightHelper.java +++ b/src/main/java/frc/robot/utils/LimelightHelper.java @@ -76,6 +76,4 @@ public JsonNode getTag(int ID) { return null; } - - }