diff --git a/.gitignore b/.gitignore index ca51fb8e5..032a1d7e1 100644 --- a/.gitignore +++ b/.gitignore @@ -161,3 +161,5 @@ photon-client/playwright/.auth/ shell.nix flake.nix flake.lock + +/workspace diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp index 4602def8a..40ba22f6b 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp @@ -32,7 +32,7 @@ SwerveDrive::SwerveDrive() : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), - gyroSim(gyro), + // gyroSim(gyro), swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), constants::Swerve::kDriveGearRatio, constants::Swerve::kWheelDiameter / 2, @@ -82,8 +82,8 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { for (int i = 0; i < swerveMods.size(); i++) { swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); } - gyroSim.SetAngle(-pose.Rotation().Degrees()); - gyroSim.SetRate(0_rad_per_s); + // gyroSim.SetAngle(-pose.Rotation().Degrees()); + // gyroSim.SetRate(0_rad_per_s); } poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); @@ -190,8 +190,8 @@ void SwerveDrive::SimulationPeriodic() { swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.SetRate(-swerveDriveSim.GetOmega()); - gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); + // gyroSim.SetRate(-swerveDriveSim.GetOmega()); + // gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); } frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } 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 98b0b2a58..f47638fbd 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -24,11 +24,9 @@ #pragma once -#include -#include #include #include -#include +#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -70,11 +68,12 @@ class SwerveDrive { swerveMods[2].GetModuleConstants().centerOffset, swerveMods[3].GetModuleConstants().centerOffset, }; - frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::OnboardIMU gyro{MountOrientation::kFlat}; frc::SwerveDrivePoseEstimator<4> poseEstimator; frc::ChassisSpeeds targetChassisSpeeds{}; - 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/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 4d0a2bd62..131232caa 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -78,7 +78,7 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } 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 c1676f6ff..871414f87 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 @@ -38,9 +38,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; @@ -62,7 +61,7 @@ public class SwerveDrive { swerveMods[2].getModuleConstants().centerOffset, swerveMods[3].getModuleConstants().centerOffset); - private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat); // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; @@ -70,7 +69,8 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - private final ADXRS450_GyroSim gyroSim; + // TODO(Jade) Onboard IMU doesnt have sim yet + // private final ADXRS450_GyroSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public class SwerveDrive { visionStdDevs); // ----- Simulation - gyroSim = new ADXRS450_GyroSim(gyro); + // gyroSim = new ADXRS450_GyroSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -117,13 +117,13 @@ public class SwerveDrive { * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to * specific swerve module states. * - * @param vxMeters X velocity (forwards/backwards) - * @param vyMeters Y velocity (strafe left/right) - * @param omegaRadians Angular velocity (rotation CCW+) + * @param vx X velocity (forwards/backwards) + * @param vy Y velocity (strafe left/right) + * @param omega Angular velocity (rotation CCW+) */ - public void drive(double vxMeters, double vyMeters, double omegaRadians) { + public void drive(double vx, double vy, double omega) { var targetChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -189,8 +189,8 @@ public class SwerveDrive { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - gyroSim.setAngle(-pose.getRotation().getDegrees()); - gyroSim.setRate(0); + // gyroSim.setAngle(-pose.getRotation().getDegrees()); + // gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -267,14 +267,14 @@ public class SwerveDrive { SmartDashboard.putNumber(table + "Y", pose.getY()); SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); - SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( - table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega)); for (SwerveModule module : swerveMods) { module.log(); @@ -314,8 +314,8 @@ public class SwerveDrive { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index ccef2bba6..07bff2115 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -341,7 +341,7 @@ public class SwerveDriveSim { driveStates.set(i, moduleDriveStates.get(i).copy()); steerStates.set(i, moduleSteerStates.get(i).copy()); } - omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega; } /** @@ -444,12 +444,12 @@ public class SwerveDriveSim { */ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { - double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); // calculate battery current - return (inputVolts / battVolts) * (effVolts / motor.rOhms); + return (inputVolts / battVolts) * (effVolts / motor.R); } /** 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 5d77dcce5..6242a4e7c 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 @@ -88,12 +88,12 @@ public class SwerveModule { steerMotor.setVoltage(steerPid); // Use feedforward model to translate target drive velocities into voltages - double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double driveFF = kDriveFF.calculate(desiredState.speed); double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -157,10 +157,10 @@ public class SwerveModule { SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speedMetersPerSecond)); + Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index f4401d5af..4622163ee 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -74,7 +74,7 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } 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 c1676f6ff..7feea5c44 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 @@ -38,9 +38,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; @@ -62,7 +61,7 @@ public class SwerveDrive { swerveMods[2].getModuleConstants().centerOffset, swerveMods[3].getModuleConstants().centerOffset); - private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat); // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; @@ -70,7 +69,8 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - private final ADXRS450_GyroSim gyroSim; + // TODO(Jade) OnboardIMU doesn't have sim yet + // private final ADXRS450_GyroSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public class SwerveDrive { visionStdDevs); // ----- Simulation - gyroSim = new ADXRS450_GyroSim(gyro); + // gyroSim = new ADXRS450_GyroSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -117,13 +117,13 @@ public class SwerveDrive { * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to * specific swerve module states. * - * @param vxMeters X velocity (forwards/backwards) - * @param vyMeters Y velocity (strafe left/right) - * @param omegaRadians Angular velocity (rotation CCW+) + * @param vx X velocity (forwards/backwards) + * @param vy Y velocity (strafe left/right) + * @param omega Angular velocity (rotation CCW+) */ - public void drive(double vxMeters, double vyMeters, double omegaRadians) { + public void drive(double vx, double vy, double omega) { var targetChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -189,8 +189,8 @@ public class SwerveDrive { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - gyroSim.setAngle(-pose.getRotation().getDegrees()); - gyroSim.setRate(0); + // gyroSim.setAngle(-pose.getRotation().getDegrees()); + // gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -267,14 +267,14 @@ public class SwerveDrive { SmartDashboard.putNumber(table + "Y", pose.getY()); SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); - SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( - table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega)); for (SwerveModule module : swerveMods) { module.log(); @@ -314,8 +314,8 @@ public class SwerveDrive { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index ccef2bba6..07bff2115 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -341,7 +341,7 @@ public class SwerveDriveSim { driveStates.set(i, moduleDriveStates.get(i).copy()); steerStates.set(i, moduleSteerStates.get(i).copy()); } - omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega; } /** @@ -444,12 +444,12 @@ public class SwerveDriveSim { */ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { - double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); // calculate battery current - return (inputVolts / battVolts) * (effVolts / motor.rOhms); + return (inputVolts / battVolts) * (effVolts / motor.R); } /** 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 5d77dcce5..6242a4e7c 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 @@ -88,12 +88,12 @@ public class SwerveModule { steerMotor.setVoltage(steerPid); // Use feedforward model to translate target drive velocities into voltages - double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double driveFF = kDriveFF.calculate(desiredState.speed); double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -157,10 +157,10 @@ public class SwerveModule { SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speedMetersPerSecond)); + Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/poseest/build.gradle b/photonlib-java-examples/poseest/build.gradle index f4401d5af..4622163ee 100644 --- a/photonlib-java-examples/poseest/build.gradle +++ b/photonlib-java-examples/poseest/build.gradle @@ -74,7 +74,7 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } 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 8d76b1d78..cb1a6ecb0 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 @@ -51,7 +51,7 @@ public class GamepieceLauncher { public void periodic() { double maxRPM = - Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec); + Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); curMotorCmd = curDesSpd / maxRPM; curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); motor.set(curMotorCmd); @@ -76,7 +76,7 @@ public class GamepieceLauncher { public void simulationPeriodic() { launcherSim.setInputVoltage(curMotorCmd * RobotController.getBatteryVoltage()); launcherSim.update(0.02); - var spd = launcherSim.getAngularVelocityRPM(); + var spd = launcherSim.getAngularVelocity(); SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd); } } 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 c1676f6ff..60e125802 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 @@ -38,9 +38,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; @@ -62,7 +61,7 @@ public class SwerveDrive { swerveMods[2].getModuleConstants().centerOffset, swerveMods[3].getModuleConstants().centerOffset); - private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat); // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; @@ -70,7 +69,8 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - private final ADXRS450_GyroSim gyroSim; + // TODO(Jade) WPILib doesn't have onboard IMU sim yet + // private final ADXRS450_GyroSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public class SwerveDrive { visionStdDevs); // ----- Simulation - gyroSim = new ADXRS450_GyroSim(gyro); + // gyroSim = new ADXRS450_GyroSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -117,13 +117,13 @@ public class SwerveDrive { * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to * specific swerve module states. * - * @param vxMeters X velocity (forwards/backwards) - * @param vyMeters Y velocity (strafe left/right) - * @param omegaRadians Angular velocity (rotation CCW+) + * @param vx X velocity (forwards/backwards) + * @param vy Y velocity (strafe left/right) + * @param omega Angular velocity (rotation CCW+) */ - public void drive(double vxMeters, double vyMeters, double omegaRadians) { + public void drive(double vx, double vy, double omega) { var targetChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -189,8 +189,8 @@ public class SwerveDrive { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - gyroSim.setAngle(-pose.getRotation().getDegrees()); - gyroSim.setRate(0); + // gyroSim.setAngle(-pose.getRotation().getDegrees()); + // gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -267,14 +267,14 @@ public class SwerveDrive { SmartDashboard.putNumber(table + "Y", pose.getY()); SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); - SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( - table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega)); for (SwerveModule module : swerveMods) { module.log(); @@ -314,8 +314,8 @@ public class SwerveDrive { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index ccef2bba6..07bff2115 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -341,7 +341,7 @@ public class SwerveDriveSim { driveStates.set(i, moduleDriveStates.get(i).copy()); steerStates.set(i, moduleSteerStates.get(i).copy()); } - omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega; } /** @@ -444,12 +444,12 @@ public class SwerveDriveSim { */ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { - double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); // calculate battery current - return (inputVolts / battVolts) * (effVolts / motor.rOhms); + return (inputVolts / battVolts) * (effVolts / motor.R); } /** 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 5d77dcce5..6242a4e7c 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 @@ -88,12 +88,12 @@ public class SwerveModule { steerMotor.setVoltage(steerPid); // Use feedforward model to translate target drive velocities into voltages - double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double driveFF = kDriveFF.calculate(desiredState.speed); double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -157,10 +157,10 @@ public class SwerveModule { SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speedMetersPerSecond)); + Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); }