diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index e72fa05a..c8d35ab6 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -184,8 +184,7 @@ private void setBindingsDriver() { new POVButton(driverController, 0) .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter))); + new AutoMATICALLYGetNote(drivetrain, limelight))); axisTrigger(driverController, Axis.kLeftTrigger) // .onTrue(new AlignToApriltag(drivetrain, limelight)); @@ -193,7 +192,7 @@ private void setBindingsDriver() { .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true))); axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) - .whileTrue(new AlignToApriltagMegatag2(drivetrain, limelight)); + .whileTrue(new AlignToApriltag(drivetrain, limelight)); new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue( new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java index 331c04f1..4e73513f 100644 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java @@ -9,36 +9,34 @@ import edu.wpi.first.wpilibj2.command.Command; public class AimArmSpeaker extends Command { - private final Arm arm; - private final Limelight ll; - - /** Creates a new AimOuttakeSpeaker. */ - public AimArmSpeaker(Arm arm, Limelight ll) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(this.arm = arm); - this.ll = ll; - } - - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double goal = ll.getOptimizedArmAngleRadsMT2(); - arm.setArmTarget(goal); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { -} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return arm.armAtSetpoint(); - } + private final Arm arm; + private final Limelight ll; + + /** Creates a new AimOuttakeSpeaker. */ + public AimArmSpeaker(Arm arm, Limelight ll) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.arm = arm); + this.ll = ll; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double goal = ll.getOptimizedArmAngleRadsMT2(); + arm.setArmTarget(goal); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return arm.armAtSetpoint(); + } } diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java deleted file mode 100644 index 329c0b6b..00000000 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java +++ /dev/null @@ -1,44 +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 org.carlmontrobotics.commands; - -import org.carlmontrobotics.subsystems.Arm; -import org.carlmontrobotics.subsystems.Limelight; -import edu.wpi.first.wpilibj2.command.Command; - -public class AimArmSpeakerMT2 extends Command { - private final Arm arm; - private final Limelight ll; - - /** Creates a new AimOuttakeSpeaker. */ - public AimArmSpeakerMT2(Arm arm, Limelight ll) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(this.arm = arm); - this.ll = ll; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double goal = ll.getOptimizedArmAngleRadsMT2(); - arm.setArmTarget(goal); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return arm.armAtSetpoint(); - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index e5d8aa51..121119b0 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class AlignToApriltag extends Command { @@ -21,18 +22,25 @@ public class AlignToApriltag extends Command { public final Drivetrain drivetrain; private Limelight limelight; - public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], - thetaPIDController[2]); + public final PIDController rotationPID = new PIDController( + SmartDashboard.getNumber("apriltag align kp", + thetaPIDController[0]), + SmartDashboard.getNumber("apriltag align ki", + thetaPIDController[1]), + SmartDashboard.getNumber("apriltag align kd", + thetaPIDController[2])); public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { - this.limelight = limelight; + this.limelight = limelight; this.drivetrain = drivetrain; this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); rotationPID.enableContinuousInput(-180, 180); Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad())); - rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + .plus(Rotation2d.fromRadians( + limelight.getRotateAngleRadMT2())); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), + -180, 180)); rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); addRequirements(drivetrain); @@ -41,14 +49,21 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { @Override public void execute() { Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad())); - rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + .plus(Rotation2d.fromRadians( + limelight.getRotateAngleRadMT2())); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), + -180, 180)); + double rotationDrive = rotationPID.calculate(drivetrain.getHeading()); + if (!limelight.seesTag()) { + rotationDrive = 0; + } + if (teleopDrive == null) - drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + drivetrain.drive(0, 0, rotationDrive); else { double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], - rotationPID.calculate(drivetrain.getHeading())); + rotationDrive); } } diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java deleted file mode 100644 index a46f1a1f..00000000 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java +++ /dev/null @@ -1,70 +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 org.carlmontrobotics.commands; - -import static org.carlmontrobotics.Constants.Drivetrainc.*; - -import org.carlmontrobotics.subsystems.Drivetrain; -import org.carlmontrobotics.subsystems.Limelight; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj2.command.Command; - -public class AlignToApriltagMegatag2 extends Command { - - public final TeleopDrive teleopDrive; - public final Drivetrain drivetrain; - private Limelight limelight; - - public final PIDController rotationPID = - new PIDController(thetaPIDController[0], thetaPIDController[1], - thetaPIDController[2]); - - public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) { - this.limelight = limelight; - this.drivetrain = drivetrain; - this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); - - rotationPID.enableContinuousInput(-180, 180); - Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2())); - rotationPID.setSetpoint( - MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); - rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); - SendableRegistry.addChild(this, rotationPID); - addRequirements(drivetrain); - } - - @Override - public void execute() { - Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2())); - rotationPID.setSetpoint( - MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); - double rotationDrive = rotationPID.calculate(drivetrain.getHeading()); - if (!limelight.seesTag()) { - rotationDrive = 0; - } - - if (teleopDrive == null) - drivetrain.drive(0, 0, rotationDrive); - else { - double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); - drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], - rotationDrive); - } - } - - @Override - public boolean isFinished() { - return false; - // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); - // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); - // return rotationPID.atSetpoint(); - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index 546736b5..b572973c 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java +++ b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java @@ -27,11 +27,9 @@ public class AutoMATICALLYGetNote extends Command { Timer timer = new Timer(); private IntakeShooter intake; - public AutoMATICALLYGetNote(Drivetrain dt, Limelight ll, - IntakeShooter intake) { + public AutoMATICALLYGetNote(Drivetrain dt, Limelight ll) { addRequirements(this.dt = dt); this.ll = ll; - this.intake = intake; //addRequirements(this.effector = effector); // Use addRequirements() here to declare subsystem dependencies. } @@ -51,6 +49,11 @@ public void execute() { double forwardDistErrMeters = ll.getDistanceToNoteMeters(); double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad); + forwardDistErrMeters = Math.max( + forwardDistErrMeters + * SmartDashboard.getNumber("forward speed multiplier", 1.5), + MIN_MOVEMENT_METERSPSEC); + if (LimelightHelpers.getTV(INTAKE_LL_NAME)) { dt.drive(forwardDistErrMeters, strafeDistErrMeters, angleErrRad); } @@ -108,6 +111,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return intake.intakeDetectsNote(); + return intake.outtakeDetectsNote(); } } diff --git a/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java index 639cbe38..d1fb25b9 100644 --- a/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java +++ b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class RotateToFieldRelativeAngle extends Command { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index dc5dd83b..d58699c6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -1,12 +1,10 @@ package org.carlmontrobotics.subsystems; +import static org.carlmontrobotics.Constants.Drivetrainc.*; import static org.carlmontrobotics.Constants.Limelightc.*; import static org.carlmontrobotics.Constants.Limelightc.Apriltag.*; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -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.interpolation.InterpolatingDoubleTreeMap; @@ -15,21 +13,11 @@ public class Limelight extends SubsystemBase { private final Drivetrain drivetrain; - // private final SwerveDrivePoseEstimator poseEstimator; - - // private double tv, tx; - // private double[] rawBotPose = null; - // private double[] targetPose = null; private final InterpolatingDoubleTreeMap shooterMap; public Limelight(Drivetrain drivetrain) { this.drivetrain = drivetrain; - // poseEstimator = new SwerveDrivePoseEstimator( - // drivetrain.getKinematics(), - // Rotation2d.fromDegrees(drivetrain.getHeading()), - // drivetrain.getModulePositions(), - // new Pose2d()); LimelightHelpers.SetFiducialIDFiltersOverride(SHOOTER_LL_NAME, VALID_IDS); @@ -42,10 +30,6 @@ public Limelight(Drivetrain drivetrain) { @Override public void periodic() { - // poseEstimator.update(Rotation2d.fromDegrees(drivetrain.getHeading()), - // drivetrain.getModulePositions()); - - // updateMT2Odometry(); // intake limelight testing SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME)); @@ -55,22 +39,19 @@ public void periodic() { // shooter limelight testing SmartDashboard.putNumber("distance to speaker (meters)", getDistanceToSpeakerMetersMT2()); - SmartDashboard.putNumber("optimized arm angle", getArmAngleToShootSpeakerRad()); + SmartDashboard.putNumber("optimized arm angle", + getOptimizedArmAngleRadsMT2()); // changing speed multipliers for auto intaking note SmartDashboard.putNumber("forward speed multiplier", 1.5); SmartDashboard.putNumber("strafe speed multiplier", 1.5); SmartDashboard.putNumber("rotational speed multiplier", 2); - } - // public Pose2d getCurrentPose() { - // Pose2d estimatedPos = poseEstimator.getEstimatedPosition(); - // SmartDashboard.putNumber("estimated x", estimatedPos.getX()); - // // SmartDashboard.putNumber("estimated y", estimatedPos.getY()); - // // SmartDashboard.putNumber("estimated rotation (deg)", - // estimatedPos.getRotation().getDegrees()); - // return estimatedPos; - // } + // tuning apriltag alignment pid + SmartDashboard.putNumber("apriltag align kp", thetaPIDController[0]); + SmartDashboard.putNumber("apriltag align ki", thetaPIDController[1]); + SmartDashboard.putNumber("apriltag align kd", thetaPIDController[2]); + } public double getTXDeg(String limelightName) { return (limelightName == INTAKE_LL_NAME) ? LimelightHelpers.getTX(INTAKE_LL_NAME) : -LimelightHelpers.getTY(SHOOTER_LL_NAME); @@ -115,40 +96,6 @@ public double getArmAngleToShootSpeakerRad() { return END_EFFECTOR_BASE_ANGLE_RADS - Math.atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters); } - public double getRotateAngleRad() { - double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters(); - double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); - return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); - } - - // megatag2 - - // public void updateMT2Odometry() { - // boolean rejectVisionUpdate = false; - - // LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME, - // poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, - // 0); - // LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers - // .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); - - // if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { - // // degrees per second - // rejectVisionUpdate = true; - // } - - // if (visionPoseEstimate.tagCount == 0) { - // rejectVisionUpdate = true; - // } - - // if (!rejectVisionUpdate) { - // poseEstimator - // .setVisionMeasurementStdDevs(VecBuilder.fill(STD_DEV_X_METERS, - // STD_DEV_Y_METERS, STD_DEV_HEADING_RADS)); - // poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, - // visionPoseEstimate.timestampSeconds); - // } - // } public double getRotateAngleRadMT2() { Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target