diff --git a/src/main/java/team3647/frc2024/commands/ClimbCommands.java b/src/main/java/team3647/frc2024/commands/ClimbCommands.java index 3272e7e..a1edac3 100644 --- a/src/main/java/team3647/frc2024/commands/ClimbCommands.java +++ b/src/main/java/team3647/frc2024/commands/ClimbCommands.java @@ -10,11 +10,11 @@ public class ClimbCommands { private final Set requirements; public Command goUp() { - return Commands.run(() -> climb.openLoop(0.5), climb); + return Commands.run(() -> climb.openLoop(1), climb); } public Command goDown() { - return Commands.run(() -> climb.openLoop(-0.8), climb); + return Commands.run(() -> climb.openLoop(-1), climb); } public Command kill() { diff --git a/src/main/java/team3647/frc2024/commands/ShooterCommands.java b/src/main/java/team3647/frc2024/commands/ShooterCommands.java index 6d9cd9d..dba4842 100644 --- a/src/main/java/team3647/frc2024/commands/ShooterCommands.java +++ b/src/main/java/team3647/frc2024/commands/ShooterCommands.java @@ -48,6 +48,16 @@ public Command setVelocity(DoubleSupplier bill, DoubleSupplier ratio) { shooterLeft); } + public Command setVelocityIndep(DoubleSupplier left, DoubleSupplier right) { + return Commands.run( + () -> { + shooterRight.setVelocity(right.getAsDouble()); + shooterLeft.setVelocity(left.getAsDouble()); + }, + shooterRight, + shooterLeft); + } + public Command kill() { return Commands.run( () -> { diff --git a/src/main/java/team3647/frc2024/robot/Robot.java b/src/main/java/team3647/frc2024/robot/Robot.java index dd8fb84..0f68fa6 100644 --- a/src/main/java/team3647/frc2024/robot/Robot.java +++ b/src/main/java/team3647/frc2024/robot/Robot.java @@ -73,7 +73,7 @@ public void robotInit() { LiveWindow.disableAllTelemetry(); LiveWindow.setEnabled(false); SignalLogger.enableAutoLogging(false); - + // SignalLogger.setPath("/home/lvuser/logs/"); SignalLogger.stop(); // Instantiate our RobotContainer. This will perform all our button bindings, diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java index 3bf8f95..431b4f6 100644 --- a/src/main/java/team3647/frc2024/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2024/robot/RobotContainer.java @@ -382,6 +382,7 @@ public Command getAutonomousCommand() { new VisionController( swerve::addVisionData, swerve::shouldAddData, + swerve::seedFieldRelative, coController.buttonA, coController.buttonX, backLeft, diff --git a/src/main/java/team3647/frc2024/subsystems/Pivot.java b/src/main/java/team3647/frc2024/subsystems/Pivot.java index ff5798c..43dd241 100644 --- a/src/main/java/team3647/frc2024/subsystems/Pivot.java +++ b/src/main/java/team3647/frc2024/subsystems/Pivot.java @@ -6,9 +6,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; import java.util.function.BooleanSupplier; -import org.littletonrobotics.junction.Logger; import team3647.lib.TalonFXSubsystem; public class Pivot extends TalonFXSubsystem { @@ -61,12 +59,12 @@ public void setEncoder(double degree) { @Override public void periodic() { super.periodic(); - Logger.recordOutput( - "Pivot/Pose", - new Pose3d( - new Translation3d( - Units.inchesToMeters(0.3), 0, Units.inchesToMeters(17.2 + 1.75)), - new Rotation3d(0, Units.degreesToRadians(getAngle()), 0))); + // Logger.recordOutput( + // "Pivot/Pose", + // new Pose3d( + // new Translation3d( + // Units.inchesToMeters(0.3), 0, Units.inchesToMeters(17.2 + 1.75)), + // new Rotation3d(0, Units.degreesToRadians(getAngle()), 0))); if (underStage.getAsBoolean()) { this.maxAngle = maxAngleUnderStage; } else { diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java index a843cfc..afea2f6 100644 --- a/src/main/java/team3647/frc2024/subsystems/Superstructure.java +++ b/src/main/java/team3647/frc2024/subsystems/Superstructure.java @@ -12,7 +12,6 @@ import team3647.frc2024.commands.PivotCommands; import team3647.frc2024.commands.ShooterCommands; import team3647.frc2024.commands.WristCommands; -import team3647.frc2024.constants.ShooterConstants; public class Superstructure { @@ -114,8 +113,7 @@ public boolean isClimbing() { } public Command spinUp() { - return shooterCommands.setVelocity( - () -> getDesiredSpeed(), () -> ShooterConstants.kLeftRatio); + return shooterCommands.setVelocityIndep(() -> 28, () -> 18); } public Command spinUpTrap() { @@ -137,7 +135,7 @@ public Command stowChurro() { } public Command spinUpAmp() { - return shooterCommands.setVelocity(() -> 3.5, () -> 1); + return shooterCommands.setVelocity(() -> 5, () -> 1); } public double getDesiredSpeed() { @@ -173,7 +171,7 @@ public Command ejectPiece() { } public boolean flywheelReadY() { - return shooterLeft.velocityReached(15, 1); + return shooterRight.velocityGreater(27.5); } public boolean pivotReady() { diff --git a/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java index 4a389a5..20aa433 100644 --- a/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java +++ b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java @@ -268,7 +268,7 @@ public Command runSteerDynamTest(SysIdRoutine.Direction direction) { } public void setRobotPose(Pose2d pose) { - m_odometry.resetPosition(this.m_pigeon2.getRotation2d(), getModulePositions(), pose); + seedFieldRelative(pose); periodicIO = new PeriodicIO(); } @@ -327,8 +327,20 @@ public Pose2d getOdoPose() { } public void setStuff(SwerveDriveState state) { + // SignalLogger.writeDoubleArray( + // "odometry", + // new double[] { + // state.Pose.getX(), state.Pose.getY(), state.Pose.getRotation().getDegrees() + // }); periodicIO.pose = state.Pose; periodicIO.speeds = this.m_kinematics.toChassisSpeeds(state.ModuleStates); + // SignalLogger.writeDoubleArray( + // "speeds", + // new double[] { + // periodicIO.speeds.vxMetersPerSecond, + // periodicIO.speeds.vyMetersPerSecond, + // periodicIO.speeds.omegaRadiansPerSecond + // }); } public Rotation2d getOdoRot() { @@ -366,9 +378,10 @@ public ChassisSpeeds getFieldRelativeChassisSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getOdoRot()); } - public boolean shouldAddData(Pose2d visionPose) { - double distance = visionPose.minus(getOdoPose()).getTranslation().getNorm(); - double angle = visionPose.getRotation().minus(getOdoPose().getRotation()).getDegrees(); + public boolean shouldAddData(VisionMeasurement measurement) { + double distance = measurement.pose.minus(getOdoPose()).getTranslation().getNorm(); + double angle = + measurement.pose.getRotation().minus(getOdoPose().getRotation()).getDegrees(); return (distance < 1.5 && Math.abs(angle) < 15) || DriverStation.isAutonomous() ? true : false; @@ -377,6 +390,14 @@ public boolean shouldAddData(Pose2d visionPose) { public void addVisionData(VisionMeasurement data) { periodicIO.visionPose = data.pose; SmartDashboard.putNumber("timestamped viison", data.timestamp); + // SignalLogger.writeDoubleArray( + // "vision pose", + // new double[] { + // data.pose.getX(), + // data.pose.getY(), + // data.pose.getRotation().getDegrees(), + // data.timestamp + // }); addVisionMeasurement(data.pose, data.timestamp, data.stdDevs); } diff --git a/src/main/java/team3647/frc2024/subsystems/Wrist.java b/src/main/java/team3647/frc2024/subsystems/Wrist.java index c8bb2c1..ff27287 100644 --- a/src/main/java/team3647/frc2024/subsystems/Wrist.java +++ b/src/main/java/team3647/frc2024/subsystems/Wrist.java @@ -2,11 +2,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import org.littletonrobotics.junction.Logger; import team3647.frc2024.util.InverseKinematics; import team3647.lib.TalonFXSubsystem; @@ -38,11 +33,11 @@ public void setEncoder(double degree) { @Override public void periodic() { super.periodic(); - Logger.recordOutput( - "Wrist/Pose", - new Pose3d( - new Translation3d(0.28, 0, 0.18), - new Rotation3d(0, -Units.degreesToRadians(getAngle()), 0))); + // Logger.recordOutput( + // "Wrist/Pose", + // new Pose3d( + // new Translation3d(0.28, 0, 0.18), + // new Rotation3d(0, -Units.degreesToRadians(getAngle()), 0))); // Logger.recordOutput( // "Intake/Pose", // new Pose3d( diff --git a/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java b/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java index 7394f4f..b220b83 100644 --- a/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java +++ b/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java @@ -98,7 +98,7 @@ public Optional QueueToInputs() { if (targetDistance > 4 && !hasPriority) { return Optional.empty(); } - if (targetDistance > 10) { + if (targetDistance > 8) { return Optional.empty(); } if (Math.abs(update.get().estimatedPose.getZ()) > 0.5) { diff --git a/src/main/java/team3647/frc2024/util/AutoDrive.java b/src/main/java/team3647/frc2024/util/AutoDrive.java index dabff31..44e76bb 100644 --- a/src/main/java/team3647/frc2024/util/AutoDrive.java +++ b/src/main/java/team3647/frc2024/util/AutoDrive.java @@ -186,7 +186,10 @@ public double getRot() { rotController.calculate(targetRot, 0) * MathUtil.clamp( Math.abs(swerve.getChassisSpeeds().vyMetersPerSecond), 1, 100); - double setpoint = Math.abs(targetRot) < 0.01 ? 0 : k; + if (Math.abs(k) < 0.05) { + k *= 3; + } + double setpoint = Math.abs(targetRot) < 0.002 ? 0 : k; return setpoint; } diff --git a/src/main/java/team3647/frc2024/util/VisionController.java b/src/main/java/team3647/frc2024/util/VisionController.java index ffb001c..d3c662b 100644 --- a/src/main/java/team3647/frc2024/util/VisionController.java +++ b/src/main/java/team3647/frc2024/util/VisionController.java @@ -13,18 +13,22 @@ public class VisionController extends VirtualSubsystem { private final AprilTagCamera[] cameras; private final Consumer botPoseAcceptor; - private final Function shouldAddData; + private final Consumer resetPose; + private final Function shouldAddData; private final ArrayList list = new ArrayList<>(); private final BooleanSupplier dataAddOverride; private final BooleanSupplier turnOffVision; + private int count; public VisionController( Consumer visionAcceptor, - Function shouldAddData, + Function shouldAddData, + Consumer resetPose, BooleanSupplier dataAddOverride, BooleanSupplier turnOffVision, AprilTagCamera... cameras) { this.cameras = cameras; + this.resetPose = resetPose; this.shouldAddData = shouldAddData; this.botPoseAcceptor = visionAcceptor; this.dataAddOverride = dataAddOverride; @@ -46,8 +50,16 @@ public void periodic() { var getInputs = inputs.get(); - if (shouldAddData.apply(getInputs.pose) || dataAddOverride.getAsBoolean()) { + if (shouldAddData.apply(getInputs) || dataAddOverride.getAsBoolean()) { list.add(getInputs); + count = 0; + } else { + count++; + if (count > 10) { + resetPose.accept(getInputs.pose); + Logger.recordOutput("Robot/Reset", getInputs.pose); + break; + } } // Logger.recordOutput("Robot/" + camera.getName(), getInputs.pose);