diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h index f47638fbd..81e3d47bd 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -24,9 +24,9 @@ #pragma once +#include #include #include -#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -72,8 +72,8 @@ class SwerveDrive { frc::SwerveDrivePoseEstimator<4> poseEstimator; frc::ChassisSpeeds targetChassisSpeeds{}; - //TODO(Jade) onboard imu doesn't have sim yet - // frc::sim::ADXRS450_GyroSim gyroSim; + // TODO(Jade) onboard imu doesn't have sim yet + // frc::sim::ADXRS450_GyroSim gyroSim; SwerveDriveSim swerveDriveSim; units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 871414f87..8e3fc9737 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -122,8 +122,7 @@ public class SwerveDrive { * @param omega Angular velocity (rotation CCW+) */ public void drive(double vx, double vy, double omega) { - var targetChassisSpeeds = - new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); + var targetChassisSpeeds = new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -269,8 +268,7 @@ public class SwerveDrive { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); - SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 6242a4e7c..c39332171 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -92,8 +92,7 @@ public class SwerveModule { double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances - drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); + drivePid = drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -156,11 +155,9 @@ public class SwerveModule { table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber(table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); - SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speed)); + table + "Drive Velocity Target Feet", Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 7feea5c44..547d237ba 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -122,8 +122,7 @@ public class SwerveDrive { * @param omega Angular velocity (rotation CCW+) */ public void drive(double vx, double vy, double omega) { - var targetChassisSpeeds = - new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); + var targetChassisSpeeds = new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -269,8 +268,7 @@ public class SwerveDrive { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); - SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 6242a4e7c..c39332171 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -92,8 +92,7 @@ public class SwerveModule { double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances - drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); + drivePid = drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -156,11 +155,9 @@ public class SwerveModule { table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber(table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); - SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speed)); + table + "Drive Velocity Target Feet", Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java index cb1a6ecb0..93e0fd661 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java @@ -50,8 +50,7 @@ public class GamepieceLauncher { } public void periodic() { - double maxRPM = - Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); + double maxRPM = Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); curMotorCmd = curDesSpd / maxRPM; curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); motor.set(curMotorCmd); diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 60e125802..cdb04cff6 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -122,8 +122,7 @@ public class SwerveDrive { * @param omega Angular velocity (rotation CCW+) */ public void drive(double vx, double vy, double omega) { - var targetChassisSpeeds = - new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); + var targetChassisSpeeds = new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -269,8 +268,7 @@ public class SwerveDrive { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); - SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 6242a4e7c..c39332171 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -92,8 +92,7 @@ public class SwerveModule { double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances - drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); + drivePid = drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -156,11 +155,9 @@ public class SwerveModule { table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber(table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); - SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speed)); + table + "Drive Velocity Target Feet", Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); }