Skip to content

Jk collision detection - improving poseEstimation #285

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Mobby42 wants to merge 3 commits into
base: main
Choose a base branch
from
+76 −1
Open

Jk collision detection - improving poseEstimation #285

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f5bdae1
collision update
Mobby42 Mar 22, 2025
09b7d56
Merge branch 'main' of https://github.com/FRC4048/FRC2025_Java into J…
Mobby42 Mar 22, 2025
8b75e69
Added collision detection
Mobby42 Mar 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
59 changes: 59 additions & 0 deletions src/main/java/frc/robot/commands/collision/CheckCollision.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
package frc.robot.commands.collision;

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;
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) {
Copy link
Contributor

@Levercpu Levercpu Mar 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This current math just checks the magnitude of the velocities; it would be interesting if we could instead use vectors, and after subtracting the two vectors, we could do the vector.norm method to find the magnitude of the vector and then see if that's greater than the collision value squared. Now that I am also obsessed with optimization, this can be improved even further if we change the constant to constants. Collision_Value_squared and do Vector.dot(Vector), which is more efficient as the square root is computationally expensive. Alternatively, we can also do the two velocity vectors (gyro and odometry) and also dot them together, which will scale with both of their magnitudes and the angle between them.

Copy link
Contributor

@Levercpu Levercpu Mar 22, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should this apply deadband and have Max_Velocity as its third parameter instead? Also, why are we multiplying by Max_velocity /10? (This one i dont really know.)

Copy link
Contributor

@Levercpu Levercpu Mar 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

also branch needs to be tested.

trustOdometry = false;
}
}

public boolean getTrustOdometry() {
return trustOdometry;
}

@Override
public void end(boolean interrupted) {}

@Override
public boolean isFinished() {
return false;
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/constants/GameConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,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;
Expand Down
13 changes: 13 additions & 0 deletions src/main/java/frc/robot/subsystems/gyro/ThreadedGyro.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand Down