From f5bdae1313f786396697feb76eb956c2ee44c580 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Sat, 22 Mar 2025 13:30:08 -0400 Subject: [PATCH 1/2] collision update --- .../commands/collision/CheckCollision.java | 59 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 1 + .../robot/subsystems/gyro/ThreadedGyro.java | 13 ++++ 3 files changed, 73 insertions(+) create mode 100644 src/main/java/frc/robot/commands/collision/CheckCollision.java diff --git a/src/main/java/frc/robot/commands/collision/CheckCollision.java b/src/main/java/frc/robot/commands/collision/CheckCollision.java new file mode 100644 index 00000000..5b7726d8 --- /dev/null +++ b/src/main/java/frc/robot/commands/collision/CheckCollision.java @@ -0,0 +1,59 @@ +package frc.robot.commands.collision; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import frc.robot.constants.Constants; +import frc.robot.subsystems.gyro.ThreadedGyro; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class CheckCollision extends LoggableCommand { + private SwerveDrivetrain drivetrain; + private DoubleSupplier horizSupplier; + private final DoubleSupplier vertSupplier; + private ThreadedGyro accelerometer; + private double currentVelValue; + private double predictedVelValue; + private boolean trustOdometry; + + public CheckCollision(ThreadedGyro accelerometer, + SwerveDrivetrain drivetrain, DoubleSupplier horizSupplier, DoubleSupplier vertSupplier) { + this.drivetrain = drivetrain; + this.horizSupplier = horizSupplier; + this.vertSupplier = vertSupplier; + this.accelerometer = accelerometer; + trustOdometry = true; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + double horizVal = -horizSupplier.getAsDouble(); + double vertVal = -vertSupplier.getAsDouble(); + double y = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY / 10; + double x = MathUtil.applyDeadband(vertVal, 0.05) * Constants.MAX_VELOCITY / 10; + currentVelValue = accelerometer.getVelocityValue(); + predictedVelValue = Math.sqrt(Math.pow(y,2) + Math.pow(x,2)); + if (Math.abs(currentVelValue - predictedVelValue) > Constants.COLLISION_VALUE) { + trustOdometry = false; + } + } + + public boolean getTrustOdometry() { + return trustOdometry; + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e1040c99..24422191 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -199,6 +199,7 @@ public enum Mode { public static final boolean HI_HI_SIMULATE_GRAVITY = false; public static final int MAX_VALID_TICKS_INTAKE = 15; // TODO: Change Later public static final int MAX_VALID_TICKS_ELEVATOR = 10; // TODO: Change Later + public static final double COLLISION_VALUE = 10; // should be tested to find what works well public static final double ROBOT_MASS = 58.967; // In Kg, change later public static final double ROBOT_BUMPER_WIDTH = 0.914; diff --git a/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java b/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java index eea52aff..31d200bc 100644 --- a/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java +++ b/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java @@ -16,9 +16,15 @@ public class ThreadedGyro { private final AtomicLong lastGyro; private final AtomicLong gyroOffset = new AtomicLong(); private final ScheduledExecutorService executor; + private double currentVelocityValue; + private double currentVelValueX; + private double currentVelValueY; public ThreadedGyro(AHRS gyro) { this.gyro = gyro; + this.currentVelocityValue = 0; + this.currentVelValueX = 0; + this.currentVelValueY = 0; this.lastGyro = new AtomicLong((Double.doubleToLongBits(0))); this.executor = Executors.newScheduledThreadPool(1); } @@ -59,12 +65,19 @@ public boolean stopAndWait(long maxTime, TimeUnit timeUnit) { private void updateGyro() { lastGyro.set(Double.doubleToLongBits(((gyro.getAngle()) % 360) * -1)); + currentVelValueX = gyro.getVelocityX(); + currentVelValueY = gyro.getVelocityY(); + currentVelocityValue = Math.sqrt(Math.pow(currentVelValueX,2) + Math.pow(currentVelValueY, 2)); } public double getGyroValue() { return Double.longBitsToDouble(lastGyro.get()); } + public double getVelocityValue() { + return currentVelocityValue; + } + public void resetGyro() { shouldReset.set(true); } From 8b75e69c1e57f1c97232ed81963bdb264ffb1807 Mon Sep 17 00:00:00 2001 From: Joseph Kantrowitz Date: Sat, 22 Mar 2025 13:33:05 -0400 Subject: [PATCH 2/2] Added collision detection --- src/main/java/frc/robot/RobotContainer.java | 4 +++- .../commands/collision/CheckCollision.java | 18 +++++++++--------- .../robot/subsystems/gyro/ThreadedGyro.java | 2 +- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 424ee467..52e4fc79 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -250,7 +250,9 @@ private void configureBindings() { new SetElevatorTargetPosition(controller::getLeftY, elevatorSubsystem); elevatorSubsystem.setDefaultCommand(setElevatorTargetPosition); controller.b().onTrue(new ClimbToLimit(climber, Constants.CLIMBER_PHASE2_SPEED)); - controller.a().onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); + controller + .a() + .onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); // controller.a().onTrue(new DeployClimber(climber)); controller.x().onTrue(new ByeByeAllDone(byebyeTilt, byebyeRoller, elevatorSubsystem)); controller.y().onTrue(new RemoveAlgaeFromReef(byebyeTilt, byebyeRoller, elevatorSubsystem)); diff --git a/src/main/java/frc/robot/commands/collision/CheckCollision.java b/src/main/java/frc/robot/commands/collision/CheckCollision.java index 5b7726d8..1cc3e678 100644 --- a/src/main/java/frc/robot/commands/collision/CheckCollision.java +++ b/src/main/java/frc/robot/commands/collision/CheckCollision.java @@ -1,12 +1,11 @@ package frc.robot.commands.collision; -import java.util.function.DoubleSupplier; - import edu.wpi.first.math.MathUtil; import frc.robot.constants.Constants; import frc.robot.subsystems.gyro.ThreadedGyro; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableCommand; +import java.util.function.DoubleSupplier; public class CheckCollision extends LoggableCommand { private SwerveDrivetrain drivetrain; @@ -17,8 +16,11 @@ public class CheckCollision extends LoggableCommand { private double predictedVelValue; private boolean trustOdometry; - public CheckCollision(ThreadedGyro accelerometer, - SwerveDrivetrain drivetrain, DoubleSupplier horizSupplier, DoubleSupplier vertSupplier) { + public CheckCollision( + ThreadedGyro accelerometer, + SwerveDrivetrain drivetrain, + DoubleSupplier horizSupplier, + DoubleSupplier vertSupplier) { this.drivetrain = drivetrain; this.horizSupplier = horizSupplier; this.vertSupplier = vertSupplier; @@ -28,8 +30,7 @@ public CheckCollision(ThreadedGyro accelerometer, } @Override - public void initialize() { - } + public void initialize() {} @Override public void execute() { @@ -38,7 +39,7 @@ public void execute() { double y = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY / 10; double x = MathUtil.applyDeadband(vertVal, 0.05) * Constants.MAX_VELOCITY / 10; currentVelValue = accelerometer.getVelocityValue(); - predictedVelValue = Math.sqrt(Math.pow(y,2) + Math.pow(x,2)); + predictedVelValue = Math.sqrt(Math.pow(y, 2) + Math.pow(x, 2)); if (Math.abs(currentVelValue - predictedVelValue) > Constants.COLLISION_VALUE) { trustOdometry = false; } @@ -49,8 +50,7 @@ public boolean getTrustOdometry() { } @Override - public void end(boolean interrupted) { - } + public void end(boolean interrupted) {} @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java b/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java index 31d200bc..f78599c3 100644 --- a/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java +++ b/src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java @@ -67,7 +67,7 @@ private void updateGyro() { lastGyro.set(Double.doubleToLongBits(((gyro.getAngle()) % 360) * -1)); currentVelValueX = gyro.getVelocityX(); currentVelValueY = gyro.getVelocityY(); - currentVelocityValue = Math.sqrt(Math.pow(currentVelValueX,2) + Math.pow(currentVelValueY, 2)); + currentVelocityValue = Math.sqrt(Math.pow(currentVelValueX, 2) + Math.pow(currentVelValueY, 2)); } public double getGyroValue() {