diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 47dcdc0..1fc9a03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -160,8 +160,8 @@ private void configureBindings() { ); }, _swerveSubsystem)); - _driveController.L2().onTrue( - new AutoAim( // TODO: test + _driveController.L2().whileTrue( + new AutoAim( _swerveSubsystem, _shooterSubsystem, _elevatorSubsystem, @@ -177,7 +177,9 @@ private void configureBindings() { _swerveSubsystem, _shooterSubsystem, _elevatorSubsystem, - _ledSubsystem + _ledSubsystem, + () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05), + () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05) ) ); } diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java index a0eb5e5..dd8a27a 100644 --- a/src/main/java/frc/robot/commands/auto/AutoAim.java +++ b/src/main/java/frc/robot/commands/auto/AutoAim.java @@ -76,7 +76,7 @@ public AutoAim( DoubleSupplier xSpeed, DoubleSupplier ySpeed ) { - this(swerve, shooter, elevator, leds, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, true); + this(swerve, shooter, elevator, leds, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, false); } /** diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index d7c8291..7a0b5d7 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -121,6 +121,8 @@ public void driveElevator(double speed) { ff = UtilFuncs.FromVolts(ff); + speed = MathUtil.applyDeadband(speed, 0.08); + _leftMotor.set(ff + speed); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index af093c7..ab9a892 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -253,6 +253,10 @@ public double getDriveCoeff() { * @see ChassisSpeeds (wpilib chassis speeds class) */ public void driveChassis(ChassisSpeeds chassisSpeeds) { + chassisSpeeds.vxMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vxMetersPerSecond, .01); + chassisSpeeds.vyMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vyMetersPerSecond, .01); + chassisSpeeds.omegaRadiansPerSecond = MathUtil.applyDeadband(chassisSpeeds.omegaRadiansPerSecond, Math.PI / 20); + // IMPORTANT: X-axis and Y-axis are flipped (based on wpilib coord system) if (fieldOriented) { double relativeHeading = getHeading().getDegrees() + (UtilFuncs.GetAlliance() == Alliance.Red ? 180 : 0); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 1306d47..1789651 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -1,8 +1,12 @@ /* Copyright (C) 2024 Team 334. All Rights Reserved.*/ package frc.robot.subsystems; +import java.lang.reflect.Array; +import java.util.ArrayList; +import java.util.Arrays; import java.util.Collections; import java.util.Comparator; +import java.util.List; import java.util.Optional; import javax.swing.text.html.Option; @@ -19,6 +23,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.FieldConstants; +import frc.robot.utils.NoteSort; import frc.robot.utils.UtilFuncs; import frc.robot.utils.helpers.LimelightHelper; @@ -185,31 +190,8 @@ public double[] tagAngleOffsets(int ID) { } public double[] getNoteAngles() { - ArrayNode targets = _intake.getNeuralTargets(); - - // Collections.sort( - // targets, new Comparator() { - // @Override - // public int compare(JsonNode noteA, JsonNode noteB) { - // double noteA_area = noteA.get("ta").asDouble(); - // double noteB_area = noteB.get("ta").asDouble(); - - // if (noteA_area > noteB_area) { - // return 1; - // } - - // if (noteB_area > noteA_area) { - // return -1; - // } - - // else { - // return 0; - // } - // } - // } - // ); - - // return {0, 0}; + ArrayList targets = _intake.getNeuralTargets(); + Arrays.sort(targets.toArray(new JsonNode[targets.size()]), new NoteSort()); JsonNode target = targets.get(0); diff --git a/src/main/java/frc/robot/utils/NoteSort.java b/src/main/java/frc/robot/utils/NoteSort.java new file mode 100644 index 0000000..6b95a6b --- /dev/null +++ b/src/main/java/frc/robot/utils/NoteSort.java @@ -0,0 +1,20 @@ +// 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 frc.robot.utils; + +import java.util.Comparator; + +import com.fasterxml.jackson.databind.JsonNode; + +public class NoteSort implements Comparator { + @Override + public int compare(JsonNode noteA, JsonNode noteB) + { + double ta_A = noteA.get("ta").asDouble(); + double ta_B = noteB.get("ta").asDouble(); + + return (int) Math.signum(ta_A - ta_B); + } +} diff --git a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java index 87ba8a9..0dcd45c 100644 --- a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java +++ b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java @@ -1,6 +1,8 @@ /* Copyright (C) 2024 Team 334. All Rights Reserved.*/ package frc.robot.utils.helpers; +import java.util.ArrayList; + import com.fasterxml.jackson.databind.JsonNode; import com.fasterxml.jackson.databind.ObjectMapper; import com.fasterxml.jackson.databind.node.ArrayNode; @@ -44,10 +46,15 @@ public JsonNode getJson() { * Returns the neural targets from the limelight. * */ - public ArrayNode getNeuralTargets() { + public ArrayList getNeuralTargets() { ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector"); + ArrayList out = new ArrayList<>(); + + for (JsonNode t : targets) { + out.add(t); + } - return targets; + return out; } /**