From f7984abc509a695e2284e017f9385e74665ade39 Mon Sep 17 00:00:00 2001 From: Dhvan <34456470+Dh-Van@users.noreply.github.com> Date: Mon, 13 May 2024 20:31:55 -0400 Subject: [PATCH] Add comments and questions --- src/main/java/frc/robot/PoseEstimation.java | 1 + src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/drivetrain/DriveToNote.java | 53 +++++++++++++++++++ .../java/frc/robot/drivetrain/Drivetrain.java | 10 ++-- .../frc/robot/vision/GamePieceDetection.java | 8 +++ src/main/java/frc/robot/vision/Vision.java | 2 +- 6 files changed, 72 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/drivetrain/DriveToNote.java diff --git a/src/main/java/frc/robot/PoseEstimation.java b/src/main/java/frc/robot/PoseEstimation.java index c9ae32f..7defe65 100644 --- a/src/main/java/frc/robot/PoseEstimation.java +++ b/src/main/java/frc/robot/PoseEstimation.java @@ -16,6 +16,7 @@ import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLogOutput; +// why is this in our code? Shou;dn't this be in a library we use? public class PoseEstimation { private static PoseEstimation m_instance; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5a50142..7284d72 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import frc.robot.arm.Arm; import frc.robot.arm.Arm.ArmSetpoints; import frc.robot.climber.Climber; +import frc.robot.drivetrain.DriveToNote; import frc.robot.drivetrain.DriveToPose; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.WheelRadiusCharacterization; @@ -247,6 +248,7 @@ public Robot() { m_operatorController .b() .toggleOnTrue(m_arm.aimWristForTuning(() -> -m_operatorController.getLeftY())); + m_operatorController.povDown().whileTrue(new DriveToNote(m_drivetrain, m_lights, m_gamePieceDetection, m_intake)); m_driverController.rightBumper().whileTrue(Superstructure.spit(m_shooter, m_feeder, m_intake)); m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb)); m_operatorController diff --git a/src/main/java/frc/robot/drivetrain/DriveToNote.java b/src/main/java/frc/robot/drivetrain/DriveToNote.java new file mode 100644 index 0000000..8d39f62 --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/DriveToNote.java @@ -0,0 +1,53 @@ +package frc.robot.drivetrain; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.intake.Intake; +import frc.robot.lights.Lights; +import frc.robot.vision.GamePieceDetection; + +public class DriveToNote extends Command { +private Drivetrain m_drivetrain; +private Lights m_lights; +private GamePieceDetection m_gamePieceDetection; + +private PIDController m_thetaController; + +private double m_horizontalOffset; +private double m_verticalOffset; + +public DriveToNote(Drivetrain drivetrain, Lights lights, GamePieceDetection gamePieceDetection, Intake intake) { +m_drivetrain = drivetrain; +m_lights = lights; +m_gamePieceDetection = gamePieceDetection; + +m_thetaController = new PIDController(5.0, 0.0, 0.0); + +m_horizontalOffset = m_gamePieceDetection.getHorizontalOffsetDeg(); +m_verticalOffset = m_gamePieceDetection.getVerticalOffsetDeg(); + +m_thetaController.setTolerance(1); +} + +@Override +public void initialize() {} + +@Override +public void execute() { +m_horizontalOffset = m_gamePieceDetection.getHorizontalOffsetDeg(); +m_verticalOffset = m_gamePieceDetection.getVerticalOffsetDeg(); +m_drivetrain.runVelocity( +new ChassisSpeeds(0.0, 0.0, m_thetaController.calculate(m_horizontalOffset, 0))); +// m_lights.setBlink(Color.kMidnightBlue); +} + +@Override +public void end(boolean interrupted) {} + +@Override +public boolean isFinished() { +return false; +} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index cdb611d..57d3a88 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -46,6 +46,8 @@ public class Drivetrain extends SubsystemBase { private static final double kTrackwidthMeters = Units.inchesToMeters(20.75); private static double kWheelbaseMeters = Units.inchesToMeters(15.75); + + // @todo What order are these in? Is it Front Left then Clockwise private static final Translation2d[] m_modulePositions = new Translation2d[] { new Translation2d(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0), @@ -55,6 +57,7 @@ public class Drivetrain extends SubsystemBase { }; public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(m_modulePositions); + public static final double kDriveBaseRadius = Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0); private static final LoggedTunableNumber speakerRangeMeters = @@ -154,8 +157,7 @@ public void periodic() { SwerveModulePosition[] newModulePositions = new SwerveModulePosition[m_modules.length]; SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; for (int moduleIndex = 0; moduleIndex < m_modules.length; moduleIndex++) { - newModulePositions[moduleIndex] = - m_modules[moduleIndex].getOdometryPositions()[updateIndex]; + newModulePositions[moduleIndex] = m_modules[moduleIndex].getOdometryPositions()[updateIndex]; moduleDeltas[moduleIndex] = new SwerveModulePosition( newModulePositions[moduleIndex].distanceMeters @@ -168,6 +170,7 @@ public void periodic() { if (m_imuInputs.connected) { m_gyroRotation = m_imuInputs.odometryYawPositions[updateIndex]; } else { + //when does this code run? Was this added when we had the pidgeon issue at worlds? final Twist2d twist = swerveKinematics.toTwist2d(moduleDeltas); m_gyroRotation = m_gyroRotation.plus(new Rotation2d(twist.dtheta)); } @@ -242,7 +245,8 @@ public Command joystickDrive( * drive, no path following) */ public Command blankDrive() { - return this.run(() -> runVelocity(new ChassisSpeeds(0.0, 0.0, 0.0))); + return this.run(() -> + runVelocity(new ChassisSpeeds(0.0, 0.0, 0.0))); } public Command zeroGyro() { diff --git a/src/main/java/frc/robot/vision/GamePieceDetection.java b/src/main/java/frc/robot/vision/GamePieceDetection.java index f6eb922..41831dd 100644 --- a/src/main/java/frc/robot/vision/GamePieceDetection.java +++ b/src/main/java/frc/robot/vision/GamePieceDetection.java @@ -32,4 +32,12 @@ public void periodic() { Logger.recordOutput("GamePieceDetection/ValidTarget", hasValidTarget.getAsBoolean()); Logger.recordOutput("GamePieceDetection/HorizontalErrorDeg", horizontalErrorDeg.getAsDouble()); } + + public double getHorizontalOffsetDeg() { + return horizontalErrorDeg.getAsDouble(); + } + + public double getVerticalOffsetDeg() { + return m_inputs.targetVerticalOffsetDegrees; + } } diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 907e479..fcbb379 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -126,7 +126,7 @@ public void periodic() { .ifPresent( tagPose -> m_lastDetectionTimeIds.put( - target.getFiducialId(), Timer.getFPGATimestamp())); + target.getFiducialId(), Timer.getFPGATimestamp())); //shouldn't this be the timestamp of capture on the photonvision processor and not our timestamp }); m_allEstimatedPosesToLog.add(estimatedPose); m_newVisionUpdates.add(