From 485340f46068f973cf876ac27f3a680606c0f4a5 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Sun, 25 Aug 2024 22:12:57 +0300 Subject: [PATCH 1/6] misc --- src/main/java/frc/trigon/robot/Robot.java | 4 +++- .../robot/constants/OperatorConstants.java | 2 +- .../robot/constants/RobotConstants.java | 20 +++++++++++------- .../ObjectDetectionCamera.java | 6 +++--- .../subsystems/climber/ClimberConstants.java | 21 ++++++++++--------- .../elevator/ElevatorConstants.java | 11 +++++----- .../subsystems/pitcher/PitcherConstants.java | 17 ++++++++------- .../subsystems/shooter/ShooterConstants.java | 9 ++++---- .../subsystems/swerve/SwerveConstants.java | 21 ++++++++++--------- .../swerve/SwerveModuleConstants.java | 16 +++++++------- 10 files changed, 69 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index 64947f2..79ea6a8 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -15,6 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.utilities.LocalADStarAK; public class Robot extends LoggedRobot { @@ -25,6 +26,7 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { + RobotConstants.init(); Pathfinding.setPathfinder(new LocalADStarAK()); configLogger(); robotContainer = new RobotContainer(); @@ -76,7 +78,7 @@ public void testPeriodic() { } private void configLogger() { - if (RobotConstants.IS_REPLAY) { + if (RobotHardwareStats.isReplay()) { setUseTiming(false); final String logPath = LogFileUtil.findReplayLog(); final String logWriterPath = LogFileUtil.addPathSuffix(logPath, "_replay"); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index ee0a8bd..a4c1bcb 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -9,7 +9,7 @@ public class OperatorConstants { private static final int DRIVER_CONTROLLER_PORT = 0; private static final int DRIVER_CONTROLLER_EXPONENT = 1; - private static final double DRIVER_CONTROLLER_DEADBAND = 0.1; + private static final double DRIVER_CONTROLLER_DEADBAND = 0; public static final XboxController DRIVER_CONTROLLER = new XboxController( DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 6b683a9..39e7a1d 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -1,15 +1,19 @@ package frc.trigon.robot.constants; import frc.trigon.robot.Robot; +import org.trigon.hardware.RobotHardwareStats; +import org.trigon.utilities.FilesHandler; public class RobotConstants { private static final boolean - UNFILTERED_IS_SIMULATION = true, - UNFILTERED_IS_REPLAY = false; - public static final boolean - IS_SIMULATION = UNFILTERED_IS_SIMULATION && !Robot.IS_REAL, - IS_REPLAY = UNFILTERED_IS_REPLAY && !Robot.IS_REAL; - public static final double PERIODIC_TIME_SECONDS = 0.02; + IS_SIMULATION = true, + IS_REPLAY = false; + private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = "/media/sda1/akitlogs/"; -} + public static final String LOGGING_PATH = IS_SIMULATION && Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + + public static void init() { + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); + RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index e4e8f73..e9658b0 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,8 +1,8 @@ package frc.trigon.robot.misc.objectdetectioncamera; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.trigon.robot.constants.RobotConstants; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; public class ObjectDetectionCamera extends SubsystemBase { private final ObjectDetectionCameraInputsAutoLogged objectDetectionCameraInputs = new ObjectDetectionCameraInputsAutoLogged(); @@ -54,9 +54,9 @@ public void startTrackingBestObject() { } private ObjectDetectionCameraIO generateIO(String hostname) { - if (RobotConstants.IS_REPLAY) + if (RobotHardwareStats.isReplay()) return new ObjectDetectionCameraIO(); - if (RobotConstants.IS_SIMULATION) + if (RobotHardwareStats.isSimulation()) return new SimulationObjectDetectionCameraIO(hostname); return new PhotonObjectDetectionCameraIO(hostname); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index c9189fc..48a0f41 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -55,21 +56,21 @@ public class ClimberConstants { MAX_CLIMBING_VELOCITY = 1, MAX_CLIMBING_ACCELERATION = 1; private static final double - NON_CLIMBING_P = RobotConstants.IS_SIMULATION ? 30 : 0, + NON_CLIMBING_P = RobotHardwareStats.isSimulation() ? 30 : 0, NON_CLIMBING_I = 0, NON_CLIMBING_D = 0, - NON_CLIMBING_KS = RobotConstants.IS_SIMULATION ? 0.030713 : 0.3081, - NON_CLIMBING_KG = RobotConstants.IS_SIMULATION ? 0.009791 : -0.094675, - NON_CLIMBING_KV = RobotConstants.IS_SIMULATION ? 2.385 : 2.3525, - NON_CLIMBING_KA = RobotConstants.IS_SIMULATION ? 0.049261 : 0.045652; + NON_CLIMBING_KS = RobotHardwareStats.isSimulation() ? 0.030713 : 0.3081, + NON_CLIMBING_KG = RobotHardwareStats.isSimulation() ? 0.009791 : -0.094675, + NON_CLIMBING_KV = RobotHardwareStats.isSimulation() ? 2.385 : 2.3525, + NON_CLIMBING_KA = RobotHardwareStats.isSimulation() ? 0.049261 : 0.045652; private static final double - CLIMBING_P = RobotConstants.IS_SIMULATION ? 30 : 15, + CLIMBING_P = RobotHardwareStats.isSimulation() ? 30 : 15, CLIMBING_I = 0, CLIMBING_D = 0, - CLIMBING_KS = RobotConstants.IS_SIMULATION ? 0.030713 : 0.30095, - CLIMBING_KG = RobotConstants.IS_SIMULATION ? 0.009791 : -0.58087, - CLIMBING_KV = RobotConstants.IS_SIMULATION ? 2.385 : 2.2353, - CLIMBING_KA = RobotConstants.IS_SIMULATION ? 0.049261 : 0.061477; + CLIMBING_KS = RobotHardwareStats.isSimulation() ? 0.030713 : 0.30095, + CLIMBING_KG = RobotHardwareStats.isSimulation() ? 0.009791 : -0.58087, + CLIMBING_KV = RobotHardwareStats.isSimulation() ? 2.385 : 2.2353, + CLIMBING_KA = RobotHardwareStats.isSimulation() ? 0.049261 : 0.061477; static final int NON_CLIMBING_SLOT = 0, CLIMBING_SLOT = 1; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 78f2c7d..50b7ee0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -42,13 +43,13 @@ public class ElevatorConstants { private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.719726; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; private static final double - P = RobotConstants.IS_SIMULATION ? 52 : 2.5, + P = RobotHardwareStats.isSimulation() ? 52 : 2.5, I = 0, D = 0, - KS = RobotConstants.IS_SIMULATION ? 0.019539 : 0.036646 + 0.0798, - KV = RobotConstants.IS_SIMULATION ? 0.987 : 0.44458, - KG = RobotConstants.IS_SIMULATION ? 0.12551 : 0.4, - KA = RobotConstants.IS_SIMULATION ? 0.017514 : 0.026516; + KS = RobotHardwareStats.isSimulation() ? 0.019539 : 0.036646 + 0.0798, + KV = RobotHardwareStats.isSimulation() ? 0.987 : 0.44458, + KG = RobotHardwareStats.isSimulation() ? 0.12551 : 0.4, + KA = RobotHardwareStats.isSimulation() ? 0.017514 : 0.026516; static final double MOTION_MAGIC_CRUISE_VELOCITY = 25, MOTION_MAGIC_ACCELERATION = 25; diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index b94d0d8..ae3ae84 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; @@ -45,15 +46,15 @@ public class PitcherConstants { private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; private static final double OFFSET = Conversions.degreesToRotations(11.337891 + 90); private static final double - MOTION_MAGIC_P = RobotConstants.IS_SIMULATION ? 100 : 90, + MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 90, MOTION_MAGIC_I = 0, - MOTION_MAGIC_D = RobotConstants.IS_SIMULATION ? 0 : 45, - KS = RobotConstants.IS_SIMULATION ? 0.053988 : 1.4, - KV = RobotConstants.IS_SIMULATION ? 39 : 0, - KA = RobotConstants.IS_SIMULATION ? 0.85062 : 9.2523, - KG = RobotConstants.IS_SIMULATION ? 0.04366 : 1.2, - EXPO_KV = RobotConstants.IS_SIMULATION ? KV : 38.757, - EXPO_KA = RobotConstants.IS_SIMULATION ? KA : 0.6; + MOTION_MAGIC_D = RobotHardwareStats.isSimulation() ? 0 : 45, + KS = RobotHardwareStats.isSimulation() ? 0.053988 : 1.4, + KV = RobotHardwareStats.isSimulation() ? 41 : 0, + KA = RobotHardwareStats.isSimulation() ? 0.85062 : 9.2523, + KG = RobotHardwareStats.isSimulation() ? 0.04366 : 1.2, + EXPO_KV = RobotHardwareStats.isSimulation() ? KV : 38.757, + EXPO_KA = RobotHardwareStats.isSimulation() ? KA : 0.6; static final boolean FOC_ENABLED = true; private static final double GEAR_RATIO = 352.8; diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 649ddf6..881711f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.FlywheelSimulation; @@ -28,12 +29,12 @@ public class ShooterConstants { MASTER_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive, FOLLOWER_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final double - P = RobotConstants.IS_SIMULATION ? 12 : 12, + P = RobotHardwareStats.isSimulation() ? 12 : 12, I = 0, D = 0, - KV = RobotConstants.IS_SIMULATION ? 0 : 0, - KA = RobotConstants.IS_SIMULATION ? 0 : 0, - KS = RobotConstants.IS_SIMULATION ? 0 : 8; + KV = RobotHardwareStats.isSimulation() ? 0 : 0, + KA = RobotHardwareStats.isSimulation() ? 0 : 0, + KS = RobotHardwareStats.isSimulation() ? 0 : 8; private static final boolean FOLLOWER_OPPOSES_MASTER = false; private static final double GEAR_RATIO = 1; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index a3d44b2..a3df31a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -12,6 +12,7 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Signal; import org.trigon.utilities.Conversions; @@ -64,28 +65,28 @@ public abstract class SwerveConstants { TRANSLATION_VELOCITY_TOLERANCE = 0.05, ROTATION_VELOCITY_TOLERANCE = 0.3; static final double - DRIVE_NEUTRAL_DEADBAND = 0.2, - ROTATION_NEUTRAL_DEADBAND = 0.2; + DRIVE_NEUTRAL_DEADBAND = 0.01, + ROTATION_NEUTRAL_DEADBAND = 0.01; static final double - MAX_SPEED_METERS_PER_SECOND = RobotConstants.IS_SIMULATION ? 4.9 : 4.04502, - MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND = RobotConstants.IS_SIMULATION ? 12.03 : 12.03; + MAX_SPEED_METERS_PER_SECOND = RobotHardwareStats.isSimulation() ? 4.9 : 4.04502, + MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND = RobotHardwareStats.isSimulation() ? 12.03 : 12.03; private static final PIDConstants - TRANSLATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : new PIDConstants(5, 0, 0), - PROFILED_ROTATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(8, 0, 0) : new PIDConstants(5, 0, 0), - AUTO_TRANSLATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(9, 0, 0) : new PIDConstants(6.5, 0, 0), - AUTO_ROTATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(8.9, 0, 0) : new PIDConstants(3, 0, 0); private static final double - MAX_ROTATION_VELOCITY = RobotConstants.IS_SIMULATION ? 720 : 720, - MAX_ROTATION_ACCELERATION = RobotConstants.IS_SIMULATION ? 720 : 720; + MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 720, + MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 720; private static final TrapezoidProfile.Constraints ROTATION_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_ROTATION_VELOCITY, MAX_ROTATION_ACCELERATION diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 928a103..714bccc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -4,7 +4,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.*; import edu.wpi.first.math.system.plant.DCMotor; -import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.simulation.SimpleMotorSimulation; public class SwerveModuleConstants { @@ -12,8 +12,8 @@ public class SwerveModuleConstants { DRIVE_GEAR_RATIO = 6.12, STEER_GEAR_RATIO = 12.8; private static final double - DRIVE_OPEN_LOOP_RAMP_RATE = RobotConstants.IS_SIMULATION ? 0.1 : 0.1, - DRIVE_CLOSED_LOOP_RAMP_RATE = RobotConstants.IS_SIMULATION ? 0.1 : 0.1; + DRIVE_OPEN_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.1, + DRIVE_CLOSED_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.1; private static final InvertedValue DRIVE_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, STEER_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; @@ -23,14 +23,14 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotConstants.IS_SIMULATION ? 1000 : 100, - STEER_CURRENT_LIMIT = RobotConstants.IS_SIMULATION ? 1000 : 50; + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 100, + STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 50; private static final double - STEER_MOTOR_P = RobotConstants.IS_SIMULATION ? 75 : 75, + STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotConstants.IS_SIMULATION ? 50 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 50 : 50, DRIVE_MOTOR_I = 0, DRIVE_MOTOR_D = 0; static final boolean ENABLE_FOC = true; @@ -49,7 +49,7 @@ public class SwerveModuleConstants { DRIVE_MOTOR_GEARBOX = DCMotor.getKrakenX60Foc(DRIVE_MOTOR_AMOUNT), STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); - static final double WHEEL_DIAMETER_METERS = RobotConstants.IS_SIMULATION ? 0.1016 : 0.049149 * 2; + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049149 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { From 78400dda465d781030cca8586fdce21987f71468 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Sun, 6 Oct 2024 23:57:22 +0300 Subject: [PATCH 2/6] Everything WORKS and WORKS well (that is, calibrations, camera stuff will probably need some tuning) --- build.gradle | 2 +- simgui-window.json | 7 +- .../java/frc/trigon/robot/RobotContainer.java | 21 +- .../robot/commands/AlignToNoteCommand.java | 51 +- .../frc/trigon/robot/commands/Commands.java | 2 +- .../VisualizeNoteShootingCommand.java | 2 +- .../robot/constants/CameraConstants.java | 38 +- .../robot/constants/CommandConstants.java | 6 +- .../robot/constants/FieldConstants.java | 3 +- .../robot/constants/OperatorConstants.java | 2 +- .../robot/constants/RobotConstants.java | 4 +- .../robot/constants/ShootingConstants.java | 4 +- .../robot/misc/ShootingCalculations.java | 4 +- .../ObjectDetectionCamera.java | 29 +- .../apriltagcamera/AprilTagCamera.java | 212 +++++ .../AprilTagCameraConstants.java | 27 + .../apriltagcamera/AprilTagCameraIO.java | 20 + .../io/AprilTagLimelightIO.java | 87 ++ .../io/AprilTagPhotonCameraIO.java | 124 +++ .../EstimatedRobotPose.java | 72 -- .../PhotonPoseEstimator.java | 791 ------------------ .../poseestimator/PoseEstimator.java | 41 +- .../poseestimator/PoseEstimator6328.java | 7 + .../poseestimator/PoseEstimatorConstants.java | 4 - .../robotposesources/AprilTagLimelightIO.java | 21 - .../AprilTagPhotonCameraIO.java | 91 -- .../robotposesources/RobotPoseSource.java | 82 -- .../RobotPoseSourceConstants.java | 40 - .../robotposesources/RobotPoseSourceIO.java | 18 - .../robotposesources/T265IO.java | 76 -- .../robot/subsystems/MotorSubsystem.java | 34 +- .../robot/subsystems/climber/Climber.java | 20 +- .../robot/subsystems/elevator/Elevator.java | 33 +- .../subsystems/elevator/ElevatorCommands.java | 11 + .../elevator/ElevatorConstants.java | 58 +- .../robot/subsystems/intake/Intake.java | 17 +- .../subsystems/intake/IntakeConstants.java | 8 +- .../robot/subsystems/pitcher/Pitcher.java | 31 +- .../subsystems/pitcher/PitcherConstants.java | 38 +- .../robot/subsystems/shooter/Shooter.java | 14 +- .../robot/subsystems/swerve/Swerve.java | 49 +- .../subsystems/swerve/SwerveConstants.java | 14 +- .../robot/subsystems/swerve/SwerveModule.java | 35 +- .../swerve/SwerveModuleConstants.java | 18 +- .../subsystems/transporter/Transporter.java | 16 +- vendordeps/AdvantageKit.json | 10 +- 46 files changed, 847 insertions(+), 1447 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java diff --git a/build.gradle b/build.gradle index b0d27e6..9b3bef3 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.4' + implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.11' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/simgui-window.json b/simgui-window.json index d1d16c3..0b3a77f 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -15,6 +15,11 @@ } }, "Table": { + "0x542B5671,2": { + "Column 0 Width": "347", + "Column 1 Width": "204", + "RefScale": "16" + }, "0xE56EC1C2,4": { "Column 0 Weight": "1.0000", "Column 1 Weight": "1.0000", @@ -86,7 +91,7 @@ "###Joysticks": { "Collapsed": "0", "Pos": "320,888", - "Size": "976,278" + "Size": "976,82" }, "###NetworkTables": { "Collapsed": "0", diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2bd310b..b142fb2 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -20,8 +20,6 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.Climber; -import frc.trigon.robot.subsystems.climber.ClimberCommands; -import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.elevator.Elevator; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; @@ -30,7 +28,6 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.Pitcher; -import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -43,6 +40,12 @@ import java.awt.*; public class RobotContainer { + public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( +// CameraConstants.REAR_LEFT_CAMERA, +// CameraConstants.REAR_RIGHT_CAMERA, +// CameraConstants.FRONT_MIDDLE_CAMERA, + CameraConstants.REAR_MIDDLE_CAMERA + ); public static final Swerve SWERVE = new Swerve(); public static final Shooter SHOOTER = new Shooter(); public static final Pitcher PITCHER = new Pitcher(); @@ -50,12 +53,6 @@ public class RobotContainer { public static final Elevator ELEVATOR = new Elevator(); public static final Transporter TRANSPORTER = new Transporter(); public static final Climber CLIMBER = new Climber(); - public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( -// CameraConstants.REAR_LEFT_CAMERA, -// CameraConstants.REAR_RIGHT_CAMERA, -// CameraConstants.FRONT_MIDDLE_CAMERA, - CameraConstants.REAR_MIDDLE_CAMERA - ); private final LoggedDashboardChooser autoChooser; public RobotContainer() { @@ -76,7 +73,7 @@ public Command getAutonomousCommand() { private void configureBindings() { bindDefaultCommands(); bindControllerCommands(); -// configureSysIdBindings(PITCHER); +// configureSysIdBindings(ELEVATOR); } private void bindDefaultCommands() { @@ -86,7 +83,7 @@ private void bindDefaultCommands() { PITCHER.setDefaultCommand(CommandConstants.PITCHER_RESTING_COMMAND); ELEVATOR.setDefaultCommand(new WaitUntilCommand(() -> ELEVATOR.isBelowCameraPlate() && ELEVATOR.didOpenElevator()).andThen(Commands.withoutRequirements(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.ALIGNING_FOR_AMP_BACKWARDS)).withTimeout(0.13).andThen(new InstantCommand(() -> ELEVATOR.setDidOpenElevator(false)))).alongWith(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.RESTING))); TRANSPORTER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(TRANSPORTER)); - CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RESTING)); + CLIMBER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(CLIMBER)); LEDStrip.setDefaultCommandForAllLEDS((ledStrip) -> LEDStripCommands.getAnimateColorFlowCommand(new Color(0, 150, 255), 0.5, ledStrip)); } @@ -119,7 +116,7 @@ private void bindControllerCommands() { OperatorConstants.TURN_AUTOMATIC_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTOMATIC_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTOMATIC_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTOMATIC_NOTE_ALIGNING_OFF_COMMAND); - OperatorConstants.DEBUGGING_BUTTON.whileTrue(PitcherCommands.getDebuggingCommand()); + OperatorConstants.DEBUGGING_BUTTON.whileTrue(ElevatorCommands.getDebuggingCommand()); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index eaae663..5e15479 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -3,10 +3,8 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.CommandConstants; @@ -15,48 +13,26 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.utilities.mirrorable.MirrorableRotation2d; import java.awt.*; public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; - private static final PIDController Y_PID_CONTROLLER = new PIDController(0.005, 0, 0); - private boolean didCollect = false; - private boolean wasVisible = false; - private double trackedNoteYaw = 0; + private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new PIDController(0.0075, 0, 0) : + new PIDController(0, 0, 0); public AlignToNoteCommand() { - configureNoteCollectionDetectionTrigger(); addCommands( - new InstantCommand(() -> { - didCollect = false; - wasVisible = false; - }), - getCurrentLEDColorCommand().asProxy(), + getSetCurrentLEDColorCommand().asProxy(), Commands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), Commands.duplicate(CommandConstants.SELF_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), - new RunCommand(this::trackObject) + new RunCommand(CAMERA::trackObject) ); } - private void trackObject() { - if (hasTarget() && !wasVisible) { - wasVisible = true; - CAMERA.startTrackingBestObject(); - trackedNoteYaw = CAMERA.getTrackedObjectYaw(); - return; - } - - if (!hasTarget()) { - wasVisible = false; - return; - } - - if (hasTarget()) - trackedNoteYaw = CAMERA.getTrackedObjectYaw(); - } - - private Command getCurrentLEDColorCommand() { + private Command getSetCurrentLEDColorCommand() { return Commands.getContinuousConditionalCommand( LEDStripCommands.getStaticColorCommand(Color.green, LEDStripConstants.LED_STRIPS), LEDStripCommands.getStaticColorCommand(Color.red, LEDStripConstants.LED_STRIPS), @@ -67,22 +43,17 @@ private Command getCurrentLEDColorCommand() { private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), - () -> Y_PID_CONTROLLER.calculate(-trackedNoteYaw), + () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); } private MirrorableRotation2d getTargetAngle() { final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return new MirrorableRotation2d(currentRotation.plus(Rotation2d.fromDegrees(trackedNoteYaw)), false); - } - - private void configureNoteCollectionDetectionTrigger() { - final Trigger noteCollectionTrigger = RobotContainer.INTAKE.getEarlyNoteCollectionDetectionTrigger(); - noteCollectionTrigger.onTrue(new InstantCommand(() -> didCollect = true)); + return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } private boolean hasTarget() { - return CAMERA.hasTargets() && !didCollect; + return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/Commands.java b/src/main/java/frc/trigon/robot/commands/Commands.java index 3c95a28..338f4f7 100644 --- a/src/main/java/frc/trigon/robot/commands/Commands.java +++ b/src/main/java/frc/trigon/robot/commands/Commands.java @@ -8,6 +8,7 @@ import frc.trigon.robot.Robot; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.*; +import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.ClimberCommands; @@ -24,7 +25,6 @@ import frc.trigon.robot.subsystems.transporter.TransporterCommands; import frc.trigon.robot.subsystems.transporter.TransporterConstants; import org.littletonrobotics.junction.Logger; -import org.trigon.utilities.ShootingCalculations; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableRotation2d; diff --git a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java index 7e53619..cb747c2 100644 --- a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java +++ b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java @@ -5,8 +5,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.ShootingConstants; +import frc.trigon.robot.misc.ShootingCalculations; import org.littletonrobotics.junction.Logger; -import org.trigon.utilities.ShootingCalculations; import org.trigon.utilities.mirrorable.MirrorableRotation2d; /** diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index edbbf73..cb088f5 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -5,8 +5,8 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; public class CameraConstants { public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); @@ -24,28 +24,30 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(-31.7), 0) ), REAR_MIDDLE_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0, 0, 0.632), - new Rotation3d(0, Units.degreesToRadians(-24), Units.degreesToRadians(180)) + new Translation3d(0, 0, 0.63), + new Rotation3d(0, Units.degreesToRadians(-23.7), Units.degreesToRadians(180)) ); - public static final RobotPoseSource - REAR_LEFT_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, - "Rear Left Camera", - REAR_LEFT_CENTER_TO_CAMERA - ), - REAR_RIGHT_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, - "Rear Right Camera", - REAR_RIGHT_CENTER_TO_CAMERA - ), + public static final AprilTagCamera +// REAR_LEFT_CAMERA = new AprilTagCamera( +// AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, +// "Rear Left Camera", +// REAR_LEFT_CENTER_TO_CAMERA +// ), +// REAR_RIGHT_CAMERA = new AprilTagCamera( +// AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, +// "Rear Right Camera", +// REAR_RIGHT_CENTER_TO_CAMERA +// ), // FRONT_MIDDLE_CAMERA = new RobotPoseSource( // RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, // "Front Middle Camera", // FRONT_MIDDLE_CENTER_TO_CAMERA // ), - REAR_MIDDLE_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, + REAR_MIDDLE_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "Rear Middle Camera", - REAR_MIDDLE_CENTER_TO_CAMERA + REAR_MIDDLE_CENTER_TO_CAMERA, + 0.0004, + 0.001 ); } diff --git a/src/main/java/frc/trigon/robot/constants/CommandConstants.java b/src/main/java/frc/trigon/robot/constants/CommandConstants.java index d2ca3d8..9e5c6ba 100644 --- a/src/main/java/frc/trigon/robot/constants/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CommandConstants.java @@ -25,7 +25,7 @@ public class CommandConstants { private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; private static final double MINIMUM_TRANSLATION_SHIFT_POWER = 0.18, - MINIMUM_ROTATION_SHIFT_POWER = 0.4; + MINIMUM_ROTATION_SHIFT_POWER = 0.6; public static final Command FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopFieldRelativeDriveCommand( @@ -100,7 +100,7 @@ public static double calculateDriveStickAxisValue(double axisValue) { } public static double calculateRotationStickAxisValue(double axisValue) { - return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER); + return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER) / 1.5; } /** @@ -111,7 +111,7 @@ public static double calculateRotationStickAxisValue(double axisValue) { * @return the power to apply to the robot */ public static double calculateShiftModeValue(double minimumPower) { - final double squaredShiftModeValue = DRIVER_CONTROLLER.getRightTriggerAxis(); + final double squaredShiftModeValue = Math.sqrt(DRIVER_CONTROLLER.getRightTriggerAxis()); final double minimumShiftValueCoefficient = 1 - (1 / minimumPower); return 1 - squaredShiftModeValue * minimumShiftValueCoefficient; diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 5e53b53..910a25d 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -18,10 +18,11 @@ public class FieldConstants { FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.21; private static final int SPEAKER_TAG_ID = 7; - private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.82); + private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.84); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), TARGET_DELIVERY_POSITION = new MirrorableTranslation3d(2.5, 7, 0, true); + public static final double MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; public static final MirrorablePose2d IN_FRONT_OF_AMP_POSE = new MirrorablePose2d(1.842, 8.204 - 0.48, Rotation2d.fromDegrees(90), true); private static HashMap fieldLayoutToTagIdToPoseMap() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index a4c1bcb..d481630 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -9,7 +9,7 @@ public class OperatorConstants { private static final int DRIVER_CONTROLLER_PORT = 0; private static final int DRIVER_CONTROLLER_EXPONENT = 1; - private static final double DRIVER_CONTROLLER_DEADBAND = 0; + private static final double DRIVER_CONTROLLER_DEADBAND = 0.05; public static final XboxController DRIVER_CONTROLLER = new XboxController( DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 39e7a1d..29a37bc 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -6,11 +6,11 @@ public class RobotConstants { private static final boolean - IS_SIMULATION = true, + IS_SIMULATION = false, IS_REPLAY = false; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = IS_SIMULATION && Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; public static void init() { RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index d3c284e..f4eb6c7 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,7 +8,7 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 55, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; public static final double CLOSE_SHOT_VELOCITY_METERS_PER_SECOND = 45; @@ -16,5 +16,5 @@ public class ShootingConstants { public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(40); public static final Pose3d ROBOT_RELATIVE_PIVOT_POINT = new Pose3d(-0.025, 0, 0.190, new Rotation3d(0, 0, Math.PI)); - public static final Transform3d PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.122446, 0, -0.046625, new Rotation3d()); + public static final Transform3d PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.1224469, 0, -0.046625, new Rotation3d()); } diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index b7e3996..05bf276 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -1,4 +1,4 @@ -package org.trigon.utilities; +package frc.trigon.robot.misc; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -137,7 +137,7 @@ private TargetShootingState calculateTargetShootingState(Translation3d shootingV final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false); final Rotation2d targetPitch = getPitch(shootingVector); final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()); - + return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index e9658b0..d9a7684 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,5 +1,6 @@ package frc.trigon.robot.misc.objectdetectioncamera; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.RobotHardwareStats; @@ -9,6 +10,8 @@ public class ObjectDetectionCamera extends SubsystemBase { private final ObjectDetectionCameraIO objectDetectionCameraIO; private final String hostname; private double lastVisibleObjectYaw = 0; + private Rotation2d trackedObjectYaw = new Rotation2d(); + private boolean wasVisible = false; public ObjectDetectionCamera(String hostname) { this.hostname = hostname; @@ -21,6 +24,20 @@ public void periodic() { Logger.processInputs(hostname, objectDetectionCameraInputs); } + public void trackObject() { + if (hasTargets() && !wasVisible) { + wasVisible = true; + startTrackingBestObject(); + trackedObjectYaw = calculateTrackedObjectYaw(); + return; + } + if (!hasTargets()) { + wasVisible = false; + return; + } + trackedObjectYaw = calculateTrackedObjectYaw(); + } + public boolean hasTargets() { return objectDetectionCameraInputs.hasTargets; } @@ -32,7 +49,11 @@ public double getBestObjectYaw() { return objectDetectionCameraInputs.bestObjectYaw; } - public double getTrackedObjectYaw() { + public Rotation2d getTrackedObjectYaw() { + return trackedObjectYaw; + } + + private Rotation2d calculateTrackedObjectYaw() { double closestYawDifference = 10000000; double closestYaw = 10000000; for (double currentYaw : objectDetectionCameraInputs.visibleObjectsYaw) { @@ -44,9 +65,9 @@ public double getTrackedObjectYaw() { } if (closestYawDifference != 10000000) { lastVisibleObjectYaw = closestYaw; - return lastVisibleObjectYaw; + return Rotation2d.fromDegrees(lastVisibleObjectYaw); } - return lastVisibleObjectYaw; + return Rotation2d.fromDegrees(lastVisibleObjectYaw); } public void startTrackingBestObject() { @@ -60,4 +81,4 @@ private ObjectDetectionCameraIO generateIO(String hostname) { return new SimulationObjectDetectionCameraIO(hostname); return new PhotonObjectDetectionCameraIO(hostname); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java new file mode 100644 index 0000000..2525d41 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -0,0 +1,212 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.Robot; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; +import org.littletonrobotics.junction.Logger; + +/** + * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. + * An april tag is like a 2D barcode used to find the robot's position on the field. + * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field. + */ +public class AprilTagCamera { + protected final String name; + private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); + private final Transform3d robotCenterToCamera; + private final AprilTagCameraIO aprilTagCameraIO; + private final double + thetaStandardDeviationsExponent, + translationStandardDeviationsExponent; + private double lastUpdatedTimestamp; + private Pose2d robotPose = null; + + /** + * Constructs a new AprilTagCamera. + * + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param thetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP + * @param translationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP + */ + public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationsExponent, double translationStandardDeviationsExponent) { + this.name = name; + this.robotCenterToCamera = robotCenterToCamera; + this.thetaStandardDeviationsExponent = thetaStandardDeviationsExponent; + this.translationStandardDeviationsExponent = translationStandardDeviationsExponent; + + if (Robot.IS_REAL) + aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); + else + aprilTagCameraIO = new AprilTagCameraIO(); + } + + public void update() { + aprilTagCameraIO.updateInputs(inputs); + Logger.processInputs("Cameras/" + name, inputs); + robotPose = calculateBestRobotPose(); + + logEstimatedRobotPose(); + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); + } + + public boolean hasNewResult() { + return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); + } + + public Pose2d getEstimatedRobotPose() { + return robotPose; + } + + public String getName() { + return name; + } + + public double getLatestResultTimestampSeconds() { + return inputs.latestResultTimestampSeconds; + } + + public void setGyroHeadingToSolvePNPHeading() { + final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d(); + RobotContainer.POSE_ESTIMATOR.resetPose(new Pose2d(currentTranslation, solvePNPHeading)); + } + + /** + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. + * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. + * + * @return the standard deviations for the pose estimation strategy used + */ + public Matrix calculateStandardDeviations() { + final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = isWithinBestTagRangeForSolvePNP() ? calculateStandardDeviations(thetaStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length) : Double.POSITIVE_INFINITY; + + return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); + } + + /** + * If the robot is close enough to the tag, it calculates the pose using the solve PNP heading. + * If it's too far, it assumes the robot's heading from the gyro and calculates its position from there. + * Assuming the robot's heading from the gyro is more robust, but won't fix current wrong heading. + * To fix this, we use solve PNP to reset the gyro when we are close enough for an accurate result. + * + * @return the robot's pose + */ + private Pose2d calculateBestRobotPose() { + final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); + } + + /** + * Calculates the robot's pose by assuming its heading, the apriltag's position, and the camera's position on the robot. + * This method of pose estimation is more robust than solve PNP, but relies on knowing the robot's heading beforehand. + * + * @return the robot's pose + */ + private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { + if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult) + return null; + + final Translation2d fieldRelativeRobotTranslation = getFieldRelativeRobotTranslation(gyroHeading); + + if (!isWithinBestTagRangeForSolvePNP()) + return new Pose2d(fieldRelativeRobotTranslation, gyroHeading); + + final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + return new Pose2d(fieldRelativeRobotTranslation, solvePNPHeading); + } + + private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); + + final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); + final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); + final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(gyroHeading); + return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); + } + + private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { + final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); + final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); + final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); + return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); + } + + private double getRobotPlaneTargetYawRadians() { + double targetYawRadians = -inputs.bestTargetRelativeYawRadians; + for (int i = 0; i < AprilTagCameraConstants.CALCULATE_YAW_ITERATIONS; i++) { + final double projectedPitch = projectToPlane(-robotCenterToCamera.getRotation().getY(), targetYawRadians + Math.PI / 2); + targetYawRadians = -inputs.bestTargetRelativeYawRadians - Math.tan(projectedPitch) * -inputs.bestTargetRelativePitchRadians; + } + return projectToPlane(targetYawRadians, robotCenterToCamera.getRotation().getY()); + } + + private double projectToPlane(double targetAngleRadians, double cameraAngleRadians) { + if (cameraAngleRadians < 0) + return Math.atan(Math.tan(targetAngleRadians) / Math.cos(cameraAngleRadians)); + return Math.atan(Math.tan(targetAngleRadians) * Math.cos(cameraAngleRadians)); + } + + private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { + final double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + final double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); + return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); + } + + private Translation2d getFieldRelativeRobotPose(Translation2d tagRelativeCameraTranslation, Pose3d tagPose) { + return tagPose.getTranslation().toTranslation2d().minus(tagRelativeCameraTranslation); + } + + private boolean isNewTimestamp() { + if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) + return false; + + lastUpdatedTimestamp = getLatestResultTimestampSeconds(); + return true; + } + + /** + * Calculates the standard deviation of the estimated pose using a formula. + * As we get further from the tag(s), this will return a less trusting (higher deviation) result. + * + * @param exponent a calibrated gain, different for each pose estimating strategy + * @param distance the distance from the tag(s) + * @param numberOfVisibleTags the number of visible tags + * @return the standard deviation + */ + private double calculateStandardDeviations(double exponent, double distance, int numberOfVisibleTags) { + return exponent * (distance * distance) / numberOfVisibleTags; + } + + private boolean isWithinBestTagRangeForSolvePNP() { + return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + } + + private void logEstimatedRobotPose() { + if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) + Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); + else + Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); + } + + private void logUsedTags() { + if (!inputs.hasResult) { + Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]); + return; + } + + final Pose3d[] usedTagPoses = isWithinBestTagRangeForSolvePNP() ? new Pose3d[inputs.visibleTagIDs.length] : new Pose3d[1]; + for (int i = 0; i < usedTagPoses.length; i++) + usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); + Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java new file mode 100644 index 0000000..2e9d642 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; + +import java.util.function.Function; + +public class AprilTagCameraConstants { + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0.06, new Rotation3d(0, 0, 0)); + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; + static final int CALCULATE_YAW_ITERATIONS = 3; + static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + + public enum AprilTagCameraType { + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + LIMELIGHT(AprilTagLimelightIO::new); + + final Function createIOFunction; + + AprilTagCameraType(Function createIOFunction) { + this.createIOFunction = createIOFunction; + } + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java new file mode 100644 index 0000000..c9233bb --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose3d; +import org.littletonrobotics.junction.AutoLog; + +public class AprilTagCameraIO { + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + } + + @AutoLog + public static class AprilTagCameraInputs { + public boolean hasResult = false; + public double latestResultTimestampSeconds = 0; + public Pose3d cameraSolvePNPPose = new Pose3d(); + public int[] visibleTagIDs = new int[0]; + public double bestTargetRelativeYawRadians = 0; + public double bestTargetRelativePitchRadians = 0; + public double distanceFromBestTag = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java new file mode 100644 index 0000000..065e827 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -0,0 +1,87 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.trigon.utilities.LimelightHelpers; + +public class AprilTagLimelightIO extends AprilTagCameraIO { + private final String hostname; + + public AprilTagLimelightIO(String hostname) { + this.hostname = hostname; + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); + + inputs.hasResult = results.targets_Fiducials.length > 0; + + if (inputs.hasResult) + updateHasResultInputs(inputs, results); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { + final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); + + inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; + inputs.visibleTagIDs = getVisibleTagIDs(results); + inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); + inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); + inputs.distanceFromBestTag = getDistanceFromBestTag(results); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + final int[] visibleTagIDs = new int[visibleTags.length]; + visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = (int) visibleTags[i].fiducialID; + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; + } + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param results the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); + return new Rotation3d(0, -Units.degreesToRadians(bestTag.tx), -Units.degreesToRadians(bestTag.ty)); + } + + private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) { + return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue()); + } + + private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { + return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); + } + + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) { + return results.targets_Fiducials[0]; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java new file mode 100644 index 0000000..1375f52 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -0,0 +1,124 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.opencv.core.Point; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.TargetCorner; + +import java.util.List; + +public class AprilTagPhotonCameraIO extends AprilTagCameraIO { + private final PhotonCamera photonCamera; + + public AprilTagPhotonCameraIO(String cameraName) { + photonCamera = new PhotonCamera(cameraName); + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); + + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[]{}; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param result the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { + final List tagCorners = result.getBestTarget().getDetectedCorners(); + final Point tagCenter = getTagCenter(tagCorners); + if (photonCamera.getCameraMatrix().isPresent()) + return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + return null; + } + + /** + * Estimates the camera's pose using Solve PNP using as many tags as possible. + * + * @param result the camera's pipeline result + * @return the estimated pose + */ + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + if (result.getMultiTagResult().estimatedPose.isPresent) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + } + + final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); + final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); + final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); + return tagPose.transformBy(targetToCamera); + } + + private int[] getVisibleTagIDs(PhotonPipelineResult result) { + final int[] visibleTagIDs = new int[result.getTargets().size()]; + + for (int i = 0; i < visibleTagIDs.length; i++) + visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); + return visibleTagIDs; + } + + private double getDistanceFromBestTag(PhotonPipelineResult result) { + return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); + } + + private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + double fx = camIntrinsics.get(0, 0); + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - pixel.x; + + double fy = camIntrinsics.get(1, 1); + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - pixel.y; + + // calculate yaw normally + var yaw = new Rotation2d(fx, xOffset); + // correct pitch based on yaw + var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + + return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java deleted file mode 100644 index dcc605b..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java +++ /dev/null @@ -1,72 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.math.geometry.Pose3d; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; - -/** - * An estimated pose based on pipeline result - */ -public class EstimatedRobotPose { - /** - * The estimated pose - */ - public final Pose3d estimatedPose; - - /** - * The estimated time the frame used to derive the robot pose was taken - */ - public final double timestampSeconds; - - /** - * A list of the targets used to compute this pose - */ - public final List targetsUsed; - - /** - * The strategy actually used to produce this pose - */ - public final PhotonPoseEstimator.PoseStrategy strategy; - - /** - * Constructs an EstimatedRobotPose - * - * @param estimatedPose estimated pose - * @param timestampSeconds timestamp of the estimate - */ - public EstimatedRobotPose( - Pose3d estimatedPose, - double timestampSeconds, - List targetsUsed, - PhotonPoseEstimator.PoseStrategy strategy) { - this.estimatedPose = estimatedPose; - this.timestampSeconds = timestampSeconds; - this.targetsUsed = targetsUsed; - this.strategy = strategy; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java deleted file mode 100644 index 9c932c2..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java +++ /dev/null @@ -1,791 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.wpilibj.DriverStation; -import frc.trigon.robot.RobotContainer; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; - -/** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set - * below. Example usage can be found in our apriltagExample example project. - */ -public class PhotonPoseEstimator { - private static int InstanceCount = 0; - - /** - * Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. - */ - public enum PoseStrategy { - /** - * Choose the Pose with the lowest ambiguity. - */ - LOWEST_AMBIGUITY, - - /** - * Choose the Pose which is closest to the camera height. - */ - CLOSEST_TO_CAMERA_HEIGHT, - - /** - * Choose the Pose which is closest to a set Reference position. - */ - CLOSEST_TO_REFERENCE_POSE, - - /** - * Choose the Pose which is closest to the last pose calculated - */ - CLOSEST_TO_LAST_POSE, - - /** - * Return the average of the best target poses using ambiguity as weight. - */ - AVERAGE_BEST_TARGETS, - - /** - * Use all visible tags to compute a single pose estimate on coprocessor. This option needs to - * be enabled on the PhotonVision web UI as well. - */ - MULTI_TAG_PNP_ON_COPROCESSOR, - - /** - * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can - * take a lot of time. - */ - MULTI_TAG_PNP_ON_RIO, - - CLOSEST_TO_HEADING - } - - private AprilTagFieldLayout fieldTags; - private TargetModel tagModel = TargetModel.kAprilTag16h5; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private final PhotonCamera camera; - private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; - private final Set reportedErrors = new HashSet<>(); - - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot -> camera) in the Robot - * Coordinate System. - */ - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, - PoseStrategy strategy, - PhotonCamera camera, - Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = camera; - this.robotToCamera = robotToCamera; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this(fieldTags, strategy, null, robotToCamera); - } - - /** - * Invalidates the pose cache. - */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { - invalidatePoseCache(); - } - } - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @return the AprilTagFieldLayout - */ - public AprilTagFieldLayout getFieldTags() { - return fieldTags; - } - - /** - * Set the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @param fieldTags the AprilTagFieldLayout - */ - public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); - this.fieldTags = fieldTags; - } - - /** - * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - *

By default, this is {@link TargetModel#kAprilTag16h5}. - */ - public TargetModel getTagModel() { - return tagModel; - } - - /** - * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. - */ - public void setTagModel(TargetModel tagModel) { - this.tagModel = tagModel; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - * NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - */ - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { - DriverStation.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); - strategy = PoseStrategy.LOWEST_AMBIGUITY; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns - * empty if: - * - *

    - *
  • New data has not been received since the last call to {@code update()}. - *
  • No targets were found from the camera - *
  • There is no camera set - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update() { - if (camera == null) { - DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false); - return Optional.empty(); - } - - PhotonPipelineResult cameraResult = camera.getLatestResult(); - - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult) { - if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty()); - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result -// - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - PoseStrategy strat) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - estimatedPose = closestToCameraHeightStrategy(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case CLOSEST_TO_LAST_POSE: - setReferencePose(lastPose); - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case AVERAGE_BEST_TARGETS: - estimatedPose = averageBestTargetsStrategy(cameraResult); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case CLOSEST_TO_HEADING: - estimatedPose = closestToHeadingStrategy(cameraResult); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - - private Optional closestToHeadingStrategy(PhotonPipelineResult result) { - double smallestAngleDifferenceRadians; - EstimatedRobotPose closestAngleTarget; - double currentHeadingRadians = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getRadians(); - - PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - return Optional.empty(); - } - - Transform3d bestCameraToTarget = target.getBestCameraToTarget(); - - Pose3d bestPose = - targetPosition - .get() - .transformBy(bestCameraToTarget.inverse()) - .transformBy(robotToCamera.inverse()); - double bestTransformAngle = bestPose.getRotation().getZ(); - smallestAngleDifferenceRadians = Math.abs(currentHeadingRadians - bestTransformAngle); - closestAngleTarget = - new EstimatedRobotPose( - bestPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - - Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - if (alternateCameraToTarget.getRotation().getZ() != 0) { - Pose3d altPose = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - double alternateTransformAngle = altPose.getRotation().getZ(); - double alternateTransformDelta = Math.abs(currentHeadingRadians - alternateTransformAngle); - - if (alternateTransformDelta < smallestAngleDifferenceRadians) { - closestAngleTarget = - new EstimatedRobotPose( - altPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.of(closestAngleTarget); - } - - private Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - var best_tf = result.getMultiTagResult().estimatedPose.best; - var cam = new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()); - var best = cam.plus(robotToCamera.inverse()); // field-to-robot -// Logger.recordOutput(camera.getName(), Math.toDegrees(cam.getRotation().getY())); - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - } - - private Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent) - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - var best = - new Pose3d() - .plus(pnpResult.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = fieldTags.getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - return Optional.of( - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY)); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta height - * difference between the estimated and actual height of the camera. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { - double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - - - double alternateTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - - if (bestTransformDelta < smallestHeightDifference) { - smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta in the vector - * magnitude between it and the reference pose. - * - * @param result pipeline result - * @param referencePose reference pose to check vector magnitude difference against. - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToReferencePoseStrategy( - PhotonPipelineResult result, Pose3d referencePose) { - if (referencePose == null) { -// DriverStation.reportError( -// "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!", -// false); - return Optional.empty(); - } - - double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - Pose3d altTransformPosition = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - - double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); - double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); - - if (altDifference < smallestPoseDelta) { - smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose( - altTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - if (bestDifference < smallestPoseDelta) { - smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose( - bestTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - } - return Optional.ofNullable(lowestDeltaPose); - } - - /** - * Return the average of the best target poses using ambiguity as weight. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { - if (!result.hasTargets()) - return Optional.empty(); - final PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - final Pose3d cameraPose = targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()); -// Logger.recordOutput(camera.getName(), Math.toDegrees(cameraPose.getRotation().getY())); - final Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse()); - - return Optional.of( - new EstimatedRobotPose( - robotPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.AVERAGE_BEST_TARGETS - ) - ); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 1cc1dee..13c12f3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -1,19 +1,15 @@ package frc.trigon.robot.poseestimation.poseestimator; import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; 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.kinematics.SwerveDriveWheelPositions; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -26,16 +22,16 @@ */ public class PoseEstimator implements AutoCloseable { private final Field2d field = new Field2d(); - private final RobotPoseSource[] robotPoseSources; + private final AprilTagCamera[] aprilTagCameras; private final PoseEstimator6328 poseEstimator6328 = PoseEstimator6328.getInstance(); /** * Constructs a new PoseEstimator. * - * @param robotPoseSources the sources that should update the pose estimator apart from the odometry. This should be cameras etc. + * @param aprilTagCameras the sources that should update the pose estimator apart from the odometry. This should be cameras etc. */ - public PoseEstimator(RobotPoseSource... robotPoseSources) { - this.robotPoseSources = robotPoseSources; + public PoseEstimator(AprilTagCamera... aprilTagCameras) { + this.aprilTagCameras = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); PathPlannerLogging.setLogActivePathCallback((pose) -> { @@ -92,38 +88,31 @@ private void updateFromVision() { private List getViableVisionObservations() { List viableVisionObservations = new ArrayList<>(); - for (RobotPoseSource robotPoseSource : robotPoseSources) { - final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(robotPoseSource); + for (AprilTagCamera aprilTagCamera : aprilTagCameras) { + final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(aprilTagCamera); if (visionObservation != null) viableVisionObservations.add(visionObservation); } return viableVisionObservations; } - private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource) { - robotPoseSource.update(); - if (!robotPoseSource.hasNewResult()) + private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera aprilTagCamera) { + aprilTagCamera.update(); + if (!aprilTagCamera.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getRobotPose(); + final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); if (robotPose == null) return null; return new PoseEstimator6328.VisionObservation( robotPose, - robotPoseSource.getLastResultTimestamp(), - averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getVisibleTags()) + aprilTagCamera.getLatestResultTimestampSeconds(), + aprilTagCamera.calculateStandardDeviations() ); } - private Matrix averageDistanceToStdDevs(double averageDistance, int visibleTags) { - final double translationStd = PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT * Math.pow(averageDistance, 2) / (visibleTags * visibleTags); - final double thetaStd = PoseEstimatorConstants.THETA_STD_EXPONENT * Math.pow(averageDistance, 2) / visibleTags; - - return VecBuilder.fill(translationStd, translationStd, thetaStd); - } - private void putAprilTagsOnFieldWidget() { - for (Map.Entry entry : RobotPoseSourceConstants.TAG_ID_TO_POSE.entrySet()) { + for (Map.Entry entry : FieldConstants.TAG_ID_TO_POSE.entrySet()) { final Pose2d tagPose = entry.getValue().toPose2d(); field.getObject("Tag " + entry.getKey()).setPose(tagPose); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 6819d8e..e2cb1b5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -145,6 +145,13 @@ public void addVisionObservation(VisionObservation observation) { estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } + public Pose2d samplePose(double timestamp) { + Pose2d sample = poseBuffer.getSample(timestamp).orElse(new Pose2d()); + Transform2d odometryToSampleTransform = new Transform2d(odometryPose, sample); + + return estimatedPose.plus(odometryToSampleTransform); + } + /** * Reset estimated pose and odometry pose to pose
* Clear pose buffer diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index a87aa90..72d7479 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -14,9 +14,5 @@ public class PoseEstimatorConstants { * Increase these numbers to trust the estimate less. */ static final Vector ODOMETRY_AMBIGUITY = VecBuilder.fill(0.003, 0.003, 0.0002); - - static final double - TRANSLATIONS_STD_EXPONENT = 0.005, - THETA_STD_EXPONENT = 0.01; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java deleted file mode 100644 index aecd3b2..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import org.trigon.utilities.LimelightHelpers; - -public class AprilTagLimelightIO extends RobotPoseSourceIO { - private final String hostname; - - protected AprilTagLimelightIO(String hostname) { - this.hostname = hostname; - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = LimelightHelpers.getTV(hostname); - if (inputs.hasResult) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - inputs.cameraPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java deleted file mode 100644 index 3e40add..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ /dev/null @@ -1,91 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.trigon.robot.poseestimation.photonposeestimator.EstimatedRobotPose; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; -import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; -import java.util.Optional; - -public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { - private final PhotonCamera photonCamera; - private final PhotonPoseEstimator photonPoseEstimator; - - protected AprilTagPhotonCameraIO(String cameraName, Transform3d robotCenterToCamera) { - photonCamera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - RobotPoseSourceConstants.APRIL_TAG_FIELD_LAYOUT, - RobotPoseSourceConstants.PRIMARY_POSE_STRATEGY, - photonCamera, - robotCenterToCamera - ); - - photonPoseEstimator.setMultiTagFallbackStrategy(RobotPoseSourceConstants.SECONDARY_POSE_STRATEGY); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - Optional optionalEstimatedRobotPose = photonPoseEstimator.update(latestResult); - - inputs.hasResult = hasResult(optionalEstimatedRobotPose); - if (inputs.hasResult) { - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - inputs.cameraPose = estimatedRobotPose.estimatedPose; - inputs.lastResultTimestamp = estimatedRobotPose.timestampSeconds; - inputs.visibleTags = estimatedRobotPose.targetsUsed.size(); - inputs.averageDistanceFromTags = getAverageDistanceFromTags(latestResult); - } else { - inputs.visibleTags = 0; - inputs.cameraPose = new Pose3d(); - } - - logVisibleTags(inputs.hasResult, optionalEstimatedRobotPose); - } - - private void logVisibleTags(boolean hasResult, Optional optionalEstimatedRobotPose) { - if (!hasResult) { - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); - return; - } - - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - final Pose2d[] visibleTagPoses = new Pose2d[estimatedRobotPose.targetsUsed.size()]; - for (int i = 0; i < visibleTagPoses.length; i++) { - final int currentId = estimatedRobotPose.targetsUsed.get(i).getFiducialId(); - final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); - visibleTagPoses[i] = currentPose; - } - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); - } - - private boolean hasResult(Optional optionalEstimatedRobotPose) { - final boolean isEmpty = optionalEstimatedRobotPose.isEmpty(); - if (isEmpty) - return false; - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - if (estimatedRobotPose.strategy == PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) - return true; - return estimatedRobotPose.targetsUsed.get(0).getPoseAmbiguity() < RobotPoseSourceConstants.MAXIMUM_AMBIGUITY; - } - - private double getAverageDistanceFromTags(PhotonPipelineResult result) { - final List targets = result.targets; - double distanceSum = 0; - - for (PhotonTrackedTarget currentTarget : targets) { - final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); - distanceSum += distanceTranslation.getNorm(); - } - - return distanceSum / targets.size(); - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java deleted file mode 100644 index 28a3fbd..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ /dev/null @@ -1,82 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.Robot; -import org.littletonrobotics.junction.Logger; - -/** - * A pose source is a class that provides the robot's pose, from a camera. - */ -public class RobotPoseSource { - protected final String name; - private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); - private final Transform3d robotCenterToCamera; - private final RobotPoseSourceIO robotPoseSourceIO; - private double lastUpdatedTimestamp; - private Pose2d cachedPose = null; - - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera) { - this.name = name; - if (robotPoseSourceType != RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA) - this.robotCenterToCamera = robotCenterToCamera; - else - this.robotCenterToCamera = new Transform3d(); - - if (Robot.IS_REAL) - robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name, robotCenterToCamera); - else - robotPoseSourceIO = new RobotPoseSourceIO(); - } - - public void update() { - robotPoseSourceIO.updateInputs(inputs); - Logger.processInputs("Cameras/" + name, inputs); - cachedPose = getUnCachedRobotPose(); - if (!inputs.hasResult || inputs.averageDistanceFromTags == 0 || cachedPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); - else - Logger.recordOutput("Poses/Robot/" + name + "Pose", cachedPose); - } - - public int getVisibleTags() { - return inputs.visibleTags; - } - - public double getAverageDistanceFromTags() { - return inputs.averageDistanceFromTags; - } - - public boolean hasNewResult() { - return (inputs.hasResult && inputs.averageDistanceFromTags != 0) && isNewTimestamp(); - } - - public Pose2d getRobotPose() { - return cachedPose; - } - - public String getName() { - return name; - } - - public double getLastResultTimestamp() { - return inputs.lastResultTimestamp; - } - - private Pose2d getUnCachedRobotPose() { - final Pose3d cameraPose = inputs.cameraPose; - if (cameraPose == null) - return null; - - return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); - } - - private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLastResultTimestamp()) - return false; - - lastUpdatedTimestamp = getLastResultTimestamp(); - return true; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java deleted file mode 100644 index 8303aae..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; - -import java.util.HashMap; -import java.util.function.BiFunction; - -public class RobotPoseSourceConstants { - public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); - static final PhotonPoseEstimator.PoseStrategy - PRIMARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - SECONDARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_HEADING; - static AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - static { - for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - TAG_ID_TO_POSE.put(aprilTag.ID, aprilTag.pose); - } - - static final double MAXIMUM_AMBIGUITY = 0.2; - static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; - - public enum RobotPoseSourceType { - PHOTON_CAMERA(AprilTagPhotonCameraIO::new), - LIMELIGHT((name, transform3d) -> new AprilTagLimelightIO(name)), - T265((name, transform3d) -> new T265IO(name)); - - final BiFunction createIOFunction; - - RobotPoseSourceType(BiFunction createIOFunction) { - this.createIOFunction = createIOFunction; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java deleted file mode 100644 index a37f35d..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose3d; -import org.littletonrobotics.junction.AutoLog; - -public class RobotPoseSourceIO { - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - } - - @AutoLog - public static class RobotPoseSourceInputs { - public boolean hasResult = false; - public double lastResultTimestamp = 0; - public Pose3d cameraPose = new Pose3d(); - public double averageDistanceFromTags = 0; - public int visibleTags = 0; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java deleted file mode 100644 index 8bc6ea0..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import org.trigon.utilities.JsonHandler; - -public class T265IO extends RobotPoseSourceIO { - private static final NetworkTable NETWORK_TABLE = NetworkTableInstance.getDefault().getTable("T265"); - private static final short CONFIDENCE_THRESHOLD = 2; - private final NetworkTableEntry jsonDump; - - protected T265IO(String name) { - jsonDump = NETWORK_TABLE.getEntry(name + "/jsonDump"); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = canUseJsonDump(); - if (inputs.hasResult) - inputs.cameraPose = getCameraPose(); - inputs.lastResultTimestamp = (double) jsonDump.getLastChange() / 1000000; - } - - private Pose3d getCameraPose() { - if (!canUseJsonDump()) - return null; - - return getRobotPoseFromJsonDump(); - } - - private Pose3d getRobotPoseFromJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - final Translation3d translation = getTranslationFromDoubleArray(jsonDump.translation); - final Rotation3d rotation = getRotationFromDoubleArray(jsonDump.rotation); - - return t265PoseToWPIPose(new Pose3d(translation, rotation)); - } - - private Pose3d t265PoseToWPIPose(Pose3d t265Pose) { - final CoordinateSystem eusCoordinateSystem = new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.U(), CoordinateAxis.S()); - final Pose3d convertedPose = CoordinateSystem.convert(t265Pose, eusCoordinateSystem, CoordinateSystem.NWU()); - final Rotation3d convertedRotation = convertedPose.getRotation().plus(new Rotation3d(0, 0, Math.toRadians(90))); - - return new Pose3d(convertedPose.getTranslation(), convertedRotation); - } - - private Translation3d getTranslationFromDoubleArray(double[] xyz) { - return new Translation3d(xyz[0], xyz[1], xyz[2]); - } - - private Rotation3d getRotationFromDoubleArray(double[] wxyz) { - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } - - private boolean canUseJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - - try { - return jsonDump.confidence >= CONFIDENCE_THRESHOLD && jsonDump.translation.length == 3 && jsonDump.rotation.length == 4; - } catch (NullPointerException e) { - return false; - } - } - - private T265JsonDump getJsonDump() { - return JsonHandler.parseJsonStringToObject(jsonDump.getString(""), T265JsonDump.class); - } - - private static class T265JsonDump { - private double[] translation; - private double[] rotation; - private int confidence; - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 3c2b13f..abe8bed 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -10,6 +10,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.Commands; import frc.trigon.robot.constants.CommandConstants; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.trigon.hardware.RobotHardwareStats; import java.util.ArrayList; import java.util.List; @@ -24,6 +26,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase { private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); + private static final LoggedDashboardBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedDashboardBoolean("EnableExtensiveLogging", true); static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); @@ -59,6 +62,22 @@ public static void setAllSubsystemsBrakeAsync(boolean brake) { CompletableFuture.runAsync(() -> forEach((subsystem) -> subsystem.setBrake(brake))); } + public static boolean isExtensiveLoggingEnabled() { + return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay(); + } + + /** + * Runs periodically, to update the subsystem, and update the mechanism of the subsystem (if there is one). + * This only updates the mechanism if the robot is in replay mode or extensive logging is enabled. + * This function cannot be overridden. Use {@linkplain MotorSubsystem#updatePeriodically} or {@linkplain MotorSubsystem#updateMechanism} (depending on the usage) instead. + */ + @Override + public final void periodic() { + updatePeriodically(); + if (isExtensiveLoggingEnabled()) + updateMechanism(); + } + /** * Creates a quasistatic (ramp up) command for characterizing the subsystem's mechanism. * @@ -110,6 +129,19 @@ public void drive(Measure voltageMeasure) { public void updateLog(SysIdRoutineLog log) { } + /** + * Updates the mechanism of the subsystem periodically if the robot is in replay mode, or if {@linkplain MotorSubsystem#ENABLE_EXTENSIVE_LOGGING) is true. + * This doesn't always run in order to save resources. + */ + public void updateMechanism() { + } + + /** + * Updates periodically. Anything that should be updated periodically but isn't related to the mechanism (or shouldn't always be logged, in order to save resources) should be put here. + */ + public void updatePeriodically() { + } + public SysIdRoutine.Config getSysIdConfig() { return null; } @@ -137,4 +169,4 @@ private SysIdRoutine createSysIdRoutine() { ) ); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 561dd23..17cc03c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -48,12 +48,21 @@ public Climber() { } @Override - public void periodic() { + public void updatePeriodically() { masterMotor.update(); ClimberConstants.LIMIT_SWITCH.updateSensor(); updateNetworkTables(); } + @Override + public void updateMechanism() { + ClimberConstants.MECHANISM.update( + getPositionMeters(), + toMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + ); + Logger.recordOutput("Poses/Components/ClimberPose", getClimberPose()); + } + @Override public void drive(Measure voltageMeasure) { masterMotor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); @@ -109,19 +118,10 @@ void setTargetPosition(double targetPositionMeters, boolean affectedByWeight) { } private void updateNetworkTables() { - updateMechanisms(); Logger.recordOutput("Climber/PositionMeters", getPositionMeters()); Logger.recordOutput("Climber/VelocityMeters", toMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY))); } - private void updateMechanisms() { - ClimberConstants.MECHANISM.update( - getPositionMeters(), - toMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) - ); - Logger.recordOutput("Poses/Components/ClimberPose", getClimberPose()); - } - private Pose3d getClimberPose() { final Transform3d climberTransform = new Transform3d( new Translation3d(0, 0, getPositionMeters()), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 3fbb277..f8e880e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.utilities.Conversions; @@ -23,7 +24,7 @@ public class Elevator extends MotorSubsystem { 0, ElevatorConstants.MOTION_MAGIC_CRUISE_VELOCITY, ElevatorConstants.MOTION_MAGIC_ACCELERATION, - 0 + ElevatorConstants.MOTION_MAGIC_JERK ).withUpdateFreqHz(1000).withEnableFOC(ElevatorConstants.FOC_ENABLED); private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ElevatorConstants.FOC_ENABLED).withUpdateFreqHz(1000); private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.RESTING; @@ -34,16 +35,24 @@ public Elevator() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); + ElevatorConstants.ENCODER.update(); updateNetworkTables(); } + @Override + public void updateMechanism() { + Logger.recordOutput("Poses/Components/ElevatorPose", getElevatorComponentPose()); + Logger.recordOutput("Poses/Components/TransporterPose", getTransporterComponentPose()); + ElevatorConstants.MECHANISM.update(getPositionMeters(), toMeters(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); + } + @Override public void updateLog(SysIdRoutineLog log) { log.motor("Elevator") - .linearPosition(Units.Meters.of(motor.getSignal(TalonFXSignal.POSITION))) - .linearVelocity(Units.MetersPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY))) + .angularPosition(Units.Rotations.of(getEncoderPosition())) + .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ElevatorConstants.GEAR_RATIO)) .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @@ -55,6 +64,7 @@ public SysIdRoutine.Config getSysIdConfig() { @Override public void setBrake(boolean brake) { motor.setBrake(brake); + ElevatorConstants.FOLLOWER_MOTOR.setBrake(brake); } @Override @@ -67,6 +77,14 @@ public void drive(Measure voltageMeasure) { motor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); } + public double getRotorPosition() { + return motor.getSignal(TalonFXSignal.ROTOR_POSITION); + } + + public double getEncoderPosition() { + return ElevatorConstants.ENCODER.getSignal(CANcoderSignal.POSITION); + } + public boolean atTargetState() { return Math.abs(this.targetState.positionMeters - getPositionMeters()) < ElevatorConstants.TOLERANCE_METERS; } @@ -105,7 +123,6 @@ void setTargetPosition(double targetPositionMeters, double speedPercentage) { } private void updateNetworkTables() { - updateMechanism(); Logger.recordOutput("Elevator/ElevatorPositionMeters", getPositionMeters()); Logger.recordOutput("Elevator/ElevatorVelocityMetersPerSecond", toMeters(motor.getSignal(TalonFXSignal.VELOCITY))); } @@ -114,12 +131,6 @@ private DynamicMotionMagicVoltage scaleProfile(DynamicMotionMagicVoltage profile return profile.withVelocity(ElevatorConstants.MOTION_MAGIC_CRUISE_VELOCITY * (speedPercentage / 100)).withAcceleration(ElevatorConstants.MOTION_MAGIC_ACCELERATION * (speedPercentage / 100)); } - private void updateMechanism() { - Logger.recordOutput("Poses/Components/ElevatorPose", getElevatorComponentPose()); - Logger.recordOutput("Poses/Components/TransporterPose", getTransporterComponentPose()); - ElevatorConstants.MECHANISM.update(getPositionMeters(), toMeters(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); - } - private Pose3d getElevatorComponentPose() { final Transform3d elevatorTransform = new Transform3d( new Translation3d(0, 0, getPositionMeters()), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index e18b261..c342faf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -1,8 +1,10 @@ package frc.trigon.robot.subsystems.elevator; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import org.trigon.commands.GearRatioCalculationCommand; import org.trigon.commands.NetworkTablesCommand; public class ElevatorCommands { @@ -14,6 +16,15 @@ public static Command getDebuggingCommand() { ); } + public static Command getGearRatioCalibrationCommand() { + return new GearRatioCalculationCommand( + RobotContainer.ELEVATOR::getRotorPosition, + RobotContainer.ELEVATOR::getEncoderPosition, + (voltage) -> RobotContainer.ELEVATOR.drive(Units.Volt.of(voltage)), + RobotContainer.ELEVATOR + ); + } + public static Command getSetTargetPositionCommand(double targetPositionMeters) { return new StartEndCommand( () -> RobotContainer.ELEVATOR.setTargetPosition(targetPositionMeters, 100), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 50b7ee0..791fabd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -14,6 +14,7 @@ import frc.trigon.robot.constants.RobotConstants; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.ElevatorSimulation; @@ -40,20 +41,21 @@ public class ElevatorConstants { private static final boolean FOLLOWER_MOTOR_OPPOSITE_DIRECTION = true; private static final AbsoluteSensorRangeValue ENCODER_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Unsigned_0To1; private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.719726; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.711181640625; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; private static final double - P = RobotHardwareStats.isSimulation() ? 52 : 2.5, + P = RobotHardwareStats.isSimulation() ? 52 : 1.5, I = 0, D = 0, - KS = RobotHardwareStats.isSimulation() ? 0.019539 : 0.036646 + 0.0798, - KV = RobotHardwareStats.isSimulation() ? 0.987 : 0.44458, - KG = RobotHardwareStats.isSimulation() ? 0.12551 : 0.4, - KA = RobotHardwareStats.isSimulation() ? 0.017514 : 0.026516; + KS = RobotHardwareStats.isSimulation() ? 0.019539 : 0.02, + KV = RobotHardwareStats.isSimulation() ? 0.987 : 0.415, + KG = RobotHardwareStats.isSimulation() ? 0.12551 : 0.37, + KA = RobotHardwareStats.isSimulation() ? 0.017514 : 0.01; static final double - MOTION_MAGIC_CRUISE_VELOCITY = 25, - MOTION_MAGIC_ACCELERATION = 25; - private static final double GEAR_RATIO = 3.44; + MOTION_MAGIC_CRUISE_VELOCITY = 23, + MOTION_MAGIC_ACCELERATION = 30, + MOTION_MAGIC_JERK = MOTION_MAGIC_ACCELERATION * 7; + static final double GEAR_RATIO = 3.2; static final boolean FOC_ENABLED = true; private static final double @@ -67,7 +69,7 @@ public class ElevatorConstants { static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(0.25).per(Units.Second.of(1)), - Units.Volts.of(2), + Units.Volts.of(1), Units.Second.of(1000) ); @@ -87,9 +89,23 @@ public class ElevatorConstants { CAMERA_PLATE_HEIGHT_METERS = 0.190193; static { + configureEncoder(); configureMasterMotor(); configureFollowerMotor(); - configureEncoder(); + } + + private static void configureEncoder() { + final CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.AbsoluteSensorRange = ENCODER_SENSOR_RANGE_VALUE; + config.MagnetSensor.SensorDirection = ENCODER_SENSOR_DIRECTION_VALUE; + config.MagnetSensor.MagnetOffset = ENCODER_MAGNET_OFFSET_VALUE; + + ENCODER.applyConfiguration(config); + ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); + + ENCODER.registerSignal(CANcoderSignal.POSITION, 100); + ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); } private static void configureMasterMotor() { @@ -116,17 +132,20 @@ private static void configureMasterMotor() { config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = -0.01; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 3.5; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 4.440674; config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; MASTER_MOTOR.applyConfiguration(config); MASTER_MOTOR.setPhysicsSimulation(SIMULATION); MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); @@ -146,19 +165,8 @@ private static void configureFollowerMotor() { FOLLOWER_MOTOR.setControl(new Follower(MASTER_MOTOR_ID, FOLLOWER_MOTOR_OPPOSITE_DIRECTION)); } - private static void configureEncoder() { - final CANcoderConfiguration config = new CANcoderConfiguration(); - - config.MagnetSensor.AbsoluteSensorRange = ENCODER_SENSOR_RANGE_VALUE; - config.MagnetSensor.SensorDirection = ENCODER_SENSOR_DIRECTION_VALUE; - config.MagnetSensor.MagnetOffset = ENCODER_MAGNET_OFFSET_VALUE; - - ENCODER.applyConfiguration(config); - ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); - } - public enum ElevatorState { - RESTING(0, 100), + RESTING(-0.005, 100), FEEDING_FOR_CLOSE_SHOT(0, 100), SCORE_AMP(0.45, 100), SCORE_TRAP(0.5, 10), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 4e340f0..62ca9f6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.intake; import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.subsystems.MotorSubsystem; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -15,9 +14,13 @@ public Intake() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); - updateMechanism(); + } + + @Override + public void updateMechanism() { + IntakeConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); } @Override @@ -35,11 +38,7 @@ void setTargetVoltage(double collectionVoltage) { IntakeConstants.MECHANISM.setTargetVelocity(collectionVoltage); } - public Trigger getEarlyNoteCollectionDetectionTrigger() { - return new Trigger(() -> motor.getSignal(TalonFXSignal.SUPPLY_CURRENT) > IntakeConstants.NOTE_COLLECTION_CURRENT).debounce(IntakeConstants.NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); - } - - private void updateMechanism() { - IntakeConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); + public boolean isEarlyNoteCollectionDetected() { + return IntakeConstants.EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index a348cd4..b3fc25c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.robot.constants.RobotConstants; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -38,9 +40,13 @@ public class IntakeConstants { "IntakeMechanism", MAX_DISPLAYABLE_VELOCITY ); - static final double + private static final double NOTE_COLLECTION_CURRENT = 34, NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.15; + static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + () -> Math.abs(MOTOR.getSignal(TalonFXSignal.SUPPLY_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT + ).debounce(NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); static { configureMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index d9b684c..fa6765c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -1,6 +1,6 @@ package frc.trigon.robot.subsystems.pitcher; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.*; import edu.wpi.first.units.Measure; @@ -8,17 +8,18 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import org.trigon.utilities.ShootingCalculations; public class Pitcher extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); private final TalonFXMotor motor = PitcherConstants.MOTOR; - private final MotionMagicExpoVoltage motionMagicPositionRequest = new MotionMagicExpoVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED).withUpdateFreqHz(1000); + private final MotionMagicVoltage motionMagicPositionRequest = new MotionMagicVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED).withUpdateFreqHz(1000); private final VoltageOut voltageRequest = new VoltageOut(0); private Rotation2d targetPitch = null; @@ -32,27 +33,28 @@ public void stop() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); - updateMechanism(); + PitcherConstants.ENCODER.update(); } @Override - public void drive(Measure voltageMeasure) { - motor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); + public void updateMechanism() { + PitcherConstants.MECHANISM.update(getCurrentPitch(), Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); + Logger.recordOutput("Poses/Components/PitcherPose", getPitcherComponentPose()); } @Override - public void setBrake(boolean brake) { - motor.setBrake(brake); + public void drive(Measure voltageMeasure) { + motor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); } @Override public void updateLog(SysIdRoutineLog log) { log.motor("Pitcher") - .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.TORQUE_CURRENT))) - .angularPosition(Units.Rotations.of(motor.getSignal(TalonFXSignal.POSITION))) - .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY))); + .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))) + .angularPosition(Units.Rotations.of(PitcherConstants.ENCODER.getSignal(CANcoderSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / PitcherConstants.GEAR_RATIO)); } @Override @@ -86,11 +88,6 @@ void setTargetPitch(Rotation2d targetPitch) { this.targetPitch = targetPitch; } - private void updateMechanism() { - PitcherConstants.MECHANISM.update(getCurrentPitch(), Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); - Logger.recordOutput("Poses/Components/PitcherPose", getPitcherComponentPose()); - } - private Pose3d getPitcherComponentPose() { final Transform3d pitcherTransform = new Transform3d( new Translation3d(), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index ae3ae84..f7e6f24 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -39,24 +39,26 @@ public class PitcherConstants { RobotConstants.CANIVORE_NAME ); - private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final SensorDirectionValue SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; private static final AbsoluteSensorRangeValue ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; + private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; private static final double OFFSET = Conversions.degreesToRotations(11.337891 + 90); private static final double - MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 90, + MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 300, MOTION_MAGIC_I = 0, - MOTION_MAGIC_D = RobotHardwareStats.isSimulation() ? 0 : 45, - KS = RobotHardwareStats.isSimulation() ? 0.053988 : 1.4, - KV = RobotHardwareStats.isSimulation() ? 41 : 0, - KA = RobotHardwareStats.isSimulation() ? 0.85062 : 9.2523, - KG = RobotHardwareStats.isSimulation() ? 0.04366 : 1.2, - EXPO_KV = RobotHardwareStats.isSimulation() ? KV : 38.757, - EXPO_KA = RobotHardwareStats.isSimulation() ? KA : 0.6; + MOTION_MAGIC_D = RobotHardwareStats.isSimulation() ? 0 : 0, + KS = RobotHardwareStats.isSimulation() ? 1.0346 : 0.11847, + KV = RobotHardwareStats.isSimulation() ? 41 : 42, + KA = RobotHardwareStats.isSimulation() ? 0.85062 : 0.98195, + KG = RobotHardwareStats.isSimulation() ? 0.04366 : 0.10022; + private static final double + MOTION_MAGIC_CRUISE_VELOCITY = 0.31, + MOTION_MAGIC_ACCELERATION = 11, + MOTION_MAGIC_JERK = 110; static final boolean FOC_ENABLED = true; - private static final double GEAR_RATIO = 352.8; + static final double GEAR_RATIO = 352.8; private static final int MOTOR_AMOUNT = 1; private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); @@ -109,7 +111,7 @@ private static void configureMotor() { config.Slot0.kA = KA; config.Slot0.kS = KS; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; config.MotorOutput.Inverted = INVERTED_VALUE; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; @@ -118,12 +120,13 @@ private static void configureMotor() { config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(90); - config.Feedback.RotorToSensorRatio = PitcherConstants.GEAR_RATIO; - config.Feedback.FeedbackRemoteSensorID = PitcherConstants.ENCODER_ID; + config.Feedback.RotorToSensorRatio = GEAR_RATIO; + config.Feedback.FeedbackRemoteSensorID = ENCODER_ID; config.Feedback.FeedbackSensorSource = ENCODER_TYPE; - config.MotionMagic.MotionMagicExpo_kA = EXPO_KA; - config.MotionMagic.MotionMagicExpo_kV = EXPO_KV; + config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; MOTOR.applyConfiguration(config); MOTOR.setPhysicsSimulation(SIMULATION); @@ -131,7 +134,8 @@ private static void configureMotor() { MOTOR.registerSignal(TalonFXSignal.POSITION, 100); MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); + MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); } private static void configuredEncoder() { diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java index 9cb7b1c..5b77040 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java @@ -7,11 +7,11 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import org.trigon.utilities.ShootingCalculations; public class Shooter extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); @@ -49,9 +49,13 @@ public SysIdRoutine.Config getSysIdConfig() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); - updateMechanism(); + } + + @Override + public void updateMechanism() { + ShooterConstants.SHOOTING_MECHANISM.update(getCurrentVelocityRotationsPerSecond(), targetVelocityRotationsPerSecond); } @AutoLogOutput(key = "Shooter/AtShootingVelocity") @@ -76,9 +80,5 @@ void setTargetVelocity(double targetVelocityRotationsPerSecond) { motor.setControl(velocityRequest.withVelocity(targetVelocityRotationsPerSecond)); this.targetVelocityRotationsPerSecond = targetVelocityRotationsPerSecond; } - - private void updateMechanism() { - ShooterConstants.SHOOTING_MECHANISM.update(getCurrentVelocityRotationsPerSecond(), targetVelocityRotationsPerSecond); - } } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 09846f5..e1fad94 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -2,13 +2,17 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathfindingCommand; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -28,17 +32,22 @@ public class Swerve extends MotorSubsystem { public Swerve() { setName("Swerve"); configurePathPlanner(); + phoenix6SignalThread.setOdometryFrequencyHertz(PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.enableContinuousInput(-180, 180); } @Override - public void periodic() { + public void updatePeriodically() { Phoenix6SignalThread.SIGNALS_LOCK.lock(); updateHardware(); Phoenix6SignalThread.SIGNALS_LOCK.unlock(); updatePoseEstimatorStates(); RobotContainer.POSE_ESTIMATOR.periodic(); + } + + @Override + public void updateMechanism() { updateNetworkTables(); } @@ -54,9 +63,27 @@ public void setBrake(boolean brake) { currentModule.setBrake(brake); } + @Override + public void drive(Measure voltageMeasure) { + for (SwerveModule swerveModule : swerveModules) { + swerveModule.setTargetDriveMotorCurrent(voltageMeasure.in(edu.wpi.first.units.Units.Volts)); + swerveModule.setTargetAngle(new Rotation2d()); + } + } + + @Override + public void updateLog(SysIdRoutineLog log) { + for (SwerveModule swerveModule : swerveModules) + swerveModule.driveMotorUpdateLog(log); + } + + @Override + public SysIdRoutine.Config getSysIdConfig() { + return SwerveModuleConstants.DRIVE_MOTOR_SYSID_CONFIG; + } + public Rotation2d getHeading() { - final double inputtedHeading = MathUtil.inputModulus(gyro.getSignal(Pigeon2Signal.YAW), -0.5, 0.5); - return Rotation2d.fromRotations(inputtedHeading); + return Rotation2d.fromDegrees(SwerveConstants.GYRO.getSignal(Pigeon2Signal.YAW)); } public void setHeading(Rotation2d heading) { @@ -100,15 +127,15 @@ public boolean atAngle(MirrorableRotation2d angle) { return atTargetAngle/* && isAngleStill*/; } - public SwerveModulePosition[] getWheelPositions() { - final SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; + public double[] getDriveWheelPositionsRadians() { + final double[] swerveModulesPositions = new double[swerveModules.length]; for (int i = 0; i < swerveModules.length; i++) - swerveModulePositions[i] = swerveModules[i].getOdometryPosition(swerveModules[i].getLastOdometryUpdateIndex()); - return swerveModulePositions; + swerveModulesPositions[i] = swerveModules[i].getDriveWheelPosition(); + return swerveModulesPositions; } - public void runWheelRadiusCharacterization(double omegaRadsPerSec) { - selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadsPerSec)); + public void runWheelRadiusCharacterization(double omegaRadiansPerSecond) { + selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)); } /** @@ -271,7 +298,7 @@ private void updateHardware() { private void configurePathPlanner() { AutoBuilder.configureHolonomic( - () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose(), + RobotContainer.POSE_ESTIMATOR::getCurrentPose, (pose) -> { }, this::getSelfRelativeVelocity, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index a3df31a..2d49893 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -19,7 +19,7 @@ import java.util.function.DoubleSupplier; -public abstract class SwerveConstants { +public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double @@ -55,7 +55,7 @@ public abstract class SwerveConstants { new Translation2d(-MODULE_X_DISTANCE_FROM_CENTER, -MODULE_Y_DISTANCE_FROM_CENTER) }; public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(LOCATIONS); - public static final double DRIVE_RADIUS_METERS = Math.hypot( + public static final double FURTHEST_MODULE_DISTANCE_FROM_CENTER = Math.hypot( MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER ); @@ -65,8 +65,8 @@ public abstract class SwerveConstants { TRANSLATION_VELOCITY_TOLERANCE = 0.05, ROTATION_VELOCITY_TOLERANCE = 0.3; static final double - DRIVE_NEUTRAL_DEADBAND = 0.01, - ROTATION_NEUTRAL_DEADBAND = 0.01; + DRIVE_NEUTRAL_DEADBAND = 0.07, + ROTATION_NEUTRAL_DEADBAND = 0.07; static final double MAX_SPEED_METERS_PER_SECOND = RobotHardwareStats.isSimulation() ? 4.9 : 4.04502, MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND = RobotHardwareStats.isSimulation() ? 12.03 : 12.03; @@ -76,7 +76,7 @@ public abstract class SwerveConstants { new PIDConstants(5, 0, 0) : new PIDConstants(5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(8, 0, 0) : + new PIDConstants(4, 0, 0.05) : new PIDConstants(5, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(9, 0, 0) : @@ -108,7 +108,7 @@ public abstract class SwerveConstants { AUTO_TRANSLATION_PID_CONSTANTS, AUTO_ROTATION_PID_CONSTANTS, MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND, - SwerveConstants.DRIVE_RADIUS_METERS, + SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, REPLANNING_CONFIG ); @@ -120,6 +120,6 @@ public abstract class SwerveConstants { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(SIMULATION_YAW_VELOCITY_SUPPLIER); - GYRO.registerThreadedSignal(Pigeon2Signal.YAW, Pigeon2Signal.ANGULAR_VELOCITY_Z_WORLD, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index fbc2f0b..fb67b12 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -1,11 +1,14 @@ package frc.trigon.robot.subsystems.swerve; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; @@ -20,6 +23,7 @@ public class SwerveModule { private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0); private final VoltageOut driveVoltageRequest = new VoltageOut(0); + private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0); private boolean driveMotorClosedLoop = false; private double[] latestOdometryDrivePositions = new double[0], @@ -33,6 +37,17 @@ public SwerveModule(int moduleID, double offsetRotations) { configureHardware(offsetRotations); } + void setTargetDriveMotorCurrent(double targetCurrent) { + driveMotor.setControl(driveTorqueCurrentFOCRequest.withOutput(targetCurrent)); + } + + void driveMotorUpdateLog(SysIdRoutineLog log) { + log.motor("Module" + driveMotor.getID() + "Drive") + .angularPosition(Units.Rotations.of(driveMotor.getSignal(TalonFXSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(driveMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(driveMotor.getSignal(TalonFXSignal.TORQUE_CURRENT))); + } + void stop() { driveMotor.stopMotor(); steerMotor.stopMotor(); @@ -61,6 +76,14 @@ void setTargetState(SwerveModuleState targetState) { setTargetVelocity(this.targetState.speedMetersPerSecond, this.targetState.angle); } + void setTargetAngle(Rotation2d angle) { + steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); + } + + double getDriveWheelPosition() { + return edu.wpi.first.math.util.Units.rotationsToRadians(driveMotor.getSignal(TalonFXSignal.POSITION)); + } + /** * The odometry thread can update itself faster than the main code loop (which is 50 hertz). * Instead of using the latest odometry update, the accumulated odometry positions since the last loop to get a more accurate position. @@ -91,10 +114,6 @@ private double driveRotationsToMeters(double rotations) { return Conversions.rotationsToDistance(rotations, SwerveModuleConstants.WHEEL_DIAMETER_METERS); } - private void setTargetAngle(Rotation2d angle) { - steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); - } - /** * Sets the target velocity for the module. * @@ -155,11 +174,15 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 714bccc..85b215f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.*; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.simulation.SimpleMotorSimulation; @@ -30,9 +32,12 @@ public class SwerveModuleConstants { STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 50 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 50, DRIVE_MOTOR_I = 0, - DRIVE_MOTOR_D = 0; + DRIVE_MOTOR_D = 0, + DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 0, + DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0, + DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 0; static final boolean ENABLE_FOC = true; static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = generateDriveConfiguration(), @@ -49,6 +54,12 @@ public class SwerveModuleConstants { DRIVE_MOTOR_GEARBOX = DCMotor.getKrakenX60Foc(DRIVE_MOTOR_AMOUNT), STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); + static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(5).per(Units.Second), + Units.Volts.of(20), + Units.Second.of(1000) + ); + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049149 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; @@ -80,6 +91,9 @@ private static TalonFXConfiguration generateDriveConfiguration() { config.Slot0.kP = DRIVE_MOTOR_P; config.Slot0.kI = DRIVE_MOTOR_I; config.Slot0.kD = DRIVE_MOTOR_D; + config.Slot0.kS = DRIVE_MOTOR_KS; + config.Slot0.kV = DRIVE_MOTOR_KV; + config.Slot0.kA = DRIVE_MOTOR_KA; return config; } diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java index a580f6e..fc43d17 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java @@ -32,10 +32,14 @@ public void stop() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); beamBreak.updateSensor(); - updateMechanism(); + } + + @Override + public void updateMechanism() { + TransporterConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); } public boolean isFeeding() { @@ -46,7 +50,7 @@ public boolean isFeeding() { } public boolean isNoteDetected() { - return beamBreak.getBinaryValue(); + return !beamBreak.getBinaryValue(); } void setTargetState(TransporterConstants.TransporterState targetState) { @@ -60,7 +64,7 @@ void setTargetVoltage(double targetVoltage) { } private void configureStoppingNoteCollectionTrigger() { - final Trigger trigger = new Trigger(beamBreak::getBinaryValue).debounce(TransporterConstants.NOTE_COLLECTION_THRESHOLD_SECONDS); + final Trigger trigger = new Trigger(this::isNoteDetected).debounce(TransporterConstants.NOTE_COLLECTION_THRESHOLD_SECONDS); trigger.whileTrue(new InstantCommand(() -> { if (!isCollecting() || this.getCurrentCommand() == null) return; @@ -78,9 +82,5 @@ private void configureStoppingNoteCollectionTrigger() { private boolean isCollecting() { return targetState == TransporterConstants.TransporterState.COLLECTING; } - - private void updateMechanism() { - TransporterConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); - } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index d4bf7dd..63dacbb 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.0", + "version": "3.2.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.2.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.2.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.2.0" + "version": "3.2.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.2.0", + "version": "3.2.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ From 2db4fd4b986e593f1d6bdb3c847e5d6841ff5b29 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Thu, 10 Oct 2024 15:22:32 +0300 Subject: [PATCH 3/6] Commit --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 ++-- .../trigon/robot/commands/AlignToNoteCommand.java | 1 - .../{constants => commands}/CommandConstants.java | 14 +++++++++++++- .../java/frc/trigon/robot/commands/Commands.java | 6 +++++- .../trigon/robot/constants/CameraConstants.java | 4 ++-- .../frc/trigon/robot/constants/FieldConstants.java | 2 +- .../trigon/robot/constants/ShootingConstants.java | 2 +- .../PhotonObjectDetectionCameraIO.java | 8 ++++---- .../apriltagcamera/AprilTagCameraConstants.java | 2 +- .../trigon/robot/subsystems/MotorSubsystem.java | 2 +- .../trigon/robot/subsystems/climber/Climber.java | 2 +- .../robot/subsystems/swerve/SwerveConstants.java | 6 +++--- .../subsystems/swerve/SwerveModuleConstants.java | 2 +- 13 files changed, 35 insertions(+), 20 deletions(-) rename src/main/java/frc/trigon/robot/{constants => commands}/CommandConstants.java (89%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index b142fb2..f12717f 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -11,11 +11,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.Commands; import frc.trigon.robot.commands.LEDAutoSetupCommand; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -116,7 +116,7 @@ private void bindControllerCommands() { OperatorConstants.TURN_AUTOMATIC_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTOMATIC_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTOMATIC_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTOMATIC_NOTE_ALIGNING_OFF_COMMAND); - OperatorConstants.DEBUGGING_BUTTON.whileTrue(ElevatorCommands.getDebuggingCommand()); + OperatorConstants.DEBUGGING_BUTTON.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 5e15479..79b8055 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; diff --git a/src/main/java/frc/trigon/robot/constants/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java similarity index 89% rename from src/main/java/frc/trigon/robot/constants/CommandConstants.java rename to src/main/java/frc/trigon/robot/commands/CommandConstants.java index 9e5c6ba..be3b79d 100644 --- a/src/main/java/frc/trigon/robot/constants/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -1,17 +1,22 @@ -package frc.trigon.robot.constants; +package frc.trigon.robot.commands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; +import org.trigon.commands.WheelRadiusCharacterizationCommand; import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.Mirrorable; import org.trigon.utilities.mirrorable.MirrorableRotation2d; @@ -79,6 +84,13 @@ public class CommandConstants { IS_CLIMBING = false; Logger.recordOutput("IsClimbing", false); }).ignoringDisable(true), + WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand( + new double[]{SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER}, + RobotContainer.SWERVE::getDriveWheelPositionsRadians, + () -> RobotContainer.SWERVE.getHeading().getRadians(), + RobotContainer.SWERVE::runWheelRadiusCharacterization, + RobotContainer.SWERVE + ), ALIGN_TO_RIGHT_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), diff --git a/src/main/java/frc/trigon/robot/commands/Commands.java b/src/main/java/frc/trigon/robot/commands/Commands.java index 338f4f7..74c69a2 100644 --- a/src/main/java/frc/trigon/robot/commands/Commands.java +++ b/src/main/java/frc/trigon/robot/commands/Commands.java @@ -7,7 +7,10 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.Robot; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.*; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -99,6 +102,7 @@ private static boolean atAmpPose() { public static Command getAutonomousScoreInAmpCommand() { return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.ELEVATOR.setDidOpenElevator(true)), TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.ALIGNING_FOR_AMP).withTimeout(0.13), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_AMP).alongWith( runWhenContinueTriggerPressed(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.SCORE_AMP)), diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index cb088f5..9d50eca 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -9,7 +9,7 @@ import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; public class CameraConstants { - public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); + public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("CollectionCamera"); private static final Transform3d REAR_LEFT_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.353, 0.298, 0.282), @@ -45,7 +45,7 @@ public class CameraConstants { // ), REAR_MIDDLE_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "Rear Middle Camera", + "RearMiddleCamera", REAR_MIDDLE_CENTER_TO_CAMERA, 0.0004, 0.001 diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 910a25d..4bc368f 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -18,7 +18,7 @@ public class FieldConstants { FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.21; private static final int SPEAKER_TAG_ID = 7; - private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.84); + private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.70); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), TARGET_DELIVERY_POSITION = new MirrorableTranslation3d(2.5, 7, 0, true); diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index f4eb6c7..5884d1f 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,7 +8,7 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 55, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; public static final double CLOSE_SHOT_VELOCITY_METERS_PER_SECOND = 45; diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java index ed5da4d..0fea0e7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java @@ -26,13 +26,13 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { } private double getBestTargetYaw(PhotonPipelineResult result) { - double lowestSum = 100000; + double lowestSum = -100000; double chosenOne = 0; for (PhotonTrackedTarget target : result.getTargets()) { - final double current = Math.abs(target.getYaw()) + Math.abs(target.getPitch()); - if (lowestSum > current) { + final double current = Math.abs(target.getPitch()); + if (lowestSum < current) { lowestSum = current; - chosenOne = -target.getYaw(); + chosenOne = target.getYaw(); } } return chosenOne; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 2e9d642..ebb4cd5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -9,7 +9,7 @@ import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0.06, new Rotation3d(0, 0, 0)); + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, -0.005, new Rotation3d(0, 0, 0)); static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index abe8bed..755bba8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.Commands; -import frc.trigon.robot.constants.CommandConstants; import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; import org.trigon.hardware.RobotHardwareStats; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 17cc03c..ac1059a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -13,8 +13,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.Commands; -import frc.trigon.robot.constants.CommandConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 2d49893..f4fac23 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -23,9 +23,9 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 0, - GYRO_MOUNT_POSITION_PITCH = 0, - GYRO_MOUNT_POSITION_ROLL = 0; + GYRO_MOUNT_POSITION_YAW = 2.987403154373169, + GYRO_MOUNT_POSITION_PITCH = -2.3428704738616943, + GYRO_MOUNT_POSITION_ROLL = -1.5769307613372803; private static final double FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(225.263672 - 360), FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-256.904297 + 360), diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 85b215f..bf3c05c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -60,7 +60,7 @@ public class SwerveModuleConstants { Units.Second.of(1000) ); - static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049149 * 2; + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.048981 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { From f32f3521feaccf1baecf5b40dae156db85d9a4f7 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Thu, 10 Oct 2024 15:30:30 +0300 Subject: [PATCH 4/6] update vision stuff --- .../java/frc/trigon/robot/RobotContainer.java | 1 + .../robot/commands/CommandConstants.java | 1 + .../robot/constants/OperatorConstants.java | 1 + .../apriltagcamera/AprilTagCamera.java | 79 +++++++++++-------- .../poseestimator/PoseEstimator.java | 11 +++ 5 files changed, 61 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index f12717f..ebfe63c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -89,6 +89,7 @@ private void bindDefaultCommands() { private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); + OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(Commands.getToggleBrakeCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index be3b79d..3effffa 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -43,6 +43,7 @@ public class CommandConstants { () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), new MirrorableRotation2d(0, true)))), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index d481630..2b60724 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -22,6 +22,7 @@ public class OperatorConstants { public static final Trigger DEBUGGING_BUTTON = OPERATOR_CONTROLLER.f3(), RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 2525d41..45c69b9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.Robot; -import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; @@ -20,27 +19,30 @@ public class AprilTagCamera { protected final String name; private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); private final Transform3d robotCenterToCamera; - private final AprilTagCameraIO aprilTagCameraIO; private final double - thetaStandardDeviationsExponent, - translationStandardDeviationsExponent; + thetaStandardDeviationExponent, + translationStandardDeviationExponent; + private final AprilTagCameraIO aprilTagCameraIO; private double lastUpdatedTimestamp; private Pose2d robotPose = null; /** * Constructs a new AprilTagCamera. * - * @param aprilTagCameraType the type of camera - * @param name the camera's name - * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param thetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP - * @param translationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param thetaStandardDeviationExponent a calibrated gain for the standard theta deviations of the estimated robot pose + * @param translationStandardDeviationExponent a calibrated gain for the standard translation deviations of the estimated robot pose */ - public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationsExponent, double translationStandardDeviationsExponent) { + public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, + String name, Transform3d robotCenterToCamera, + double thetaStandardDeviationExponent, + double translationStandardDeviationExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; - this.thetaStandardDeviationsExponent = thetaStandardDeviationsExponent; - this.translationStandardDeviationsExponent = translationStandardDeviationsExponent; + this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; + this.translationStandardDeviationExponent = translationStandardDeviationExponent; if (Robot.IS_REAL) aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); @@ -50,12 +52,9 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT public void update() { aprilTagCameraIO.updateInputs(inputs); - Logger.processInputs("Cameras/" + name, inputs); - robotPose = calculateBestRobotPose(); - logEstimatedRobotPose(); - if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) - logUsedTags(); + robotPose = calculateBestRobotPose(); + logCameraInfo(); } public boolean hasNewResult() { @@ -66,6 +65,10 @@ public Pose2d getEstimatedRobotPose() { return robotPose; } + public Rotation2d getSolvePNPHeading() { + return inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + } + public String getName() { return name; } @@ -74,12 +77,6 @@ public double getLatestResultTimestampSeconds() { return inputs.latestResultTimestampSeconds; } - public void setGyroHeadingToSolvePNPHeading() { - final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); - final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d(); - RobotContainer.POSE_ESTIMATOR.resetPose(new Pose2d(currentTranslation, solvePNPHeading)); - } - /** * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. @@ -87,12 +84,16 @@ public void setGyroHeadingToSolvePNPHeading() { * @return the standard deviations for the pose estimation strategy used */ public Matrix calculateStandardDeviations() { - final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); - final double thetaStandardDeviation = isWithinBestTagRangeForSolvePNP() ? calculateStandardDeviations(thetaStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length) : Double.POSITIVE_INFINITY; + final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = isWithinBestTagRangeForSolvePNP() ? calculateStandardDeviations(thetaStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length) : Double.POSITIVE_INFINITY; return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); } + public double getDistanceToBestTagMeters() { + return inputs.distanceFromBestTag; + } + /** * If the robot is close enough to the tag, it calculates the pose using the solve PNP heading. * If it's too far, it assumes the robot's heading from the gyro and calculates its position from there. @@ -157,8 +158,8 @@ private double projectToPlane(double targetAngleRadians, double cameraAngleRadia } private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { - final double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); - final double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); + double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); } @@ -191,11 +192,17 @@ private boolean isWithinBestTagRangeForSolvePNP() { return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; } - private void logEstimatedRobotPose() { - if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); - else - Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); + private void logCameraInfo() { + Logger.processInputs("Cameras/" + name, inputs); + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); + if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { + logEstimatedRobotPose(); + logSolvePNPPose(); + } else { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", AprilTagCameraConstants.EMPTY_POSE_LIST); + } } private void logUsedTags() { @@ -209,4 +216,12 @@ private void logUsedTags() { usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); } + + private void logEstimatedRobotPose() { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", robotPose); + } + + private void logSolvePNPPose() { + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", inputs.cameraSolvePNPPose.plus(robotCenterToCamera.inverse())); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 13c12f3..4b3447a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -80,6 +80,17 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos poseEstimator6328.addOdometryObservation(new PoseEstimator6328.OdometryObservation(swerveWheelPositions[i], gyroRotations[i], timestamps[i])); } + public void setGyroHeadingToBestSolvePNPHeading() { + int closestCameraToTag = 0; + for (int i = 0; i < aprilTagCameras.length; i++) { + if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) + closestCameraToTag = i; + } + + final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + } + private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) From 6bdcc4165ecf082429f06e12a0a55c6e46d96335 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Sat, 19 Oct 2024 23:34:05 +0300 Subject: [PATCH 5/6] (I really don't this this) commit --- .pathplanner/settings.json | 4 +- build.gradle | 2 +- src/main/deploy/2024-crescendo.json | 296 ++++++++++++++++++ .../Pathplanner/paths/Picture2Collect2.path | 4 +- .../Pathplanner/paths/Picture2Collect4.path | 4 +- .../Pathplanner/paths/Picture2Collect6.path | 4 +- .../paths/Picture2CollectAndShoot1.path | 4 +- .../Pathplanner/paths/Picture2Shoot3.path | 4 +- .../Pathplanner/paths/Picture2Shoot5.path | 4 +- .../Pathplanner/paths/Picture3Collect4.path | 4 +- .../paths/Picture3CollectAndShoot2.path | 4 +- .../paths/Picture3CollectAndShoot3.path | 6 +- .../Pathplanner/paths/Picture4Collect3.path | 4 +- .../Pathplanner/paths/Picture4Collect5.path | 4 +- .../Pathplanner/paths/Picture4Collect7.path | 4 +- .../paths/Picture4CollectAndShoot1.path | 4 +- .../paths/Picture4CollectAndShoot2.path | 4 +- .../Pathplanner/paths/Picture4Shoot4.path | 4 +- .../Pathplanner/paths/Picture4Shoot6.path | 4 +- .../pathplanner/autos/CloseNotesMiddle.auto | 12 +- .../deploy/pathplanner/autos/Picture3.auto | 16 +- .../paths/CloseLeftNotesShoot2.path | 4 +- .../pathplanner/paths/CloseNotesLeft5.path | 21 +- .../paths/CloseNotesLeftCollect1.path | 39 ++- .../paths/CloseNotesLeftCollectAndShoot3.path | 10 +- .../paths/CloseNotesLeftCollectAndShoot4.path | 21 +- .../pathplanner/paths/CloseNotesMiddle2.path | 21 +- .../pathplanner/paths/CloseNotesMiddle3.path | 27 +- .../pathplanner/paths/CloseNotesMiddle4.path | 21 +- .../pathplanner/paths/CloseNotesMiddle5.path | 4 +- .../CloseNotesMiddleCollectAndShoot1.path | 10 +- .../paths/FinishOrbitNotesCollect1.path | 4 +- .../paths/FinishOrbitNotesShoot3.path | 4 +- .../FinishOrbitNotesShootAndCollect2.path | 4 +- .../paths/New New New New Path.path | 12 +- .../pathplanner/paths/New New New Path.path | 12 +- .../pathplanner/paths/New New Path.path | 4 +- .../deploy/pathplanner/paths/New Path.path | 4 +- .../deploy/pathplanner/paths/Option3P1.path | 4 +- .../deploy/pathplanner/paths/Option3P2.path | 4 +- .../deploy/pathplanner/paths/Option3P3.path | 4 +- .../deploy/pathplanner/paths/Option3P4.path | 4 +- .../deploy/pathplanner/paths/Option3P5.path | 4 +- .../deploy/pathplanner/paths/Option3P6.path | 4 +- .../deploy/pathplanner/paths/Option4P1.path | 4 +- .../deploy/pathplanner/paths/Option4P2.path | 4 +- .../deploy/pathplanner/paths/Option4P3.path | 4 +- .../deploy/pathplanner/paths/Option4P4.path | 4 +- .../deploy/pathplanner/paths/Option4P5.path | 4 +- .../deploy/pathplanner/paths/Option4P6.path | 4 +- .../deploy/pathplanner/paths/Option4P7.path | 4 +- .../deploy/pathplanner/paths/Pic3skip.path | 4 +- .../pathplanner/paths/Picture3Collect1.path | 10 +- .../pathplanner/paths/Picture3ForthShoot.path | 52 +++ .../paths/Picture3SecondCollect.path | 93 ++++++ .../pathplanner/paths/Picture3Shoot1.path | 4 +- .../pathplanner/paths/Picture3Shoot5.path | 4 +- .../paths/Picture3SkipCollect1.path | 4 +- .../pathplanner/paths/Picture3SkipShoot2.path | 4 +- .../paths/Picture3ThirdCollect.path | 93 ++++++ .../pathplanner/paths/Picture3ThirdShoot.path | 52 +++ src/main/java/frc/trigon/robot/Robot.java | 59 ++++ .../java/frc/trigon/robot/RobotContainer.java | 4 +- .../robot/commands/AlignToNoteCommand.java | 13 +- .../robot/commands/CommandConstants.java | 9 +- .../frc/trigon/robot/commands/Commands.java | 36 ++- .../commands/GearRatioCalculationCommand.java | 79 +++++ .../WheelRadiusCharacterizationCommand.java | 113 +++++++ .../robot/constants/AutonomousConstants.java | 2 + .../robot/constants/CameraConstants.java | 8 +- .../robot/constants/FieldConstants.java | 4 +- .../robot/constants/OperatorConstants.java | 4 +- .../robot/constants/ShootingConstants.java | 4 +- .../robot/misc/ShootingCalculations.java | 24 +- .../apriltagcamera/AprilTagCamera.java | 20 +- .../AprilTagCameraConstants.java | 6 +- .../io/AprilTagPhotonCameraIO.java | 42 ++- .../poseestimator/PoseEstimatorConstants.java | 2 +- .../robot/subsystems/climber/Climber.java | 4 +- .../subsystems/climber/ClimberConstants.java | 2 +- .../elevator/ElevatorConstants.java | 2 +- .../robot/subsystems/pitcher/Pitcher.java | 27 +- .../subsystems/pitcher/PitcherCommands.java | 11 + .../subsystems/pitcher/PitcherConstants.java | 35 ++- .../subsystems/shooter/ShooterConstants.java | 3 +- .../robot/subsystems/swerve/Swerve.java | 18 +- .../subsystems/swerve/SwerveConstants.java | 35 ++- .../robot/subsystems/swerve/SwerveModule.java | 16 +- .../swerve/SwerveModuleConstants.java | 18 +- .../subsystems/transporter/Transporter.java | 2 + vendordeps/photonlib.json | 10 +- 91 files changed, 1351 insertions(+), 251 deletions(-) create mode 100644 src/main/deploy/2024-crescendo.json create mode 100644 src/main/deploy/pathplanner/paths/Picture3ForthShoot.path create mode 100644 src/main/deploy/pathplanner/paths/Picture3SecondCollect.path create mode 100644 src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path create mode 100644 src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path create mode 100644 src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java create mode 100644 src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 67136f4..84a6bf9 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -13,8 +13,8 @@ "Picture4" ], "autoFolders": [], - "defaultMaxVel": 3.2, - "defaultMaxAccel": 3.2, + "defaultMaxVel": 3.4, + "defaultMaxAccel": 3.4, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/build.gradle b/build.gradle index 9b3bef3..d48ee05 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.11' + implementation 'com.github.Programming-TRIGON:TRIGONLib:c59c440' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/deploy/2024-crescendo.json b/src/main/deploy/2024-crescendo.json new file mode 100644 index 0000000..17ea2ae --- /dev/null +++ b/src/main/deploy/2024-crescendo.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.582342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect2.path b/src/main/deploy/Pathplanner/paths/Picture2Collect2.path index af0b614..1a6bf6e 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect2.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect2.path @@ -67,8 +67,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect4.path b/src/main/deploy/Pathplanner/paths/Picture2Collect4.path index e3e3e87..ead3c7b 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect4.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect4.path @@ -72,8 +72,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect6.path b/src/main/deploy/Pathplanner/paths/Picture2Collect6.path index f123605..254631e 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect6.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path b/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path index 4162020..28c2dff 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path +++ b/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path b/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path index 38185e1..bfa3e4c 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path b/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path index 1b576df..baee80b 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture3Collect4.path b/src/main/deploy/Pathplanner/paths/Picture3Collect4.path index 8d4a5d5..622208c 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3Collect4.path +++ b/src/main/deploy/Pathplanner/paths/Picture3Collect4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path index 92f6e49..1761b9f 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path +++ b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path @@ -83,8 +83,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path index ab04c7e..4acb329 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path +++ b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path @@ -28,7 +28,7 @@ "y": 5.782000590087349 }, "isLocked": false, - "linkedName": null + "linkedName": "Picture3ThirdCollect" }, { "anchor": { @@ -78,8 +78,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect3.path b/src/main/deploy/Pathplanner/paths/Picture4Collect3.path index c2ef5cc..3c44596 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect3.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect3.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect5.path b/src/main/deploy/Pathplanner/paths/Picture4Collect5.path index 0617e98..a1513a1 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect5.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect7.path b/src/main/deploy/Pathplanner/paths/Picture4Collect7.path index 82fb02a..a77b683 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect7.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect7.path @@ -72,8 +72,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path index ac8ab80..eb69802 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path +++ b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path index 1d21484..9390fd3 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path +++ b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path b/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path index 734b7f0..de7b186 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path b/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path index 3210781..ac96e77 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto b/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto index 0ec8261..d2c80a3 100644 --- a/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto +++ b/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto @@ -79,6 +79,12 @@ "pathName": "CloseNotesMiddle4" } }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, { "type": "path", "data": { @@ -96,12 +102,6 @@ } ] } - }, - { - "type": "named", - "data": { - "name": "StopShooting" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Picture3.auto b/src/main/deploy/pathplanner/autos/Picture3.auto index a0f1b29..db4b4a2 100644 --- a/src/main/deploy/pathplanner/autos/Picture3.auto +++ b/src/main/deploy/pathplanner/autos/Picture3.auto @@ -52,7 +52,13 @@ { "type": "path", "data": { - "pathName": "Picture3CollectAndShoot2" + "pathName": "Picture3SecondCollect" + } + }, + { + "type": "path", + "data": { + "pathName": "Picture3ThirdShoot" } }, { @@ -70,7 +76,13 @@ { "type": "path", "data": { - "pathName": "Picture3CollectAndShoot3" + "pathName": "Picture3ThirdCollect" + } + }, + { + "type": "path", + "data": { + "pathName": "Picture3ForthShoot" } }, { diff --git a/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path b/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path index 6f7a111..24accbe 100644 --- a/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path +++ b/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path b/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path index 7ea834b..a46b969 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path @@ -53,11 +53,28 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path index d629511..e4aa057 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.5974921231201995, - "y": 7.121079443568702 + "x": 1.5928943538798759, + "y": 6.969716047694203 }, "isLocked": false, "linkedName": null @@ -20,15 +20,21 @@ "y": 7.0 }, "prevControl": { - "x": 2.0955857298725507, - "y": 7.108274947832159 + "x": 2.072354686019913, + "y": 7.016492665463964 }, "nextControl": null, "isLocked": false, "linkedName": "EndCloseNotesCollect1" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { @@ -53,18 +59,35 @@ ] } } + }, + { + "name": "AlignToNote", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, "rotation": 0.0, - "rotateFast": true + "rotateFast": false }, "reversed": false, "folder": "CloseNotesLeft", diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path index 9d76b25..aff64bd 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path @@ -49,6 +49,12 @@ "data": { "name": "FeedNoteWithoutWaiting" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -56,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path index 52aad7b..e486892 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path @@ -59,11 +59,28 @@ ] } } + }, + { + "name": "Align", + "waypointRelativePos": 0.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path index 6649bf8..0595ee7 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path @@ -59,11 +59,28 @@ ] } } + }, + { + "name": "Align", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path index 671f713..0be13ee 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path @@ -55,6 +55,29 @@ "data": { "name": "FeedNoteWithoutWaiting" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } + }, + { + "name": "Align", + "waypointRelativePos": 0.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -62,8 +85,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path index 368d15a..9434908 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path @@ -59,11 +59,28 @@ ] } } + }, + { + "name": "Align", + "waypointRelativePos": 0.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path index 57e806e..261f255 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path index d68b29b..b100280 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path @@ -49,6 +49,12 @@ "data": { "name": "TransporterCollection" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -56,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path index 378f1e9..7ca42ac 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path index 9a0ea48..b3d2986 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path index bbe69c4..608face 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path @@ -127,8 +127,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path index 118b513..0153b02 100644 --- a/src/main/deploy/pathplanner/paths/New New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.094760892609672, - "y": 7.273319629774651 + "x": 7.997263310441111, + "y": 7.341567937292645 }, "prevControl": null, "nextControl": { - "x": 6.954039181237508, - "y": 7.195321564039803 + "x": 6.856541599068947, + "y": 7.263569871557797 }, "isLocked": false, "linkedName": "end1" @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path index 54a219d..46f15ee 100644 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.094760892609672, - "y": 7.273319629774651 + "x": 7.997263310441111, + "y": 7.341567937292645 }, "prevControl": { - "x": 7.266031444176903, - "y": 6.5128384888598765 + "x": 7.168533862008342, + "y": 6.58108679637787 }, "nextControl": null, "isLocked": false, @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path index 248b77d..4d7fc33 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 2f7461a..522b77b 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option3P1.path b/src/main/deploy/pathplanner/paths/Option3P1.path index 65268be..2a83835 100644 --- a/src/main/deploy/pathplanner/paths/Option3P1.path +++ b/src/main/deploy/pathplanner/paths/Option3P1.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option3P2.path b/src/main/deploy/pathplanner/paths/Option3P2.path index b804629..08795a6 100644 --- a/src/main/deploy/pathplanner/paths/Option3P2.path +++ b/src/main/deploy/pathplanner/paths/Option3P2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option3P3.path b/src/main/deploy/pathplanner/paths/Option3P3.path index 699afbd..2d137c7 100644 --- a/src/main/deploy/pathplanner/paths/Option3P3.path +++ b/src/main/deploy/pathplanner/paths/Option3P3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option3P4.path b/src/main/deploy/pathplanner/paths/Option3P4.path index acb8cc4..e0b5532 100644 --- a/src/main/deploy/pathplanner/paths/Option3P4.path +++ b/src/main/deploy/pathplanner/paths/Option3P4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option3P5.path b/src/main/deploy/pathplanner/paths/Option3P5.path index 95cada5..3f82648 100644 --- a/src/main/deploy/pathplanner/paths/Option3P5.path +++ b/src/main/deploy/pathplanner/paths/Option3P5.path @@ -68,8 +68,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option3P6.path b/src/main/deploy/pathplanner/paths/Option3P6.path index 23360ef..0472497 100644 --- a/src/main/deploy/pathplanner/paths/Option3P6.path +++ b/src/main/deploy/pathplanner/paths/Option3P6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P1.path b/src/main/deploy/pathplanner/paths/Option4P1.path index f211ba9..13c8472 100644 --- a/src/main/deploy/pathplanner/paths/Option4P1.path +++ b/src/main/deploy/pathplanner/paths/Option4P1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P2.path b/src/main/deploy/pathplanner/paths/Option4P2.path index e72d193..173b101 100644 --- a/src/main/deploy/pathplanner/paths/Option4P2.path +++ b/src/main/deploy/pathplanner/paths/Option4P2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P3.path b/src/main/deploy/pathplanner/paths/Option4P3.path index d75c1bb..aba5941 100644 --- a/src/main/deploy/pathplanner/paths/Option4P3.path +++ b/src/main/deploy/pathplanner/paths/Option4P3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P4.path b/src/main/deploy/pathplanner/paths/Option4P4.path index efb2266..32dd6e7 100644 --- a/src/main/deploy/pathplanner/paths/Option4P4.path +++ b/src/main/deploy/pathplanner/paths/Option4P4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P5.path b/src/main/deploy/pathplanner/paths/Option4P5.path index dbba094..2bec671 100644 --- a/src/main/deploy/pathplanner/paths/Option4P5.path +++ b/src/main/deploy/pathplanner/paths/Option4P5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P6.path b/src/main/deploy/pathplanner/paths/Option4P6.path index 726713d..920e85b 100644 --- a/src/main/deploy/pathplanner/paths/Option4P6.path +++ b/src/main/deploy/pathplanner/paths/Option4P6.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Option4P7.path b/src/main/deploy/pathplanner/paths/Option4P7.path index 34a2db1..b364c15 100644 --- a/src/main/deploy/pathplanner/paths/Option4P7.path +++ b/src/main/deploy/pathplanner/paths/Option4P7.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Pic3skip.path b/src/main/deploy/pathplanner/paths/Pic3skip.path index c8cf2e8..bf8ef41 100644 --- a/src/main/deploy/pathplanner/paths/Pic3skip.path +++ b/src/main/deploy/pathplanner/paths/Pic3skip.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Picture3Collect1.path b/src/main/deploy/pathplanner/paths/Picture3Collect1.path index d4a5258..07f6f6c 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Collect1.path +++ b/src/main/deploy/pathplanner/paths/Picture3Collect1.path @@ -49,6 +49,12 @@ "data": { "name": "IntakeCollection" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -56,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path b/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path new file mode 100644 index 0000000..c0c9ed6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.28, + "y": 5.78 + }, + "prevControl": null, + "nextControl": { + "x": 6.495800545045272, + "y": 6.288594049872185 + }, + "isLocked": false, + "linkedName": "Picture3ThirdCollect" + }, + { + "anchor": { + "x": 3.4171824469005063, + "y": 6.303149244475128 + }, + "prevControl": { + "x": 4.477600594156059, + "y": 6.395841390257602 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "EndPicture3CollectAndShoot3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.4, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 12.159999999999968, + "rotateFast": false + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path b/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path new file mode 100644 index 0000000..10cf334 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path @@ -0,0 +1,93 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9026396514331485, + "y": 6.993104356579084 + }, + "prevControl": null, + "nextControl": { + "x": 3.8536160682772684, + "y": 6.600586312811581 + }, + "isLocked": false, + "linkedName": "EndPicture3Collect1" + }, + { + "anchor": { + "x": 7.997263310441111, + "y": 7.341567937292645 + }, + "prevControl": { + "x": 6.97353869767122, + "y": 6.980826883268969 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "end1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "TransporterCollection" + } + }, + { + "type": "named", + "data": { + "name": "IntakeCollection" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.4, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 14.574216198038673, + "rotateFast": false + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Picture3Shoot1.path b/src/main/deploy/pathplanner/paths/Picture3Shoot1.path index 6cf2ed1..fbd8646 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Shoot1.path +++ b/src/main/deploy/pathplanner/paths/Picture3Shoot1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Picture3Shoot5.path b/src/main/deploy/pathplanner/paths/Picture3Shoot5.path index e18db35..2d15e25 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Shoot5.path +++ b/src/main/deploy/pathplanner/paths/Picture3Shoot5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path b/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path index ff7ae0a..28866b5 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path +++ b/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path b/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path index 02ec47d..ae9fe20 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path +++ b/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, + "maxVelocity": 3.4, + "maxAcceleration": 3.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path b/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path new file mode 100644 index 0000000..018c0d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path @@ -0,0 +1,93 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.4171824469005063, + "y": 6.303149244475128 + }, + "prevControl": null, + "nextControl": { + "x": 4.506849868806627, + "y": 6.42509066490817 + }, + "isLocked": false, + "linkedName": "EndPicture3CollectAndShoot2" + }, + { + "anchor": { + "x": 8.28, + "y": 5.78 + }, + "prevControl": { + "x": 7.217282653092623, + "y": 5.9473525122822215 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Picture3ThirdCollect" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeCollection" + } + }, + { + "type": "named", + "data": { + "name": "TransporterCollection" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.4, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -9.64280250924343, + "rotateFast": false + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path b/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path new file mode 100644 index 0000000..2f4e153 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.997263310441111, + "y": 7.341567937292645 + }, + "prevControl": null, + "nextControl": { + "x": 6.544549336129552, + "y": 6.93207809218469 + }, + "isLocked": false, + "linkedName": "end1" + }, + { + "anchor": { + "x": 3.4171824469005063, + "y": 6.303149244475128 + }, + "prevControl": { + "x": 4.7408440660111735, + "y": 6.590836554594724 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "EndPicture3CollectAndShoot2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.4, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 12.699999999999989, + "rotateFast": true + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index 79ea6a8..1a156b7 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -6,8 +6,15 @@ package frc.trigon.robot; import com.pathplanner.lib.pathfinding.Pathfinding; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.trigon.robot.constants.CameraConstants; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.RobotConstants; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -23,6 +30,7 @@ public class Robot extends LoggedRobot { private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private Command autonomousCommand; private RobotContainer robotContainer; + private Pose2d pose1 = null, pose2 = null; @Override public void robotInit() { @@ -30,6 +38,32 @@ public void robotInit() { Pathfinding.setPathfinder(new LocalADStarAK()); configLogger(); robotContainer = new RobotContainer(); + OperatorConstants.OPERATOR_CONTROLLER.numpad0().onTrue(new InstantCommand( + () -> { + CameraConstants.REAR_MIDDLE_CAMERA.update(); + pose1 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); + System.out.println("Pose 1: " + pose1); + } + )); + OperatorConstants.OPERATOR_CONTROLLER.numpad1().onTrue(new InstantCommand( + () -> { + CameraConstants.REAR_MIDDLE_CAMERA.update(); + pose2 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); + System.out.println("Pose 2: " + pose2); + } + )); + OperatorConstants.OPERATOR_CONTROLLER.numpad2().onTrue(new InstantCommand( + () -> { + final Translation2d translation2d = calc( + pose1.getRotation().getRadians(), pose2.getRotation().getRadians(), + pose1.getX(), pose2.getX(), + pose1.getY(), pose2.getY() + ); + System.out.println("__________________________" + translation2d); + Logger.recordOutput("TranslationX", translation2d.getX()); + Logger.recordOutput("TranslationY", translation2d.getY()); + } + )); } @Override @@ -37,6 +71,31 @@ public void robotPeriodic() { commandScheduler.run(); } + public static Translation2d calc(double a1, double a2, double x1, double x2, double y1, double y2) { + return new Translation2d( + calculateX(a1, a2, x1, x2, y1, y2), + calculateY(a1, a2, x1, x2, y1, y2) + ); + } + + public static double calculateX(double a1, double a2, double x1, double x2, double y1, double y2) { + double A = Math.cos(a1) - Math.cos(a2); + double B = Math.sin(a1) - Math.sin(a2); + double C = x2 - x1; + double D = y2 - y1; + + return (C * A + D * B) / (A * A + B * B); + } + + public static double calculateY(double a1, double a2, double x1, double x2, double y1, double y2) { + double A = Math.cos(a1) - Math.cos(a2); + double B = Math.sin(a1) - Math.sin(a2); + double C = x2 - x1; + double D = y2 - y1; + + return (D * A - C * B) / (A * A + B * B); + } + @Override public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index ebfe63c..7c4225f 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -73,7 +73,7 @@ public Command getAutonomousCommand() { private void configureBindings() { bindDefaultCommands(); bindControllerCommands(); -// configureSysIdBindings(ELEVATOR); +// configureSysIdBindings(PITCHER); } private void bindDefaultCommands() { @@ -84,7 +84,7 @@ private void bindDefaultCommands() { ELEVATOR.setDefaultCommand(new WaitUntilCommand(() -> ELEVATOR.isBelowCameraPlate() && ELEVATOR.didOpenElevator()).andThen(Commands.withoutRequirements(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.ALIGNING_FOR_AMP_BACKWARDS)).withTimeout(0.13).andThen(new InstantCommand(() -> ELEVATOR.setDidOpenElevator(false)))).alongWith(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.RESTING))); TRANSPORTER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(TRANSPORTER)); CLIMBER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(CLIMBER)); - LEDStrip.setDefaultCommandForAllLEDS((ledStrip) -> LEDStripCommands.getAnimateColorFlowCommand(new Color(0, 150, 255), 0.5, ledStrip)); + LEDStrip.setDefaultCommandForAllLEDS((ledStrip) -> Commands.getContinuousConditionalCommand(LEDStripCommands.getStaticColorCommand(Color.green, ledStrip), LEDStripCommands.getAnimateColorFlowCommand(new Color(0, 150, 255), 0.5, ledStrip), TRANSPORTER::isNoteDetected)); } private void bindControllerCommands() { diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 79b8055..aea0211 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -20,13 +20,13 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? - new PIDController(0.0075, 0, 0) : - new PIDController(0, 0, 0); + new PIDController(0.015, 0, 0) : + new PIDController(0.005, 0, 0); public AlignToNoteCommand() { addCommands( getSetCurrentLEDColorCommand().asProxy(), - Commands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), Commands.duplicate(CommandConstants.SELF_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), + Commands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), Commands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), new RunCommand(CAMERA::trackObject) ); } @@ -41,12 +41,17 @@ private Command getSetCurrentLEDColorCommand() { private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); } + private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { + final Rotation2d driveRelativeAngle = RobotContainer.SWERVE.getDriveRelativeAngle(); + return (CommandConstants.calculateDriveStickAxisValue(xPower) * driveRelativeAngle.getCos()) + (CommandConstants.calculateDriveStickAxisValue(yPower) * driveRelativeAngle.getSin()); + } + private MirrorableRotation2d getTargetAngle() { final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 3effffa..403a7da 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -16,7 +16,6 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; -import org.trigon.commands.WheelRadiusCharacterizationCommand; import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.Mirrorable; import org.trigon.utilities.mirrorable.MirrorableRotation2d; @@ -33,19 +32,19 @@ public class CommandConstants { MINIMUM_ROTATION_SHIFT_POWER = 0.6; public static final Command - FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopFieldRelativeDriveCommand( + FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), - SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( + SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), new MirrorableRotation2d(0, true)))), - SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( + SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> 0 @@ -113,7 +112,7 @@ public static double calculateDriveStickAxisValue(double axisValue) { } public static double calculateRotationStickAxisValue(double axisValue) { - return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER) / 1.5; + return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER) / 1.3; } /** diff --git a/src/main/java/frc/trigon/robot/commands/Commands.java b/src/main/java/frc/trigon/robot/commands/Commands.java index 74c69a2..50fa544 100644 --- a/src/main/java/frc/trigon/robot/commands/Commands.java +++ b/src/main/java/frc/trigon/robot/commands/Commands.java @@ -1,16 +1,14 @@ package frc.trigon.robot.commands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.Robot; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.ShootingConstants; +import frc.trigon.robot.constants.*; import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -32,6 +30,7 @@ import org.trigon.utilities.mirrorable.MirrorableRotation2d; import java.awt.*; +import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -120,6 +119,35 @@ public static Command getShootAtShootingTargetCommand(boolean isDelivery) { ); } + public static Command getAlignToNoteCommand() { + return new StartEndCommand( + () -> { + CameraConstants.NOTE_DETECTION_CAMERA.startTrackingBestObject(); + PPHolonomicDriveController.setRotationTargetOverride(Commands::calculateRotationOverride); + }, + () -> PPHolonomicDriveController.setRotationTargetOverride(Optional::empty) + ).alongWith(getSetCurrentLEDColorCommand()); + } + + private static Optional calculateRotationOverride() { + CameraConstants.NOTE_DETECTION_CAMERA.trackObject(); + if (RobotContainer.TRANSPORTER.isNoteDetected() || !CameraConstants.NOTE_DETECTION_CAMERA.hasTargets()) + return Optional.empty(); + + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d targetRotation = CameraConstants.NOTE_DETECTION_CAMERA.getTrackedObjectYaw().plus(currentRotation); + return Optional.of(targetRotation); + } + + private static Command getSetCurrentLEDColorCommand() { + return getContinuousConditionalCommand( + LEDStripCommands.getStaticColorCommand(Color.green, LEDStripConstants.LED_STRIPS), + LEDStripCommands.getStaticColorCommand(Color.red, LEDStripConstants.LED_STRIPS), + CameraConstants.NOTE_DETECTION_CAMERA::hasTargets + ).asProxy(); + } + + public static Command getShootAtSpeakerTestingCommand() { return new ParallelCommandGroup( getPrepareShootingTestingCommand(), diff --git a/src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java b/src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java new file mode 100644 index 0000000..7351ec2 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java @@ -0,0 +1,79 @@ +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +package frc.trigon.robot.commands; + +import edu.wpi.first.wpilibj2.command.*; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; + +public class GearRatioCalculationCommand extends Command { + private final DoubleSupplier rotorPositionSupplier; + private final DoubleSupplier encoderPositionSupplier; + private final DoubleConsumer runGearRatioCalculation; + private final String subsystemName; + private final LoggedDashboardNumber movementVoltage; + private final LoggedDashboardBoolean shouldMoveClockwise; + private double startingRotorPosition; + private double startingEncoderPosition; + private double gearRatio; + + public GearRatioCalculationCommand(DoubleSupplier rotorPositionSupplier, DoubleSupplier encoderPositionSupplier, DoubleConsumer runGearRatioCalculation, SubsystemBase requirement) { + this.rotorPositionSupplier = rotorPositionSupplier; + this.encoderPositionSupplier = encoderPositionSupplier; + this.runGearRatioCalculation = runGearRatioCalculation; + this.subsystemName = requirement.getName(); + this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1.0); + this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false); + this.addRequirements(new Subsystem[]{requirement}); + } + + public void initialize() { + new WaitCommand(0.5).andThen(new InstantCommand( () -> { + this.startingRotorPosition = this.rotorPositionSupplier.getAsDouble(); + this.startingEncoderPosition = this.encoderPositionSupplier.getAsDouble(); + })).schedule(); + this.startingRotorPosition = this.rotorPositionSupplier.getAsDouble(); + this.startingEncoderPosition = this.encoderPositionSupplier.getAsDouble(); + } + + public void execute() { + this.runGearRatioCalculation.accept(this.movementVoltage.get() * (double)this.getRotationDirection()); + this.gearRatio = this.calculateGearRatio(); + Logger.recordOutput("GearRatioCalculation/" + this.subsystemName + "/RotorDistance", this.getRotorDistance()); + Logger.recordOutput("GearRatioCalculation/" + this.subsystemName + "/EncoderDistance", this.getEncoderDistance()); + Logger.recordOutput("GearRatioCalculation/" + this.subsystemName + "/GearRatio", this.gearRatio); + } + + public void end(boolean interrupted) { + this.printResult(); + } + + private double calculateGearRatio() { + double currentRotorPosition = this.getRotorDistance(); + double currentEncoderPosition = this.getEncoderDistance(); + return currentRotorPosition / currentEncoderPosition; + } + + private double getRotorDistance() { + return this.startingRotorPosition - this.rotorPositionSupplier.getAsDouble(); + } + + private double getEncoderDistance() { + return this.startingEncoderPosition - this.encoderPositionSupplier.getAsDouble(); + } + + private int getRotationDirection() { + return this.shouldMoveClockwise.get() ? -1 : 1; + } + + private void printResult() { + System.out.println(this.subsystemName + " Gear Ratio: " + this.gearRatio); + } +} diff --git a/src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java new file mode 100644 index 0000000..f653f07 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java @@ -0,0 +1,113 @@ +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +package frc.trigon.robot.commands; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +import java.util.Arrays; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class WheelRadiusCharacterizationCommand extends Command { + private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1.0); + private static final LoggedDashboardNumber ROTATION_RATE_LIMIT = new LoggedDashboardNumber("WheelRadiusCharacterization/RotationRateLimit", 1.0); + private static final LoggedDashboardBoolean SHOULD_MOVE_CLOCKWISE = new LoggedDashboardBoolean("WheelRadiusCharacterization/ShouldMoveClockwise", false); + private final double[] wheelDistancesFromCenterMeters; + private final Supplier wheelPositionsRadiansSupplier; + private final DoubleSupplier gyroYawRadiansSupplier; + private final DoubleConsumer runWheelRadiusCharacterization; + private SlewRateLimiter rotationSlewRateLimiter; + private double startingGyroYawRadians; + private double accumulatedGyroYawRadians; + private double[] startingWheelPositions; + private double driveWheelsRadius; + + public WheelRadiusCharacterizationCommand(Translation2d[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement) { + this.rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + this.wheelDistancesFromCenterMeters = Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(); + this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; + this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; + this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; + this.addRequirements(new Subsystem[]{requirement}); + } + + public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement) { + this.rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + this.wheelDistancesFromCenterMeters = wheelDistancesFromCenterMeters; + this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; + this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; + this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; + this.addRequirements(new Subsystem[]{requirement}); + } + + public void initialize() { + this.configureStartingStats(); + } + + public void execute() { + this.driveMotors(); + this.accumulatedGyroYawRadians = this.getAccumulatedGyroYaw(); + this.driveWheelsRadius = this.calculateDriveWheelRadius(); + Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", this.accumulatedGyroYawRadians); + Logger.recordOutput("RadiusCharacterization/DriveWheelRadius", this.driveWheelsRadius); + } + + public void end(boolean interrupted) { + this.printResults(); + } + + private void configureStartingStats() { + this.startingGyroYawRadians = this.gyroYawRadiansSupplier.getAsDouble(); + this.startingWheelPositions = (double[])this.wheelPositionsRadiansSupplier.get(); + this.accumulatedGyroYawRadians = 0.0; + this.rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + this.rotationSlewRateLimiter.reset(0.0); + } + + private void driveMotors() { + this.runWheelRadiusCharacterization.accept(this.rotationSlewRateLimiter.calculate((double)this.getRotationDirection() * CHARACTERIZATION_SPEED.get())); + } + + private double getAccumulatedGyroYaw() { + return Math.abs(this.startingGyroYawRadians - this.gyroYawRadiansSupplier.getAsDouble()); + } + + private double calculateDriveWheelRadius() { + this.driveWheelsRadius = 0.0; + double[] wheelPositionsRadians = (double[])this.wheelPositionsRadiansSupplier.get(); + + for(int i = 0; i < 4; ++i) { + final double accumulatedWheelRadians = Math.abs(wheelPositionsRadians[i] - this.startingWheelPositions[i]); + final double currentWheelRadius = accumulatedGyroYawRadians * this.wheelDistancesFromCenterMeters[i] / accumulatedWheelRadians; + this.driveWheelsRadius += currentWheelRadius; + Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians); + Logger.recordOutput("RadiusCharacterization/WheelRadius" + i, currentWheelRadius); + } + + return this.driveWheelsRadius /= 4.0; + } + + private void printResults() { + if (this.accumulatedGyroYawRadians <= 6.283185307179586) { + System.out.println("Not enough data for characterization"); + } else { + System.out.println("Drive Wheel Radius: " + this.driveWheelsRadius + " meters"); + } + + } + + private int getRotationDirection() { + return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1; + } +} diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 81fa70d..233a551 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -3,6 +3,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.Commands; import frc.trigon.robot.subsystems.intake.IntakeCommands; @@ -35,5 +36,6 @@ private static void registerCommands() { NamedCommands.registerCommand("IntakeCollection", IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECTING)); NamedCommands.registerCommand("IntakeStopping", IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOPPED)); NamedCommands.registerCommand("ShootingEject", ShooterCommands.getSetTargetShootingVelocityCommand(30).alongWith(PitcherCommands.getSetTargetPitchCommand(Rotation2d.fromDegrees(26)))); + NamedCommands.registerCommand("AlignToNote", new InstantCommand()); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 9d50eca..a1673eb 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -24,8 +24,8 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(-31.7), 0) ), REAR_MIDDLE_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0, 0, 0.63), - new Rotation3d(0, Units.degreesToRadians(-23.7), Units.degreesToRadians(180)) + new Translation3d(-0.025, 0, 0.63), + new Rotation3d(0, Units.degreesToRadians(-24.2), Units.degreesToRadians(180)) ); public static final AprilTagCamera // REAR_LEFT_CAMERA = new AprilTagCamera( @@ -47,7 +47,7 @@ public class CameraConstants { AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "RearMiddleCamera", REAR_MIDDLE_CENTER_TO_CAMERA, - 0.0004, - 0.001 + 0.001, + 0.0002 ); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 4bc368f..e3c9505 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -12,13 +12,13 @@ import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.21; private static final int SPEAKER_TAG_ID = 7; - private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.70); + private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.14, 0.0, 0.65); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), TARGET_DELIVERY_POSITION = new MirrorableTranslation3d(2.5, 7, 0, true); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 2b60724..803d801 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -28,13 +28,13 @@ public class OperatorConstants { DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), COLLECT_TRIGGER = DRIVER_CONTROLLER.leftTrigger()/*.or(DEBUGGING_BUTTON)*/, CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftBumper().or(OPERATOR_CONTROLLER.k()), - FACE_AMP_TRIGGER = DRIVER_CONTROLLER.x(), FACE_SPEAKER_TRIGGER = DRIVER_CONTROLLER.a(), SECOND_CONTINUE_TRIGGER = OPERATOR_CONTROLLER.m(), CLOSE_SHOT_TRIGGER = OPERATOR_CONTROLLER.x(), TURN_AUTOMATIC_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTOMATIC_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), SCORE_IN_AMP_TRIGGER = OPERATOR_CONTROLLER.a(), + FACE_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(SCORE_IN_AMP_TRIGGER.negate()), AUTONOMOUS_SCORE_IN_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), SHOOT_AT_SPEAKER_TRIGGER = OPERATOR_CONTROLLER.s().or(DRIVER_CONTROLLER.rightBumper()), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), @@ -47,7 +47,7 @@ public class OperatorConstants { RESET_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(), EJECT_NOTE_TRIGGER = OPERATOR_CONTROLLER.e(), AMPLIFY_LEDS_TRIGGER = OPERATOR_CONTROLLER.h(), - WARM_SPEAKER_SHOOTING_TRIGGER = OPERATOR_CONTROLLER.w(), + WARM_SPEAKER_SHOOTING_TRIGGER = OPERATOR_CONTROLLER.w().and(SHOOT_AT_SPEAKER_TRIGGER.negate()), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), MOVE_CLIMBER_UP_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f2(), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 5884d1f..f4f342c 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,12 +8,12 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 50, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; public static final double CLOSE_SHOT_VELOCITY_METERS_PER_SECOND = 45; // public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(60.5); - public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(40); + public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(60.5); public static final Pose3d ROBOT_RELATIVE_PIVOT_POINT = new Pose3d(-0.025, 0, 0.190, new Rotation3d(0, 0, Math.PI)); public static final Transform3d PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.1224469, 0, -0.046625, new Rotation3d()); diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index 05bf276..538c287 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -8,6 +8,7 @@ import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import frc.trigon.robot.subsystems.shooter.ShooterConstants; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.trigon.utilities.mirrorable.MirrorableRotation2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; @@ -29,7 +30,7 @@ private ShootingCalculations() { */ public void updateCalculationsForDelivery() { Logger.recordOutput("ShootingCalculations/TargetDeliveryPose", FieldConstants.TARGET_DELIVERY_POSITION.get()); - targetShootingState = calculateTargetShootingState(FieldConstants.TARGET_DELIVERY_POSITION, ShootingConstants.DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND, true); + targetShootingState = calculateTargetShootingState(FieldConstants.TARGET_DELIVERY_POSITION, ShootingConstants.DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND, false); } /** @@ -47,6 +48,8 @@ public TargetShootingState getTargetShootingState() { return targetShootingState; } + LoggedDashboardNumber noteSpeedLoss = new LoggedDashboardNumber("NoteSpeedLoss", 0.69); + /** * Converts a given shooter's angular velocity to the shooter's tangential velocity. * This is specific for our shooter's specs. @@ -55,7 +58,7 @@ public TargetShootingState getTargetShootingState() { * @return the tangential velocity of the shooter */ public double angularVelocityToTangentialVelocity(double angularVelocity) { - return angularVelocity / ShooterConstants.ROTATIONS_TO_METERS; + return angularVelocity * (ShooterConstants.METERS_TO_ROTATIONS * noteSpeedLoss.get()); } /** @@ -66,7 +69,7 @@ public double angularVelocityToTangentialVelocity(double angularVelocity) { * @return the angular velocity of the shooter */ public double tangentialVelocityToAngularVelocity(double tangentialVelocity) { - return tangentialVelocity * ShooterConstants.ROTATIONS_TO_METERS; + return tangentialVelocity / (ShooterConstants.METERS_TO_ROTATIONS * noteSpeedLoss.get()); } /** @@ -100,7 +103,7 @@ private Translation2d predictFutureTranslation(double predictionTime) { * @return the target state of the robot so the note will reach the shooting target, as a {@linkplain ShootingCalculations.TargetShootingState} */ private TargetShootingState calculateTargetShootingState(MirrorableTranslation3d shootingTarget, double standingShootingVelocityRotationsPerSecond, boolean reachFromAbove) { - final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Translation2d currentTranslation = predictFutureTranslation(0); final MirrorableRotation2d standingTargetRobotAngle = getAngleToTarget(currentTranslation, shootingTarget); final double standingTangentialVelocity = angularVelocityToTangentialVelocity(standingShootingVelocityRotationsPerSecond); final Rotation2d standingTargetPitch = calculateTargetPitch(standingTangentialVelocity, reachFromAbove, currentTranslation, standingTargetRobotAngle, shootingTarget); @@ -161,6 +164,8 @@ private Rotation2d getPitch(Translation3d vector) { return new Rotation2d(Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); } + final LoggedDashboardNumber heightAddition = new LoggedDashboardNumber("HeightAddition"); + /** * Calculates the optimal pitch for the given parameters, using the Projectile Motion calculation. * @@ -173,10 +178,13 @@ private Rotation2d getPitch(Translation3d vector) { * @return the pitch the pitcher should reach in order to shoot to the shooting target */ private Rotation2d calculateTargetPitch(double noteTangentialVelocity, boolean reachFromAbove, Translation2d currentTranslation, MirrorableRotation2d targetRobotAngle, MirrorableTranslation3d shootingTarget) { - final Pose3d endEffectorFieldRelativePose = calculateShooterEndEffectorFieldRelativePose(RobotContainer.PITCHER.getTargetPitch(), currentTranslation, targetRobotAngle); + Rotation2d lastCalculatedPitch = RobotContainer.PITCHER.getTargetPitch(); + if (lastCalculatedPitch == null || Double.isNaN(lastCalculatedPitch.getRadians())) + lastCalculatedPitch = PitcherConstants.DEFAULT_PITCH; + final Pose3d endEffectorFieldRelativePose = calculateShooterEndEffectorFieldRelativePose(lastCalculatedPitch, currentTranslation, targetRobotAngle); final double endEffectorXYDistanceFromShootingTarget = endEffectorFieldRelativePose.getTranslation().toTranslation2d().getDistance(shootingTarget.get().toTranslation2d()); final double endEffectorHeightDifferenceFromTarget = shootingTarget.get().getZ() - endEffectorFieldRelativePose.getZ(); - return calculateTargetPitchUsingProjectileMotion(noteTangentialVelocity, endEffectorXYDistanceFromShootingTarget, endEffectorHeightDifferenceFromTarget, reachFromAbove); + return calculateTargetPitchUsingProjectileMotion(noteTangentialVelocity, endEffectorXYDistanceFromShootingTarget + heightAddition.get(), endEffectorHeightDifferenceFromTarget, reachFromAbove); } /** @@ -204,9 +212,7 @@ private Rotation2d calculateTargetPitchUsingProjectileMotion(double noteTangenti final double numerator = reachFromAbove ? velocitySquared + squareRoot : velocitySquared - squareRoot; final double denominator = gForce * shooterEndEffectorXYDistanceFromShootingTarget; final double fraction = numerator / denominator; - double angleRadians = Math.atan(fraction); - if (Double.isNaN(angleRadians) || Double.isInfinite(angleRadians) || angleRadians < 0) - angleRadians = PitcherConstants.DEFAULT_PITCH.getRadians(); + final double angleRadians = Math.atan(fraction); Logger.recordOutput("ShootingCalculations/TargetPitch", Math.toDegrees(angleRadians)); return Rotation2d.fromRadians(angleRadians); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 45c69b9..8dbaf16 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -18,7 +18,7 @@ public class AprilTagCamera { protected final String name; private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); - private final Transform3d robotCenterToCamera; + protected final Transform3d robotCenterToCamera; private final double thetaStandardDeviationExponent, translationStandardDeviationExponent; @@ -52,6 +52,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT public void update() { aprilTagCameraIO.updateInputs(inputs); + Logger.processInputs("Cameras/" + name, inputs); robotPose = calculateBestRobotPose(); logCameraInfo(); @@ -131,8 +132,11 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); - final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(gyroHeading); - return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); + return new Pose2d(fieldRelativeRobotPose, gyroHeading.minus(robotCenterToCamera.getRotation().toRotation2d())).transformBy(toTransform2d(robotCenterToCamera.inverse())).getTranslation(); + } + + private Transform2d toTransform2d(Transform3d transform3d) { + return new Transform2d(transform3d.getTranslation().toTranslation2d(), transform3d.getRotation().toRotation2d()); } private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { @@ -143,10 +147,10 @@ private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeadi } private double getRobotPlaneTargetYawRadians() { - double targetYawRadians = -inputs.bestTargetRelativeYawRadians; + double targetYawRadians = inputs.bestTargetRelativeYawRadians; for (int i = 0; i < AprilTagCameraConstants.CALCULATE_YAW_ITERATIONS; i++) { final double projectedPitch = projectToPlane(-robotCenterToCamera.getRotation().getY(), targetYawRadians + Math.PI / 2); - targetYawRadians = -inputs.bestTargetRelativeYawRadians - Math.tan(projectedPitch) * -inputs.bestTargetRelativePitchRadians; + targetYawRadians = inputs.bestTargetRelativeYawRadians - Math.tan(projectedPitch) * inputs.bestTargetRelativePitchRadians; } return projectToPlane(targetYawRadians, robotCenterToCamera.getRotation().getY()); } @@ -159,7 +163,7 @@ private double projectToPlane(double targetAngleRadians, double cameraAngleRadia private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); - double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); + double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() + inputs.bestTargetRelativePitchRadians); return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); } @@ -193,10 +197,10 @@ private boolean isWithinBestTagRangeForSolvePNP() { } private void logCameraInfo() { - Logger.processInputs("Cameras/" + name, inputs); if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) logUsedTags(); - if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { + + if (inputs.hasResult && inputs.distanceFromBestTag != 0 && robotPose != null) { logEstimatedRobotPose(); logSolvePNPPose(); } else { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index ebb4cd5..1113d13 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -9,9 +9,9 @@ import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, -0.005, new Rotation3d(0, 0, 0)); - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; - static final int CALCULATE_YAW_ITERATIONS = 3; + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0.005, new Rotation3d(0, 0, 0)); + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 1.5; + static final int CALCULATE_YAW_ITERATIONS = 5; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; public enum AprilTagCameraType { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 1375f52..a8c36a5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,15 +1,13 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; @@ -61,6 +59,8 @@ private Point getTagCenter(List tagCorners) { return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); } + LoggedDashboardNumber l = new LoggedDashboardNumber("Roll", 0); + /** * Estimates the camera's rotation relative to the apriltag. * @@ -73,6 +73,18 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { if (photonCamera.getCameraMatrix().isPresent()) return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); return null; +// final PhotonTrackedTarget bestTarget = result.getBestTarget(); +// var a = new Translation2d( +// Units.degreesToRadians(bestTarget.getYaw()), +// Units.degreesToRadians(bestTarget.getPitch()) +// ); +//// var b = a.rotateBy(Rotation2d.fromDegrees(7.549864866)); +// var b = a.rotateBy(Rotation2d.fromDegrees(l.get())); +// return new Rotation3d( +// 0, +// b.getY(), +// b.getX() +// ); } /** @@ -82,8 +94,20 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @return the estimated pose */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; +// final Pose3d rawTgPose = FieldConstants.TAG_ID_TO_POSE.get(4); +// final Pose3d rawTg1Pose = FieldConstants.TAG_ID_TO_POSE.get(3); +// PhotonTrackedTarget tag4 = null, tag3 = null; +// for (PhotonTrackedTarget re : result.getTargets()) { +// if (re.getFiducialId() == 4) +// tag4 = re; +// if (re.getFiducialId() == 3) +// tag3 = re; +// } +// var tag4Estimate = rawTgPose.transformBy(tag4.getBestCameraToTarget().inverse()); +// var tag3Estimate = rawTg1Pose.transformBy(tag3.getBestCameraToTarget().inverse()); +// Logger.recordOutput("Diff", tag4Estimate.minus(tag3Estimate)); + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } @@ -95,7 +119,7 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { private int[] getVisibleTagIDs(PhotonPipelineResult result) { final int[] visibleTagIDs = new int[result.getTargets().size()]; - + for (int i = 0; i < visibleTagIDs.length; i++) visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); return visibleTagIDs; @@ -118,7 +142,9 @@ private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { var yaw = new Rotation2d(fx, xOffset); // correct pitch based on yaw var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + var nonRolled = new Translation2d(-yaw.getRadians(), -pitch.getRadians()); + var rolled = nonRolled.rotateBy(Rotation2d.fromDegrees(l.get())); - return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + return new Rotation3d(0, rolled.getY(), rolled.getX()); } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index 72d7479..d8967b5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.numbers.N3; public class PoseEstimatorConstants { - public static final double ODOMETRY_FREQUENCY_HERTZ = 50; + public static final double ODOMETRY_FREQUENCY_HERTZ = 250; /** * The vector represents how ambiguous each value of the odometry is. diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index ac1059a..bffac19 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -43,8 +43,8 @@ public class Climber extends MotorSubsystem { public Climber() { setName("Climber"); - configurePositionResettingLimitSwitch(); Commands.getDelayedCommand(3, this::configureChangingDefaultCommand).schedule(); + Commands.getDelayedCommand(3, this::configurePositionResettingLimitSwitch).schedule(); } @Override @@ -157,7 +157,7 @@ private double toRotations(double meters) { } private void configurePositionResettingLimitSwitch() { - final Trigger limitSwitchTrigger = new Trigger(() -> ClimberConstants.LIMIT_SWITCH.getBinaryValue() && !CommandConstants.IS_CLIMBING); + final Trigger limitSwitchTrigger = new Trigger(() -> !ClimberConstants.LIMIT_SWITCH.getBinaryValue() && !CommandConstants.IS_CLIMBING); limitSwitchTrigger.and(() -> masterMotor.getSignal(TalonFXSignal.POSITION) != 0).debounce(ClimberConstants.LIMIT_SWITCH_PRESSED_THRESHOLD_SECONDS).whileTrue(new InstantCommand(this::resetPosition).repeatedly().ignoringDisable(true)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 48a0f41..49e5059 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -56,7 +56,7 @@ public class ClimberConstants { MAX_CLIMBING_VELOCITY = 1, MAX_CLIMBING_ACCELERATION = 1; private static final double - NON_CLIMBING_P = RobotHardwareStats.isSimulation() ? 30 : 0, + NON_CLIMBING_P = RobotHardwareStats.isSimulation() ? 30 : 15, NON_CLIMBING_I = 0, NON_CLIMBING_D = 0, NON_CLIMBING_KS = RobotHardwareStats.isSimulation() ? 0.030713 : 0.3081, diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 791fabd..b38ddf2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -169,7 +169,7 @@ public enum ElevatorState { RESTING(-0.005, 100), FEEDING_FOR_CLOSE_SHOT(0, 100), SCORE_AMP(0.45, 100), - SCORE_TRAP(0.5, 10), + SCORE_TRAP(0.53, 10), SCORE_TRAP_LOWERED(0.15, 10), FINISH_TRAP(0, 10); diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index fa6765c..7e8d290 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -6,6 +6,7 @@ import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.misc.ShootingCalculations; @@ -15,6 +16,7 @@ import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; +import org.trigon.utilities.Conversions; public class Pitcher extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); @@ -36,6 +38,10 @@ public void stop() { public void updatePeriodically() { motor.update(); PitcherConstants.ENCODER.update(); + var a = Conversions.rotationsToDegrees(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION); + Logger.recordOutput("CurrentPitchDegrees", Conversions.rotationsToDegrees(motor.getSignal(TalonFXSignal.POSITION) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION)); + if (!DriverStation.isDisabled() && Math.abs(a - 478.8756) > 0.01) + Logger.recordOutput("TargetPitchDegrees", a); } @Override @@ -57,6 +63,11 @@ public void updateLog(SysIdRoutineLog log) { .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / PitcherConstants.GEAR_RATIO)); } + @Override + public void setBrake(boolean brake) { + motor.setBrake(brake); + } + @Override public SysIdRoutine.Config getSysIdConfig() { return PitcherConstants.SYS_ID_CONFIG; @@ -72,7 +83,15 @@ public Rotation2d getTargetPitch() { } public Rotation2d getCurrentPitch() { - return Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.POSITION)); + return Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.POSITION) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION); + } + + public double getRotorPosition() { + return motor.getSignal(TalonFXSignal.ROTOR_POSITION); + } + + public double getEncoderPosition() { + return PitcherConstants.ENCODER.getSignal(CANcoderSignal.POSITION) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION; } void pitchToShootingTarget() { @@ -80,10 +99,12 @@ void pitchToShootingTarget() { } void setTargetPitch(Rotation2d targetPitch) { - if (targetPitch == null) + if (targetPitch == null || Double.isNaN(targetPitch.getRadians())) { + this.targetPitch = null; return; + } - motor.setControl(motionMagicPositionRequest.withPosition(targetPitch.getRotations())); + motor.setControl(motionMagicPositionRequest.withPosition(targetPitch.getRotations() + PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION)); Logger.recordOutput("Pitcher/TargetPitch", targetPitch.getDegrees()); this.targetPitch = targetPitch; } diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java index c77ab16..66512fb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java @@ -1,9 +1,11 @@ package frc.trigon.robot.subsystems.pitcher; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.GearRatioCalculationCommand; import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.NetworkTablesCommand; @@ -16,6 +18,15 @@ public static Command getDebuggingCommand() { ); } + public static Command getGearRatioCalibrationCommand() { + return new GearRatioCalculationCommand( + RobotContainer.PITCHER::getRotorPosition, + RobotContainer.PITCHER::getEncoderPosition, + (voltage) -> RobotContainer.PITCHER.drive(Units.Volt.of(voltage)), + RobotContainer.PITCHER + ); + } + public static Command getSetTargetPitchCommand(Rotation2d targetPitch) { return new StartEndCommand( () -> RobotContainer.PITCHER.setTargetPitch(targetPitch), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index f7e6f24..e5760c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -39,26 +39,27 @@ public class PitcherConstants { RobotConstants.CANIVORE_NAME ); - private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; + private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final SensorDirectionValue SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; private static final AbsoluteSensorRangeValue ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; - private static final double OFFSET = Conversions.degreesToRotations(11.337891 + 90); + public static final double GRAVITY_POSITION_TO_REAL_POSITION = Conversions.degreesToRotations(-118.8756); + private static final double OFFSET = -0.0487158583333333; private static final double - MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 300, + MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 180, MOTION_MAGIC_I = 0, MOTION_MAGIC_D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 1.0346 : 0.11847, - KV = RobotHardwareStats.isSimulation() ? 41 : 42, - KA = RobotHardwareStats.isSimulation() ? 0.85062 : 0.98195, - KG = RobotHardwareStats.isSimulation() ? 0.04366 : 0.10022; + KS = RobotHardwareStats.isSimulation() ? 1.0346 : 0.20177, + KV = RobotHardwareStats.isSimulation() ? 41 : 40.163, + KA = RobotHardwareStats.isSimulation() ? 0.85062 : 0, + KG = RobotHardwareStats.isSimulation() ? 0.04366 : 0.087458; private static final double - MOTION_MAGIC_CRUISE_VELOCITY = 0.31, - MOTION_MAGIC_ACCELERATION = 11, - MOTION_MAGIC_JERK = 110; + MOTION_MAGIC_CRUISE_VELOCITY = 0.28, + MOTION_MAGIC_ACCELERATION = 4, + MOTION_MAGIC_JERK = 40; static final boolean FOC_ENABLED = true; - static final double GEAR_RATIO = 352.8; + static final double GEAR_RATIO = 355.5; private static final int MOTOR_AMOUNT = 1; private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); @@ -79,8 +80,8 @@ public class PitcherConstants { ); static final SysIdRoutine.Config SYS_ID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(0.5).per(Units.Second), - Units.Volts.of(5), + Units.Volts.of(1).per(Units.Second), + Units.Volts.of(4), Units.Second.of(1000) ); @@ -90,7 +91,7 @@ public class PitcherConstants { ); public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(30); - static final double PITCH_TOLERANCE_DEGREES = 0.6; + static final double PITCH_TOLERANCE_DEGREES = 0.4; static { configuredEncoder(); @@ -116,9 +117,9 @@ private static void configureMotor() { config.MotorOutput.Inverted = INVERTED_VALUE; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Conversions.degreesToRotations(24); + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Conversions.degreesToRotations(29) + PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(90); + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(90) + PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION; config.Feedback.RotorToSensorRatio = GEAR_RATIO; config.Feedback.FeedbackRemoteSensorID = ENCODER_ID; @@ -136,6 +137,8 @@ private static void configureMotor() { MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); + MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); + MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); } private static void configuredEncoder() { diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 881711f..14aa96e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -53,7 +53,8 @@ public class ShooterConstants { static final SpeedMechanism2d SHOOTING_MECHANISM = new SpeedMechanism2d("ShooterMechanism", MAX_DISPLAYABLE_VELOCITY); private static final double WHEEL_DIAMETER_METERS = 0.1016; - public static final double ROTATIONS_TO_METERS = GEAR_RATIO / (WHEEL_DIAMETER_METERS * Math.PI); + private static final double NOTE_SPEED_LOSE = 1; + public static final double METERS_TO_ROTATIONS = (WHEEL_DIAMETER_METERS * Math.PI * NOTE_SPEED_LOSE); static final double TOLERANCE_ROTATIONS_PER_SECOND = 2; static { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index e1fad94..d8e6699 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -138,6 +138,11 @@ public void runWheelRadiusCharacterization(double omegaRadiansPerSecond) { selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)); } + public Rotation2d getDriveRelativeAngle() { + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; + } + /** * Locks the swerve, so it'll be hard to move it. This will make the modules look in the middle of a robot in an "x" shape. */ @@ -317,7 +322,11 @@ private boolean atTranslationPosition(double currentTranslationPosition, double private double calculateProfiledAngleSpeedToTargetAngle(MirrorableRotation2d targetAngle) { final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), targetAngle.get().getDegrees())); + final Rotation2d mirroredTargetAngle = targetAngle.get(); + Logger.recordOutput("Swerve/TurnToAngle/CurrentAngleDegrees", currentAngle.getDegrees()); + Logger.recordOutput("Swerve/TurnToAngle/TargetAngleDegrees", mirroredTargetAngle.getDegrees()); + Logger.recordOutput("Swerve/TurnToAngle/ProfiledTargetAngleDegrees", SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.getSetpoint().position); + return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), mirroredTargetAngle.getDegrees())); } private ChassisSpeeds selfRelativeSpeedsFromFieldRelativePowers(double xPower, double yPower, double thetaPower) { @@ -329,16 +338,11 @@ private ChassisSpeeds fieldRelativeSpeedsToSelfRelativeSpeeds(ChassisSpeeds fiel return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getDriveRelativeAngle()); } - private Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; - } - private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaPower) { return new ChassisSpeeds( xPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, yPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, - Math.pow(thetaPower, 2) * Math.signum(thetaPower) * SwerveConstants.MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND + (thetaPower * thetaPower) * Math.signum(thetaPower) * SwerveConstants.MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index f4fac23..3f026e3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -15,7 +15,6 @@ import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Signal; -import org.trigon.utilities.Conversions; import java.util.function.DoubleSupplier; @@ -23,24 +22,30 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 2.987403154373169, - GYRO_MOUNT_POSITION_PITCH = -2.3428704738616943, - GYRO_MOUNT_POSITION_ROLL = -1.5769307613372803; + GYRO_MOUNT_POSITION_PITCH = -1.9303209781646729, + GYRO_MOUNT_POSITION_ROLL = -1.6774744987487793, + GYRO_MOUNT_POSITION_YAW = 3.2677361965179443; + private static final double lol = 1.8; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(225.263672 - 360), - FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-256.904297 + 360), - REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(108.369141), - REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-36.035156); + FRONT_LEFT_WHEEL_DIAMETER_METERS = 0.050064 * lol, + FRONT_RIGHT_WHEEL_DIAMETER_METERS = 0.048810 * lol, + REAR_LEFT_WHEEL_DIAMETER_METERS = 0.048096 * lol, + REAR_RIGHT_WHEEL_DIAMETER_METERS = 0.048104 * lol; + private static final double + FRONT_LEFT_STEER_ENCODER_OFFSET = 0.374267578125, + FRONT_RIGHT_STEER_ENCODER_OFFSET = -0.286376953125, + REAR_LEFT_STEER_ENCODER_OFFSET = -0.30078125, + REAR_RIGHT_STEER_ENCODER_OFFSET = 0.10009765625; private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, REAR_LEFT_ID = 3, REAR_RIGHT_ID = 4; static final SwerveModule[] SWERVE_MODULES = { - new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET), - new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET), - new SwerveModule(REAR_LEFT_ID, REAR_LEFT_STEER_ENCODER_OFFSET), - new SwerveModule(REAR_RIGHT_ID, REAR_RIGHT_STEER_ENCODER_OFFSET) + new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER_METERS), + new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER_METERS), + new SwerveModule(REAR_LEFT_ID, REAR_LEFT_STEER_ENCODER_OFFSET, REAR_LEFT_WHEEL_DIAMETER_METERS), + new SwerveModule(REAR_RIGHT_ID, REAR_RIGHT_STEER_ENCODER_OFFSET, REAR_RIGHT_WHEEL_DIAMETER_METERS) }; private static final DoubleSupplier SIMULATION_YAW_VELOCITY_SUPPLIER = () -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond; @@ -77,7 +82,7 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0.05) : - new PIDConstants(5, 0, 0), + new PIDConstants(5.6, 0, 0.12), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(9, 0, 0) : new PIDConstants(6.5, 0, 0), @@ -85,8 +90,8 @@ public class SwerveConstants { new PIDConstants(8.9, 0, 0) : new PIDConstants(3, 0, 0); private static final double - MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 720, - MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 720; + MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 700, + MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 660; private static final TrapezoidProfile.Constraints ROTATION_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_ROTATION_VELOCITY, MAX_ROTATION_ACCELERATION diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index fb67b12..6259f04 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -24,16 +24,18 @@ public class SwerveModule { private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0); private final VoltageOut driveVoltageRequest = new VoltageOut(0); private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0); + private final double wheelDiameterMeters; private boolean driveMotorClosedLoop = false; private double[] latestOdometryDrivePositions = new double[0], latestOdometrySteerPositions = new double[0]; private SwerveModuleState targetState = new SwerveModuleState(); - public SwerveModule(int moduleID, double offsetRotations) { + public SwerveModule(int moduleID, double offsetRotations, double wheelDiameterMeters) { driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); steerEncoder = new CANcoderEncoder(moduleID, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + this.wheelDiameterMeters = wheelDiameterMeters; configureHardware(offsetRotations); } @@ -61,7 +63,7 @@ void setBrake(boolean brake) { void update() { driveMotor.update(); steerMotor.update(); - steerEncoder.update(); + latestOdometryDrivePositions = driveMotor.getThreadedSignal(TalonFXSignal.POSITION); latestOdometrySteerPositions = steerMotor.getThreadedSignal(TalonFXSignal.POSITION); } @@ -111,7 +113,7 @@ SwerveModuleState getTargetState() { } private double driveRotationsToMeters(double rotations) { - return Conversions.rotationsToDistance(rotations, SwerveModuleConstants.WHEEL_DIAMETER_METERS); + return Conversions.rotationsToDistance(rotations, wheelDiameterMeters); } /** @@ -130,7 +132,7 @@ private void setTargetVelocity(double targetVelocityMetersPerSecond, Rotation2d } private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond) { - final double targetVelocityRotationsPerSecond = Conversions.distanceToRotations(targetVelocityMetersPerSecond, SwerveModuleConstants.WHEEL_DIAMETER_METERS); + final double targetVelocityRotationsPerSecond = Conversions.distanceToRotations(targetVelocityMetersPerSecond, wheelDiameterMeters); driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond)); } @@ -174,6 +176,9 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { + steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); + steerEncoder.registerSignal(CANcoderSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); @@ -182,8 +187,5 @@ private void configureSignals() { steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); - - steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); - steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index bf3c05c..c6bae68 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -14,8 +14,8 @@ public class SwerveModuleConstants { DRIVE_GEAR_RATIO = 6.12, STEER_GEAR_RATIO = 12.8; private static final double - DRIVE_OPEN_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.1, - DRIVE_CLOSED_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.1; + DRIVE_OPEN_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.2, + DRIVE_CLOSED_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.2; private static final InvertedValue DRIVE_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, STEER_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; @@ -25,19 +25,19 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 100, - STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 50; + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 80, + STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 40; private static final double STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 28, DRIVE_MOTOR_I = 0, DRIVE_MOTOR_D = 0, - DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 0, + DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 6.1, DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0, - DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 0; + DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 2.7372; static final boolean ENABLE_FOC = true; static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = generateDriveConfiguration(), @@ -60,7 +60,7 @@ public class SwerveModuleConstants { Units.Second.of(1000) ); - static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.048981 * 2; + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.048445 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { @@ -110,7 +110,7 @@ private static TalonFXConfiguration generateSteerConfiguration() { config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.RotorToSensorRatio = STEER_GEAR_RATIO; - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; config.Slot0.kP = STEER_MOTOR_P; config.Slot0.kI = STEER_MOTOR_I; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java index fc43d17..1dbb431 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java @@ -8,6 +8,7 @@ import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; +import org.littletonrobotics.junction.AutoLogOutput; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -49,6 +50,7 @@ public boolean isFeeding() { targetState == TransporterConstants.TransporterState.SCORE_TRAP; } + @AutoLogOutput(key = "HasNote???") public boolean isNoteDetected() { return !beamBreak.getBinaryValue(); } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0e80a16..1d63e94 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-87-g471c90e8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-87-g471c90e8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-87-g471c90e8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.3.1" + "version": "dev-v2024.3.0-87-g471c90e8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.3.1" + "version": "dev-v2024.3.0-87-g471c90e8" } ] } \ No newline at end of file From 9c8c0d200d91d61de0f11ac1e223822dcfd005c7 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:20:44 +0200 Subject: [PATCH 6/6] Pose IOS Commit --- .pathplanner/settings.json | 8 +- build.gradle | 2 +- simgui-window.json | 12 +-- src/main/deploy/2024-crescendo.json | 2 +- .../Pathplanner/paths/Picture2Collect2.path | 8 +- .../Pathplanner/paths/Picture2Collect4.path | 8 +- .../Pathplanner/paths/Picture2Collect6.path | 8 +- .../paths/Picture2CollectAndShoot1.path | 8 +- .../Pathplanner/paths/Picture2Shoot3.path | 8 +- .../Pathplanner/paths/Picture2Shoot5.path | 8 +- .../Pathplanner/paths/Picture3Collect4.path | 8 +- .../paths/Picture3CollectAndShoot2.path | 8 +- .../paths/Picture3CollectAndShoot3.path | 8 +- .../Pathplanner/paths/Picture4Collect3.path | 8 +- .../Pathplanner/paths/Picture4Collect5.path | 8 +- .../Pathplanner/paths/Picture4Collect7.path | 8 +- .../paths/Picture4CollectAndShoot1.path | 8 +- .../paths/Picture4CollectAndShoot2.path | 8 +- .../Pathplanner/paths/Picture4Shoot4.path | 8 +- .../Pathplanner/paths/Picture4Shoot6.path | 8 +- .../pathplanner/autos/CloseNotesLeft.auto | 19 +--- .../pathplanner/autos/CloseNotesMiddle.auto | 2 +- .../paths/CloseLeftNotesShoot2.path | 8 +- .../pathplanner/paths/CloseNotesLeft5.path | 8 +- .../paths/CloseNotesLeftCollect1.path | 8 +- .../paths/CloseNotesLeftCollectAndShoot3.path | 8 +- .../paths/CloseNotesLeftCollectAndShoot4.path | 8 +- .../pathplanner/paths/CloseNotesMiddle2.path | 8 +- .../pathplanner/paths/CloseNotesMiddle3.path | 8 +- .../pathplanner/paths/CloseNotesMiddle4.path | 8 +- .../pathplanner/paths/CloseNotesMiddle5.path | 8 +- .../CloseNotesMiddleCollectAndShoot1.path | 8 +- .../paths/FinishOrbitNotesCollect1.path | 8 +- .../paths/FinishOrbitNotesShoot3.path | 8 +- .../FinishOrbitNotesShootAndCollect2.path | 8 +- .../paths/New New New New Path.path | 8 +- .../pathplanner/paths/New New New Path.path | 8 +- .../pathplanner/paths/New New Path.path | 8 +- .../deploy/pathplanner/paths/New Path.path | 8 +- .../deploy/pathplanner/paths/Option3P1.path | 8 +- .../deploy/pathplanner/paths/Option3P2.path | 8 +- .../deploy/pathplanner/paths/Option3P3.path | 8 +- .../deploy/pathplanner/paths/Option3P4.path | 8 +- .../deploy/pathplanner/paths/Option3P5.path | 8 +- .../deploy/pathplanner/paths/Option3P6.path | 8 +- .../deploy/pathplanner/paths/Option4P1.path | 8 +- .../deploy/pathplanner/paths/Option4P2.path | 8 +- .../deploy/pathplanner/paths/Option4P3.path | 8 +- .../deploy/pathplanner/paths/Option4P4.path | 8 +- .../deploy/pathplanner/paths/Option4P5.path | 8 +- .../deploy/pathplanner/paths/Option4P6.path | 8 +- .../deploy/pathplanner/paths/Option4P7.path | 8 +- .../deploy/pathplanner/paths/Pic3skip.path | 8 +- .../pathplanner/paths/Picture3Collect1.path | 10 +- .../pathplanner/paths/Picture3ForthShoot.path | 8 +- .../paths/Picture3SecondCollect.path | 8 +- .../pathplanner/paths/Picture3Shoot1.path | 8 +- .../pathplanner/paths/Picture3Shoot5.path | 8 +- .../paths/Picture3SkipCollect1.path | 8 +- .../pathplanner/paths/Picture3SkipShoot2.path | 8 +- .../paths/Picture3ThirdCollect.path | 8 +- .../pathplanner/paths/Picture3ThirdShoot.path | 8 +- src/main/java/frc/trigon/robot/Robot.java | 58 ++++++----- .../java/frc/trigon/robot/RobotContainer.java | 6 +- .../robot/commands/CommandConstants.java | 4 +- .../robot/constants/CameraConstants.java | 6 +- .../robot/constants/FieldConstants.java | 13 +++ .../robot/constants/RobotConstants.java | 2 +- .../robot/misc/ShootingCalculations.java | 2 +- .../apriltagcamera/AprilTagCamera.java | 17 +++- .../AprilTagCameraConstants.java | 2 +- .../io/AprilTagPhotonCameraIO.java | 96 ++++++++++++++----- .../poseestimator/PoseEstimator6328.java | 3 + .../elevator/ElevatorConstants.java | 4 +- .../subsystems/pitcher/PitcherConstants.java | 14 +-- .../robot/subsystems/swerve/Swerve.java | 3 +- .../subsystems/swerve/SwerveConstants.java | 18 ++-- 77 files changed, 402 insertions(+), 341 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 84a6bf9..0b98907 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -13,9 +13,9 @@ "Picture4" ], "autoFolders": [], - "defaultMaxVel": 3.4, - "defaultMaxAccel": 3.4, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxVel": 2.5, + "defaultMaxAccel": 2.5, + "defaultMaxAngVel": 400.0, + "defaultMaxAngAccel": 400.0, "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index d48ee05..84ffc8b 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:c59c440' + implementation 'com.github.Programming-TRIGON:TRIGONLib:6e39012' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/simgui-window.json b/simgui-window.json index 0b3a77f..ca3d2c2 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -76,7 +76,7 @@ "###/SmartDashboard/AutoChooser": { "Collapsed": "0", "Pos": "-3,891", - "Size": "307,60" + "Size": "32,38" }, "###/SmartDashboard/Field": { "Collapsed": "0", @@ -86,12 +86,12 @@ "###FMS": { "Collapsed": "0", "Pos": "-1,712", - "Size": "202,214" + "Size": "32,38" }, "###Joysticks": { "Collapsed": "0", "Pos": "320,888", - "Size": "976,82" + "Size": "32,38" }, "###NetworkTables": { "Collapsed": "0", @@ -121,12 +121,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "1,346", - "Size": "232,254" + "Size": "32,38" }, "###Timing": { "Collapsed": "0", "Pos": "0,172", - "Size": "169,168" + "Size": "32,38" }, "Debug##Default": { "Collapsed": "0", @@ -136,7 +136,7 @@ "Robot State": { "Collapsed": "0", "Pos": "2,25", - "Size": "109,134" + "Size": "32,38" } } } diff --git a/src/main/deploy/2024-crescendo.json b/src/main/deploy/2024-crescendo.json index 17ea2ae..8cb837a 100644 --- a/src/main/deploy/2024-crescendo.json +++ b/src/main/deploy/2024-crescendo.json @@ -40,7 +40,7 @@ "ID": 3, "pose": { "translation": { - "x": 16.582342, + "x": 16.579342, "y": 4.982717999999999, "z": 1.4511020000000001 }, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect2.path b/src/main/deploy/Pathplanner/paths/Picture2Collect2.path index 1a6bf6e..2aaca77 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect2.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect2.path @@ -67,10 +67,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect4.path b/src/main/deploy/Pathplanner/paths/Picture2Collect4.path index ead3c7b..7b61a7b 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect4.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect4.path @@ -72,10 +72,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect6.path b/src/main/deploy/Pathplanner/paths/Picture2Collect6.path index 254631e..f638519 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect6.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect6.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path b/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path index 28c2dff..d3e2c4e 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path +++ b/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path b/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path index bfa3e4c..a0025b9 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path @@ -48,10 +48,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path b/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path index baee80b..d9098d1 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture3Collect4.path b/src/main/deploy/Pathplanner/paths/Picture3Collect4.path index 622208c..de9025f 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3Collect4.path +++ b/src/main/deploy/Pathplanner/paths/Picture3Collect4.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path index 1761b9f..0ccbd9e 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path +++ b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path @@ -83,10 +83,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path index 4acb329..ac3540d 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path +++ b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path @@ -78,10 +78,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect3.path b/src/main/deploy/Pathplanner/paths/Picture4Collect3.path index 3c44596..128a51c 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect3.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect3.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect5.path b/src/main/deploy/Pathplanner/paths/Picture4Collect5.path index a1513a1..2e067e6 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect5.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect5.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect7.path b/src/main/deploy/Pathplanner/paths/Picture4Collect7.path index a77b683..b817e69 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect7.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect7.path @@ -72,10 +72,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path index eb69802..9f015bc 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path +++ b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path index 9390fd3..e414156 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path +++ b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path b/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path index de7b186..2c907c2 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path b/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path index ac96e77..bdc121f 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto b/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto index 84a4bc1..6cf4725 100644 --- a/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto +++ b/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto @@ -28,26 +28,13 @@ { "type": "wait", "data": { - "waitTime": 0.3 + "waitTime": 0.6 } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "FeedNoteWithoutWaiting" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] + "name": "FeedNote" } }, { diff --git a/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto b/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto index d2c80a3..9d679e2 100644 --- a/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto +++ b/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 0.3 + "waitTime": 0.6 } }, { diff --git a/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path b/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path index 24accbe..325302b 100644 --- a/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path +++ b/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path b/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path index a46b969..746c4aa 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path @@ -73,10 +73,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path index e4aa057..6a24a42 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path @@ -79,10 +79,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path index aff64bd..f1a9e2b 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path index e486892..82f074e 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path @@ -79,10 +79,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path index 0595ee7..88c6e80 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path @@ -79,10 +79,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path index 0be13ee..d19500e 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path @@ -85,10 +85,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path index 9434908..3e67732 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path @@ -79,10 +79,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path index 261f255..222494b 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path index b100280..1ed9ec3 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path index 7ca42ac..fe79c31 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path index b3d2986..83598de 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path @@ -48,10 +48,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path index 608face..d1e90e6 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path @@ -127,10 +127,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path index 0153b02..a374255 100644 --- a/src/main/deploy/pathplanner/paths/New New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New New Path.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path index 46f15ee..fadcf8a 100644 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path index 4d7fc33..6caf738 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 522b77b..b8cc44d 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P1.path b/src/main/deploy/pathplanner/paths/Option3P1.path index 2a83835..424ca6e 100644 --- a/src/main/deploy/pathplanner/paths/Option3P1.path +++ b/src/main/deploy/pathplanner/paths/Option3P1.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P2.path b/src/main/deploy/pathplanner/paths/Option3P2.path index 08795a6..91d1465 100644 --- a/src/main/deploy/pathplanner/paths/Option3P2.path +++ b/src/main/deploy/pathplanner/paths/Option3P2.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P3.path b/src/main/deploy/pathplanner/paths/Option3P3.path index 2d137c7..ffaab5f 100644 --- a/src/main/deploy/pathplanner/paths/Option3P3.path +++ b/src/main/deploy/pathplanner/paths/Option3P3.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P4.path b/src/main/deploy/pathplanner/paths/Option3P4.path index e0b5532..191fad2 100644 --- a/src/main/deploy/pathplanner/paths/Option3P4.path +++ b/src/main/deploy/pathplanner/paths/Option3P4.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P5.path b/src/main/deploy/pathplanner/paths/Option3P5.path index 3f82648..b46665a 100644 --- a/src/main/deploy/pathplanner/paths/Option3P5.path +++ b/src/main/deploy/pathplanner/paths/Option3P5.path @@ -68,10 +68,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P6.path b/src/main/deploy/pathplanner/paths/Option3P6.path index 0472497..1827a49 100644 --- a/src/main/deploy/pathplanner/paths/Option3P6.path +++ b/src/main/deploy/pathplanner/paths/Option3P6.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P1.path b/src/main/deploy/pathplanner/paths/Option4P1.path index 13c8472..6845503 100644 --- a/src/main/deploy/pathplanner/paths/Option4P1.path +++ b/src/main/deploy/pathplanner/paths/Option4P1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P2.path b/src/main/deploy/pathplanner/paths/Option4P2.path index 173b101..0cbecb7 100644 --- a/src/main/deploy/pathplanner/paths/Option4P2.path +++ b/src/main/deploy/pathplanner/paths/Option4P2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P3.path b/src/main/deploy/pathplanner/paths/Option4P3.path index aba5941..8f21a24 100644 --- a/src/main/deploy/pathplanner/paths/Option4P3.path +++ b/src/main/deploy/pathplanner/paths/Option4P3.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P4.path b/src/main/deploy/pathplanner/paths/Option4P4.path index 32dd6e7..e2b6c3c 100644 --- a/src/main/deploy/pathplanner/paths/Option4P4.path +++ b/src/main/deploy/pathplanner/paths/Option4P4.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P5.path b/src/main/deploy/pathplanner/paths/Option4P5.path index 2bec671..f52c1d9 100644 --- a/src/main/deploy/pathplanner/paths/Option4P5.path +++ b/src/main/deploy/pathplanner/paths/Option4P5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P6.path b/src/main/deploy/pathplanner/paths/Option4P6.path index 920e85b..bafe706 100644 --- a/src/main/deploy/pathplanner/paths/Option4P6.path +++ b/src/main/deploy/pathplanner/paths/Option4P6.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P7.path b/src/main/deploy/pathplanner/paths/Option4P7.path index b364c15..1585b30 100644 --- a/src/main/deploy/pathplanner/paths/Option4P7.path +++ b/src/main/deploy/pathplanner/paths/Option4P7.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pic3skip.path b/src/main/deploy/pathplanner/paths/Pic3skip.path index bf8ef41..c30f1bc 100644 --- a/src/main/deploy/pathplanner/paths/Pic3skip.path +++ b/src/main/deploy/pathplanner/paths/Pic3skip.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3Collect1.path b/src/main/deploy/pathplanner/paths/Picture3Collect1.path index 07f6f6c..62c9f4c 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Collect1.path +++ b/src/main/deploy/pathplanner/paths/Picture3Collect1.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "FeedNoteWithoutWaiting" + "name": "TransporterCollection" } }, { @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path b/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path index c0c9ed6..2c2ddd5 100644 --- a/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path +++ b/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path b/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path index 10cf334..29c760e 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path +++ b/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path @@ -73,10 +73,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3Shoot1.path b/src/main/deploy/pathplanner/paths/Picture3Shoot1.path index fbd8646..48a8cd0 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Shoot1.path +++ b/src/main/deploy/pathplanner/paths/Picture3Shoot1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Picture3Shoot5.path b/src/main/deploy/pathplanner/paths/Picture3Shoot5.path index 2d15e25..f62d132 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Shoot5.path +++ b/src/main/deploy/pathplanner/paths/Picture3Shoot5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path b/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path index 28866b5..bd000e9 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path +++ b/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path b/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path index ae9fe20..83355d7 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path +++ b/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path b/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path index 018c0d0..28af3db 100644 --- a/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path +++ b/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path @@ -73,10 +73,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path b/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path index 2f4e153..89805f0 100644 --- a/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path +++ b/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.4, - "maxAcceleration": 3.4, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index 1a156b7..e11f4c8 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -7,14 +7,9 @@ import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.RobotConstants; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -38,32 +33,32 @@ public void robotInit() { Pathfinding.setPathfinder(new LocalADStarAK()); configLogger(); robotContainer = new RobotContainer(); - OperatorConstants.OPERATOR_CONTROLLER.numpad0().onTrue(new InstantCommand( - () -> { - CameraConstants.REAR_MIDDLE_CAMERA.update(); - pose1 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); - System.out.println("Pose 1: " + pose1); - } - )); - OperatorConstants.OPERATOR_CONTROLLER.numpad1().onTrue(new InstantCommand( - () -> { - CameraConstants.REAR_MIDDLE_CAMERA.update(); - pose2 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); - System.out.println("Pose 2: " + pose2); - } - )); - OperatorConstants.OPERATOR_CONTROLLER.numpad2().onTrue(new InstantCommand( - () -> { - final Translation2d translation2d = calc( - pose1.getRotation().getRadians(), pose2.getRotation().getRadians(), - pose1.getX(), pose2.getX(), - pose1.getY(), pose2.getY() - ); - System.out.println("__________________________" + translation2d); - Logger.recordOutput("TranslationX", translation2d.getX()); - Logger.recordOutput("TranslationY", translation2d.getY()); - } - )); +// OperatorConstants.OPERATOR_CONTROLLER.numpad0().onTrue(new InstantCommand( +// () -> { +// CameraConstants.REAR_MIDDLE_CAMERA.update(); +// pose1 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); +// System.out.println("Pose 1: " + pose1); +// } +// )); +// OperatorConstants.OPERATOR_CONTROLLER.numpad1().onTrue(new InstantCommand( +// () -> { +// CameraConstants.REAR_MIDDLE_CAMERA.update(); +// pose2 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); +// System.out.println("Pose 2: " + pose2); +// } +// )); +// OperatorConstants.OPERATOR_CONTROLLER.numpad2().onTrue(new InstantCommand( +// () -> { +// final Translation2d translation2d = calc( +// pose1.getRotation().getRadians(), pose2.getRotation().getRadians(), +// pose1.getX(), pose2.getX(), +// pose1.getY(), pose2.getY() +// ); +// System.out.println("__________________________" + translation2d); +// Logger.recordOutput("TranslationX", translation2d.getX()); +// Logger.recordOutput("TranslationY", translation2d.getY()); +// } +// )); } @Override @@ -98,6 +93,7 @@ public static double calculateY(double a1, double a2, double x1, double x2, doub @Override public void autonomousInit() { + RobotContainer.POSE_ESTIMATOR.resetPose(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); autonomousCommand = robotContainer.getAutonomousCommand(); if (autonomousCommand != null) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7c4225f..77dbe16 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -20,6 +20,8 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.Climber; +import frc.trigon.robot.subsystems.climber.ClimberCommands; +import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.elevator.Elevator; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; @@ -83,7 +85,7 @@ private void bindDefaultCommands() { PITCHER.setDefaultCommand(CommandConstants.PITCHER_RESTING_COMMAND); ELEVATOR.setDefaultCommand(new WaitUntilCommand(() -> ELEVATOR.isBelowCameraPlate() && ELEVATOR.didOpenElevator()).andThen(Commands.withoutRequirements(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.ALIGNING_FOR_AMP_BACKWARDS)).withTimeout(0.13).andThen(new InstantCommand(() -> ELEVATOR.setDidOpenElevator(false)))).alongWith(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.RESTING))); TRANSPORTER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(TRANSPORTER)); - CLIMBER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(CLIMBER)); + CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RESTING)); LEDStrip.setDefaultCommandForAllLEDS((ledStrip) -> Commands.getContinuousConditionalCommand(LEDStripCommands.getStaticColorCommand(Color.green, ledStrip), LEDStripCommands.getAnimateColorFlowCommand(new Color(0, 150, 255), 0.5, ledStrip), TRANSPORTER::isNoteDetected)); } @@ -91,7 +93,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); - OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand()); +// OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(Commands.getToggleBrakeCommand()); OperatorConstants.SHOOT_AT_SPEAKER_TRIGGER.whileTrue(Commands.getShootAtShootingTargetCommand(false)); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 403a7da..18e203a 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -94,12 +94,12 @@ public class CommandConstants { ALIGN_TO_RIGHT_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(60, false) : MirrorableRotation2d.fromDegrees(-120, false) + () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(-120, false) : MirrorableRotation2d.fromDegrees(60, false) ), ALIGN_TO_LEFT_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(-60, false) : MirrorableRotation2d.fromDegrees(120, false) + () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(120, false) : MirrorableRotation2d.fromDegrees(-60, false) ), ALIGN_TO_MIDDLE_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index a1673eb..d86349f 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -24,7 +24,7 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(-31.7), 0) ), REAR_MIDDLE_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(-0.025, 0, 0.63), + new Translation3d(-0.02, 0, 0.63), new Rotation3d(0, Units.degreesToRadians(-24.2), Units.degreesToRadians(180)) ); public static final AprilTagCamera @@ -47,7 +47,7 @@ public class CameraConstants { AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "RearMiddleCamera", REAR_MIDDLE_CENTER_TO_CAMERA, - 0.001, - 0.0002 + 0.010, + 0.005 ); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index e3c9505..22af270 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -6,9 +6,11 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import org.trigon.utilities.FilesHandler; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; +import java.io.IOException; import java.util.HashMap; public class FieldConstants { @@ -31,4 +33,15 @@ private static HashMap fieldLayoutToTagIdToPoseMap() { tagIdToPose.put(aprilTag.ID, aprilTag.pose); return tagIdToPose; } + + private static AprilTagFieldLayout loadFieldLayout() { + final String fieldFile = FilesHandler.DEPLOY_PATH + "2024-crescendo"; + AprilTagFieldLayout layout; + try { + layout = AprilTagFieldLayout.loadFromResource(fieldFile); + } catch (IOException e) { + throw new RuntimeException("Couldn't load field file!!!!!!!"); + } + return layout; + } } diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 29a37bc..2ab5d7a 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -7,7 +7,7 @@ public class RobotConstants { private static final boolean IS_SIMULATION = false, - IS_REPLAY = false; + IS_REPLAY = true; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index 538c287..6bc14cf 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -48,7 +48,7 @@ public TargetShootingState getTargetShootingState() { return targetShootingState; } - LoggedDashboardNumber noteSpeedLoss = new LoggedDashboardNumber("NoteSpeedLoss", 0.69); + LoggedDashboardNumber noteSpeedLoss = new LoggedDashboardNumber("NoteSpeedLoss", 0.685); /** * Converts a given shooter's angular velocity to the shooter's tangential velocity. diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 8dbaf16..b48d8fe 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.Robot; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; @@ -142,10 +143,21 @@ private Transform2d toTransform2d(Transform3d transform3d) { private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); + var actualDist = PoseEstimator6328.getInstance().getOdometryPose().getTranslation().getDistance(tagPose.getTranslation().toTranslation2d()); + if (inputs.visibleTagIDs[0] == 7) + Logger.recordOutput("ActualPitch", Units.radiansToDegrees(calc(tagPose, robotPlaneTargetYawRadians, actualDist))); final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); } + + private double calc(Pose3d usedTagPose, double robotPlaneTargetYaw, double dist) { + double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + var lol = dist * Math.cos(robotPlaneTargetYaw); + var lol2 = zDistanceToUsedTagMeters / lol; + var atan = Math.atan(lol2); + return atan - inputs.bestTargetRelativePitchRadians; + } private double getRobotPlaneTargetYawRadians() { double targetYawRadians = inputs.bestTargetRelativeYawRadians; for (int i = 0; i < AprilTagCameraConstants.CALCULATE_YAW_ITERATIONS; i++) { @@ -205,7 +217,7 @@ private void logCameraInfo() { logSolvePNPPose(); } else { Logger.recordOutput("Poses/Robot/" + name + "/Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); - Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", AprilTagCameraConstants.EMPTY_POSE_LIST); + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", new Pose3d[]{}); } } @@ -226,6 +238,7 @@ private void logEstimatedRobotPose() { } private void logSolvePNPPose() { - Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", inputs.cameraSolvePNPPose.plus(robotCenterToCamera.inverse())); + Logger.recordOutput("Test/Rotation", new double[]{Units.radiansToDegrees(inputs.cameraSolvePNPPose.getRotation().getY()), Units.radiansToDegrees(inputs.cameraSolvePNPPose.getRotation().getZ())}); + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", new Pose3d[]{inputs.cameraSolvePNPPose.plus(robotCenterToCamera.inverse())}); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 1113d13..ce8eaf9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -9,7 +9,7 @@ import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0.005, new Rotation3d(0, 0, 0)); + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 1.5; static final int CALCULATE_YAW_ITERATIONS = 5; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index a8c36a5..7218d3c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,18 +1,23 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +import java.util.HashMap; import java.util.List; public class AprilTagPhotonCameraIO extends AprilTagCameraIO { @@ -26,7 +31,7 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getBestTarget(latestResult).getPoseAmbiguity() < 0.4; if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); else @@ -34,6 +39,7 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + logEstimatedTagHeights(latestResult); final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); @@ -59,6 +65,29 @@ private Point getTagCenter(List tagCorners) { return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); } + /** + * Tag ID to a pair of Height Difference and Distance + */ + private HashMap> thingLol = new HashMap<>(); + + private void logEstimatedTagHeights(PhotonPipelineResult result) { + for (PhotonTrackedTarget target : result.getTargets()) { + thingLol.putIfAbsent(target.getFiducialId(), new Pair<>(0.0, 10000000.0)); + if (target.getPoseAmbiguity() > 0.4) + continue; + var dist = target.getBestCameraToTarget().getTranslation().getNorm(); + if (thingLol.get(target.getFiducialId()).getSecond() < dist) + continue; + var camPose = FieldConstants.TAG_ID_TO_POSE.get(target.getFiducialId()).plus(target.getBestCameraToTarget().inverse()); + var diff = camPose.getY() - 0.63; + if (Math.abs(diff) > 0.2) + continue; + thingLol.put(target.getFiducialId(), new Pair<>(diff, dist)); + Logger.recordOutput("TagHeights/Tag" + target.getFiducialId() + "/Distance", dist); + Logger.recordOutput("TagHeights/Tag" + target.getFiducialId() + "/Diff", camPose.getY() - 0.63); + } + } + LoggedDashboardNumber l = new LoggedDashboardNumber("Roll", 0); /** @@ -68,23 +97,17 @@ private Point getTagCenter(List tagCorners) { * @return the estimated rotation */ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - final List tagCorners = result.getBestTarget().getDetectedCorners(); - final Point tagCenter = getTagCenter(tagCorners); - if (photonCamera.getCameraMatrix().isPresent()) - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); - return null; -// final PhotonTrackedTarget bestTarget = result.getBestTarget(); -// var a = new Translation2d( -// Units.degreesToRadians(bestTarget.getYaw()), -// Units.degreesToRadians(bestTarget.getPitch()) -// ); -//// var b = a.rotateBy(Rotation2d.fromDegrees(7.549864866)); -// var b = a.rotateBy(Rotation2d.fromDegrees(l.get())); -// return new Rotation3d( -// 0, -// b.getY(), -// b.getX() -// ); +// final List tagCorners = getBestTarget(result).getDetectedCorners(); +// final Point tagCenter = getTagCenter(tagCorners); +// if (photonCamera.getCameraMatrix().isPresent()) +// return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); +// return null; + final PhotonTrackedTarget bestTarget = getBestTarget(result); + return new Rotation3d( + 0, + Units.degreesToRadians(bestTarget.getPitch()), + Units.degreesToRadians(bestTarget.getYaw()) + ); } /** @@ -111,22 +134,43 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); + var best = getBestTarget(result); + final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(best.getFiducialId()); final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); - final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); + final Transform3d targetToCamera = best.getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { - final int[] visibleTagIDs = new int[result.getTargets().size()]; - - for (int i = 0; i < visibleTagIDs.length; i++) - visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); - return visibleTagIDs; + return new int[]{getBestTarget(result).getFiducialId()}; +// final int[] visibleTagIDs = new int[result.getTargets().size()]; +// +// for (int i = 0; i < visibleTagIDs.length; i++) +// visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); +// return visibleTagIDs; } private double getDistanceFromBestTag(PhotonPipelineResult result) { - return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); + return getBestTarget(result).getBestCameraToTarget().getTranslation().getNorm(); + } + + private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { + PhotonTrackedTarget best = result.getBestTarget(); + if (result.getTargets().size() == 1) + return best; + var l = result.getTargets(); + if (best.getFiducialId() == 8 || best.getFiducialId() == 6) { + l.remove(best); + best = l.get(0); + } + for (PhotonTrackedTarget currentTarget : l) { + if (currentTarget.getFiducialId() == 8 || currentTarget.getFiducialId() == 6) + continue; + if (currentTarget.getArea() > best.getArea()) + best = currentTarget; + } + + return best; } private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index e2cb1b5..4ada545 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.AutoLogOutput; @@ -89,6 +90,8 @@ public void addOdometryObservation(OdometryObservation observation) { } public void addVisionObservation(VisionObservation observation) { + if (DriverStation.isAutonomous() || DriverStation.isDisabled()) + return; // If measurement is old enough to be outside the pose buffer's timespan, skip. try { if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index b38ddf2..2e8260f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -41,7 +41,7 @@ public class ElevatorConstants { private static final boolean FOLLOWER_MOTOR_OPPOSITE_DIRECTION = true; private static final AbsoluteSensorRangeValue ENCODER_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Unsigned_0To1; private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.711181640625; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.703125; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; private static final double P = RobotHardwareStats.isSimulation() ? 52 : 1.5, @@ -149,6 +149,8 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + + MASTER_MOTOR.setPosition(0); } private static void configureFollowerMotor() { diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index e5760c2..3c8f44f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -44,22 +44,22 @@ public class PitcherConstants { private static final SensorDirectionValue SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; private static final AbsoluteSensorRangeValue ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; - public static final double GRAVITY_POSITION_TO_REAL_POSITION = Conversions.degreesToRotations(-118.8756); - private static final double OFFSET = -0.0487158583333333; + public static final double GRAVITY_POSITION_TO_REAL_POSITION = Conversions.degreesToRotations(-118.8756) + 0.082775; + private static final double OFFSET = -0.0487158583333333 + 0.082775; private static final double - MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 180, + MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 120, MOTION_MAGIC_I = 0, MOTION_MAGIC_D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 1.0346 : 0.20177, - KV = RobotHardwareStats.isSimulation() ? 41 : 40.163, + KS = RobotHardwareStats.isSimulation() ? 1.0346 : 0.21662, + KV = RobotHardwareStats.isSimulation() ? 41 : 39.8, KA = RobotHardwareStats.isSimulation() ? 0.85062 : 0, - KG = RobotHardwareStats.isSimulation() ? 0.04366 : 0.087458; + KG = RobotHardwareStats.isSimulation() ? 0.04366 : 0.079831; private static final double MOTION_MAGIC_CRUISE_VELOCITY = 0.28, MOTION_MAGIC_ACCELERATION = 4, MOTION_MAGIC_JERK = 40; static final boolean FOC_ENABLED = true; - static final double GEAR_RATIO = 355.5; + static final double GEAR_RATIO = 353.757785; private static final int MOTOR_AMOUNT = 1; private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index d8e6699..a81b9f0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -309,7 +310,7 @@ private void configurePathPlanner() { this::getSelfRelativeVelocity, this::selfRelativeDrive, SwerveConstants.HOLONOMIC_PATH_FOLLOWER_CONFIG, - Mirrorable::isRedAlliance, + () -> DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue).equals(DriverStation.Alliance.Red), this ); PathfindingCommand.warmupCommand().schedule(); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 3f026e3..3473e0e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -22,15 +22,15 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_PITCH = -1.9303209781646729, - GYRO_MOUNT_POSITION_ROLL = -1.6774744987487793, - GYRO_MOUNT_POSITION_YAW = 3.2677361965179443; - private static final double lol = 1.8; + GYRO_MOUNT_POSITION_PITCH = -1.4626965522766113, + GYRO_MOUNT_POSITION_ROLL = -0.621093213558197, + GYRO_MOUNT_POSITION_YAW = -2.4247663021087646; + private static final double lol = 2; private static final double - FRONT_LEFT_WHEEL_DIAMETER_METERS = 0.050064 * lol, - FRONT_RIGHT_WHEEL_DIAMETER_METERS = 0.048810 * lol, - REAR_LEFT_WHEEL_DIAMETER_METERS = 0.048096 * lol, - REAR_RIGHT_WHEEL_DIAMETER_METERS = 0.048104 * lol; + FRONT_LEFT_WHEEL_DIAMETER_METERS = 0.047853 * lol, + FRONT_RIGHT_WHEEL_DIAMETER_METERS = 0.047853 * lol, + REAR_LEFT_WHEEL_DIAMETER_METERS = 0.047853 * lol, + REAR_RIGHT_WHEEL_DIAMETER_METERS = 0.047853 * lol; private static final double FRONT_LEFT_STEER_ENCODER_OFFSET = 0.374267578125, FRONT_RIGHT_STEER_ENCODER_OFFSET = -0.286376953125, @@ -85,7 +85,7 @@ public class SwerveConstants { new PIDConstants(5.6, 0, 0.12), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(9, 0, 0) : - new PIDConstants(6.5, 0, 0), + new PIDConstants(3, 0, 0), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(8.9, 0, 0) : new PIDConstants(3, 0, 0);