From 862fc65c1c2efd0a5f3351cace09864ada4fa437 Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Sun, 7 Apr 2024 16:57:55 -0400 Subject: [PATCH 1/6] rename a log key --- src/main/java/frc/robot/vision/Vision.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 46dd298..bcd9c4b 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -144,7 +144,7 @@ public void periodic() { } Logger.recordOutput( - "Vision/AllEstimatedPoses", + "Vision/InvalidEstimatedPoses", m_allEstimatedPosesToLog.stream().map(applyPoseOffset).toArray(Pose3d[]::new)); Logger.recordOutput( "Vision/ValidEstimatedPoses", From 1eeaabefcbf4b2d8e45d1007bac3342f876696fb Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Mon, 8 Apr 2024 11:02:33 -0400 Subject: [PATCH 2/6] add candle connection monitoring --- src/main/java/frc/robot/lights/Lights.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/lights/Lights.java b/src/main/java/frc/robot/lights/Lights.java index c6d717a..3606a65 100644 --- a/src/main/java/frc/robot/lights/Lights.java +++ b/src/main/java/frc/robot/lights/Lights.java @@ -1,5 +1,6 @@ package frc.robot.lights; +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.CANdle.LEDStripType; import com.ctre.phoenix.led.CANdleConfiguration; @@ -41,6 +42,7 @@ public void periodic() { Logger.recordOutput("CANdle/5VRailVoltage", m_ledController.get5VRailVoltage()); Logger.recordOutput("CANdle/Current", m_ledController.getCurrent()); Logger.recordOutput("CANdle/Temperature", m_ledController.getTemperature()); + Logger.recordOutput("CANdle/Connected", m_ledController.getLastError() == ErrorCode.OK); } public Command idle() { From 621c988f71fecf5f896729ea30fff54b928b6698 Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Mon, 8 Apr 2024 11:11:13 -0400 Subject: [PATCH 3/6] Make minor refactors --- .../java/frc/robot/arm/ArmVisualizer.java | 4 ++-- .../frc/robot/drivetrain/DriveToPose.java | 24 +++++++++---------- .../java/frc/robot/drivetrain/Drivetrain.java | 16 ++++++------- 3 files changed, 21 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmVisualizer.java b/src/main/java/frc/robot/arm/ArmVisualizer.java index 6b0c3ad..b6a4209 100644 --- a/src/main/java/frc/robot/arm/ArmVisualizer.java +++ b/src/main/java/frc/robot/arm/ArmVisualizer.java @@ -46,7 +46,7 @@ public ArmVisualizer(String logKey, double ligamentWidth, Color color) { public void update(double elbowAngleRad, double wristAngleRad) { m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngleRad) - kShoulderAngleDegrees); m_wristLigament.setAngle(Units.radiansToDegrees(wristAngleRad - elbowAngleRad)); - Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism); + Logger.recordOutput("Arm/Mechanism2d/" + m_logKey, m_mechanism); m_elbowPose = new Pose3d(origin.getX(), 0.0, origin.getY(), new Rotation3d(0.0, -elbowAngleRad, 0.0)); @@ -55,7 +55,7 @@ public void update(double elbowAngleRad, double wristAngleRad) { new Transform3d( new Translation3d(ArmConstants.kElbowLengthMeters, 0.0, 0.0), new Rotation3d(0.0, -(wristAngleRad - elbowAngleRad), 0.0))); - Logger.recordOutput("Mechanism3d/" + m_logKey, m_elbowPose, m_wristPose); + Logger.recordOutput("Arm/Mechanism3d/" + m_logKey, m_elbowPose, m_wristPose); } public Pose3d getWristPose() { diff --git a/src/main/java/frc/robot/drivetrain/DriveToPose.java b/src/main/java/frc/robot/drivetrain/DriveToPose.java index 09bc3df..c4e0e38 100644 --- a/src/main/java/frc/robot/drivetrain/DriveToPose.java +++ b/src/main/java/frc/robot/drivetrain/DriveToPose.java @@ -72,15 +72,17 @@ public void initialize() { @Override public void execute() { Pose2d currentPose = PoseEstimation.getInstance().getPose(); - double currentDistance = currentPose.getTranslation().getDistance(m_goalPose.getTranslation()); - double ffScaler = MathUtil.clamp((currentDistance - 0.2) / (0.8 - 0.2), 0.0, 1.0); + double distanceToGoalPose = + currentPose.getTranslation().getDistance(m_goalPose.getTranslation()); + double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0); m_translationController.reset( m_lastSetpointTranslation.getDistance(m_goalPose.getTranslation()), m_translationController.getSetpoint().velocity); double driveVelocityScalar = m_translationController.getSetpoint().velocity * ffScaler - + m_translationController.calculate(currentDistance, 0.0); - if (currentDistance < m_translationController.getPositionTolerance()) driveVelocityScalar = 0.0; + + m_translationController.calculate(distanceToGoalPose, 0.0); + if (distanceToGoalPose < m_translationController.getPositionTolerance()) + driveVelocityScalar = 0.0; m_lastSetpointTranslation = new Pose2d( m_goalPose.getTranslation(), @@ -95,9 +97,9 @@ public void execute() { m_thetaController.getSetpoint().velocity * ffScaler + m_thetaController.calculate( currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians()); - double thetaErrorAbs = + double thetaErrorAbsolute = Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians()); - if (thetaErrorAbs < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0; + if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0; Translation2d driveVelocity = new Pose2d( @@ -110,15 +112,11 @@ public void execute() { ChassisSpeeds.fromFieldRelativeSpeeds( driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation())); - Logger.recordOutput("DriveToPose/DistanceMeasured", currentDistance); + Logger.recordOutput("Drivetrain/DriveToPose/DistanceToGoalPose", distanceToGoalPose); Logger.recordOutput( - "DriveToPose/DistanceSetpoint", m_translationController.getSetpoint().position); - Logger.recordOutput("DriveToPose/ThetaMeasured", currentPose.getRotation().getRadians()); - Logger.recordOutput("DriveToPose/ThetaSetpoint", m_thetaController.getSetpoint().position); - Logger.recordOutput( - "DriveToPose/Setpoint", + "Drivetrain/DriveToPose/Setpoint", new Pose2d( m_lastSetpointTranslation, new Rotation2d(m_thetaController.getSetpoint().position))); - Logger.recordOutput("DriveToPose/Goal", m_goalPose); + Logger.recordOutput("Drivetrain/DriveToPose/Goal", m_goalPose); } } diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index e705016..8dbe1cc 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -57,7 +57,7 @@ public class Drivetrain extends SubsystemBase { new SwerveDriveKinematics(m_modulePositions); public static final double kDriveBaseRadius = Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0); - private static final LoggedTunableNumber speakerRange = + private static final LoggedTunableNumber speakerRangeMeters = new LoggedTunableNumber("Drivetrain/InRangeRadius", 5.0); private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16); private final double kMaxAngularSpeedRadPerSec = 2 * Math.PI; @@ -145,8 +145,8 @@ public void periodic() { } if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + Logger.recordOutput("Drivetrain/SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("Drivetrain/SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); for (Module module : m_modules) module.stop(); } @@ -201,8 +201,8 @@ public void runVelocity(ChassisSpeeds speeds) { m_modules[moduleIndex].setSetpoint(setpointStates[moduleIndex]); } - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + Logger.recordOutput("Drivetrain/SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("Drivetrain/SwerveStates/SetpointsOptimized", optimizedSetpointStates); } public void runWheelRadiusCharacterization(double characterizationInput) { @@ -313,12 +313,12 @@ private void configurePathing() { targetPose -> Logger.recordOutput("Drivetrain/TrajectorySetpoint", targetPose)); } - @AutoLogOutput(key = "SwerveStates/StatesMeasured") + @AutoLogOutput(key = "Drivetrain/SwerveStates/StatesMeasured") private SwerveModuleState[] getModuleStates() { return Arrays.stream(m_modules).map(Module::getState).toArray(SwerveModuleState[]::new); } - @AutoLogOutput(key = "SwerveStates/PositionsMeasured") + @AutoLogOutput(key = "Drivetrain/SwerveStates/PositionsMeasured") private SwerveModulePosition[] getModulePositions() { return Arrays.stream(m_modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); } @@ -344,6 +344,6 @@ public boolean atHeadingGoal() { @AutoLogOutput(key = "Drivetrain/InRangeOfGoal") public boolean inRange() { return PoseEstimation.getInstance().getAimingParameters().effectiveDistance() - <= speakerRange.get(); + <= speakerRangeMeters.get(); } } From 6987f007025ee46066486f18b923e5f89fc67307 Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Mon, 8 Apr 2024 11:25:43 -0400 Subject: [PATCH 4/6] add formatting to CI --- .github/workflows/build.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ba0370f..d731ea8 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -16,3 +16,5 @@ jobs: run: chmod +x gradlew - name: Build robot code run: ./gradlew build + - name: Check formatting + run: ./gradlew spotlessCheck From 0a3d556a55752fd4777812fb0558d2892dd3559c Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Mon, 8 Apr 2024 11:28:49 -0400 Subject: [PATCH 5/6] check formatting before building --- .github/workflows/build.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index d731ea8..7aa3be9 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -14,7 +14,7 @@ jobs: uses: actions/checkout@v2 - name: Grant execute permission run: chmod +x gradlew - - name: Build robot code - run: ./gradlew build - name: Check formatting run: ./gradlew spotlessCheck + - name: Build robot code + run: ./gradlew build From 414d15a818b5a9fe56bc169ae4236d259743950b Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Mon, 8 Apr 2024 11:31:38 -0400 Subject: [PATCH 6/6] bump checkout version --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7aa3be9..47ce758 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -11,7 +11,7 @@ jobs: container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Grant execute permission run: chmod +x gradlew - name: Check formatting