diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index 1fe7ace220..c3ee9cbee9 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -11,17 +11,11 @@ repositories { jcenter() } -apply from: "${rootDir}/../shared/examples_common.gradle" - -ext { - wpilibVersion = "2025.0.0-alpha-1" - wpimathVersion = wpilibVersion - openCVversion = "4.8.0-2" -} +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.3.2" +wpi.versions.wpimathVersion = "2024.3.2" -wpi.getVersions().getOpencvVersion().convention(openCVversion); -wpi.getVersions().getWpilibVersion().convention(wpilibVersion); -wpi.getVersions().getWpimathVersion().convention(wpimathVersion); +apply from: "${rootDir}/../shared/examples_common.gradle" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. @@ -68,21 +62,36 @@ wpi.sim.addDriverstation() model { components { frcUserProgram(NativeExecutableSpec) { - targetPlatform wpi.platforms.roborio - targetPlatform wpi.platforms.desktop + // We don't need to build for roborio -- if we do, we need to install + // a roborio toolchain every time we build in CI + // Ideally, we'd be able to set the roborio toolchain as optional, but + // I can't figure out how to set that environment variable from build.gradle + // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) + // for now, commented out + + // targetPlatform wpi.platforms.roborio + + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } sources.cpp { source { srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' } exportedHeaders { srcDir 'src/main/include' } } + // Set deploy task to deploy this component deployArtifact.component = it + + // Enable run tasks for this component wpi.cpp.enableExternalTasks(it) + // Enable simulation for this component wpi.sim.enable(it) // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. wpi.cpp.vendor.cpp(it) @@ -100,6 +109,7 @@ model { } } + // Enable run tasks for this component wpi.cpp.enableExternalTasks(it) wpi.cpp.vendor.cpp(it) diff --git a/photonlib-cpp-examples/aimattarget/networktables.json b/photonlib-cpp-examples/aimattarget/networktables.json new file mode 100644 index 0000000000..fe51488c70 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/aimattarget/simgui-ds.json b/photonlib-cpp-examples/aimattarget/simgui-ds.json index bd35d5052f..addd5860ce 100644 --- a/photonlib-cpp-examples/aimattarget/simgui-ds.json +++ b/photonlib-cpp-examples/aimattarget/simgui-ds.json @@ -11,10 +11,13 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], "axisCount": 6, diff --git a/photonlib-cpp-examples/aimattarget/simgui-window.json b/photonlib-cpp-examples/aimattarget/simgui-window.json index 1d727b7218..3baff34d0a 100644 --- a/photonlib-cpp-examples/aimattarget/simgui-window.json +++ b/photonlib-cpp-examples/aimattarget/simgui-window.json @@ -1,50 +1,64 @@ { + "Docking": { + "Data": [] + }, "MainWindow": { "GLOBAL": { - "height": "720", + "fps": "120", + "height": "910", "maximized": "0", "style": "0", "userScale": "2", - "width": "1280", - "xpos": "3124", - "ypos": "324" + "width": "1550", + "xpos": "-1602", + "ypos": "79" } }, "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "554,10", + "Size": "991,527" + }, "###FMS": { "Collapsed": "0", - "Pos": "5,540", - "Size": "283,146" + "Pos": "12,604", + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,465", - "Size": "796,155" + "Pos": "278,685", + "Size": "976,219" }, - "###Keyboard 0 Settings": { + "###NetworkTables": { "Collapsed": "0", - "Pos": "10,50", - "Size": "300,560" + "Pos": "353,479", + "Size": "830,620" }, - "###NetworkTables": { + "###NetworkTables Info": { "Collapsed": "0", - "Pos": "250,277", - "Size": "750,185" + "Pos": "315,527", + "Size": "750,145" }, "###Other Devices": { "Collapsed": "0", "Pos": "1025,20", "Size": "250,695" }, + "###Plot <0>": { + "Collapsed": "0", + "Pos": "113,22", + "Size": "448,400" + }, "###System Joysticks": { "Collapsed": "0", "Pos": "5,350", - "Size": "192,218" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,127" + "Size": "162,142" }, "Debug##Default": { "Collapsed": "0", @@ -54,7 +68,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "109,134" } } } diff --git a/photonlib-cpp-examples/aimattarget/simgui.json b/photonlib-cpp-examples/aimattarget/simgui.json index 449f4b1d65..3f4675ea33 100644 --- a/photonlib-cpp-examples/aimattarget/simgui.json +++ b/photonlib-cpp-examples/aimattarget/simgui.json @@ -1,7 +1,164 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, + "SmartDashboard": { + "VisionSystemSim-main": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)" + } + ] + } + ], + "window": { + "visible": false + } } } } diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index 830fe147c0..56a7b6c0d7 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -24,50 +24,70 @@ #include "Robot.h" -#include +#include -#include -#include -#include +#include +#include + +void Robot::RobotInit() {} void Robot::RobotPeriodic() { - photon::PhotonCamera::SetVersionCheckEnabled(false); + drivetrain.Periodic(); + drivetrain.Log(); +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} - auto start = frc::Timer::GetFPGATimestamp(); - photon::PhotonPipelineResult result = camera.GetLatestResult(); - auto end = frc::Timer::GetFPGATimestamp(); +void Robot::AutonomousInit() {} - std::printf("DT is %.2f uS for %i targets\n", - units::microsecond_t(end - start).to(), - result.GetTargets().size()); +void Robot::AutonomousPeriodic() {} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() { + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); } void Robot::TeleopPeriodic() { - double forwardSpeed = -xboxController.GetRightY(); - double rotationSpeed; - - if (xboxController.GetAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - auto start = frc::Timer::GetFPGATimestamp(); - photon::PhotonPipelineResult result = camera.GetLatestResult(); - auto end = frc::Timer::GetFPGATimestamp(); - frc::SmartDashboard::PutNumber("decode_dt", (end - start).to()); - - if (result.HasTargets()) { - // Rotation speed is the output of the PID controller - rotationSpeed = -controller.Calculate(result.GetBestTarget().GetYaw(), 0); - } else { - // If we have no targets, stay still. - rotationSpeed = 0; - } - } else { - // Manual Driver Mode - rotationSpeed = xboxController.GetLeftX(); - } - - // Use our forward/turn speeds to control the drivetrain - drive.ArcadeDrive(forwardSpeed, rotationSpeed); + // Calculate drivetrain commands from Joystick values + auto forward = + -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed; + auto strafe = + -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed; + auto turn = + -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed; + + // Command drivetrain motors based on target speeds + drivetrain.Drive(forward, strafe, turn); + +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000000..92801c3e4c --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,200 @@ +/* + * 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. + */ + +#include "subsystems/SwerveDrive.h" + +#include +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, true, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + 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); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 0000000000..31bbe09e61 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * 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. + */ + +#include "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..785d9ca7d6 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,147 @@ +/* + * 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. + */ + +#include "subsystems/SwerveModule.h" + +#include +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt b/photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt new file mode 100644 index 0000000000..15bc5c1ebe --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h new file mode 100644 index 0000000000..98e1d5b1db --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * 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. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +inline const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, 0_rad, 0_rad}}; +inline const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)}; + +inline const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +inline constexpr units::meter_t kTrackWidth{18.5_in}; +inline constexpr units::meter_t kTrackLength{18.5_in}; +inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr units::meter_t kWheelDiameter{4_in}; +inline constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +inline constexpr double kDriveGearRatio = 6.75; +inline constexpr double kSteerGearRatio = 12.8; + +inline constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +inline constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +inline constexpr double kDriveKP = 1.0; +inline constexpr double kDriveKI = 0.0; +inline constexpr double kDriveKD = 0.0; + +inline constexpr double kSteerKP = 20.0; +inline constexpr double kSteerKI = 0.0; +inline constexpr double kSteerKD = 0.25; + +inline const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +inline const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +inline const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +inline const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +inline const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +inline const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h index aae3a98c62..d9eedc9097 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h @@ -24,31 +24,35 @@ #pragma once -#include - #include #include -#include -#include -#include -#include -#include + +#include "Vision.h" +#include + +#include "subsystems/SwerveDrive.h" class Robot : public frc::TimedRobot { public: - void TeleopPeriodic() override; + void RobotInit() override; void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; private: - // Change this to match the name of your camera as shown in the web UI - photon::PhotonCamera camera{"WPI2024"}; - // PID constants should be tuned per robot - frc::PIDController controller{.1, 0, 0}; - - frc::XboxController xboxController{0}; - - // Drive motors - frc::PWMVictorSPX leftMotor{0}; - frc::PWMVictorSPX rightMotor{1}; - frc::DifferentialDrive drive{leftMotor, rightMotor}; + photon::PhotonCamera camera{"photonvision"}; + SwerveDrive drivetrain{}; + Vision vision{&camera}; + frc::XboxController controller{0}; }; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Vision.h b/photonlib-cpp-examples/aimattarget/src/main/include/Vision.h new file mode 100644 index 0000000000..1efb300048 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Vision.h @@ -0,0 +1,89 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class Vision { + public: + Vision(photon::PhotonCamera* camera) { + + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(15_Hz); + cameraProp->SetAvgLatency(50_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = + std::make_shared(camera, *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; } + + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + + // The most recent result, cached for calculating std devs + photon::PhotonPipelineResult m_latestResult; +}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000000..98b0b2a582 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,80 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 0000000000..c1cee3e6f1 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * 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. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..444e4bdf44 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photonlib-cpp-examples/aimattarget/swerve_module.png b/photonlib-cpp-examples/aimattarget/swerve_module.png new file mode 100644 index 0000000000..25990c8399 Binary files /dev/null and b/photonlib-cpp-examples/aimattarget/swerve_module.png differ diff --git a/photonlib-cpp-examples/aimattarget/tag-blue.png b/photonlib-cpp-examples/aimattarget/tag-blue.png new file mode 100644 index 0000000000..04b9e4f7b7 Binary files /dev/null and b/photonlib-cpp-examples/aimattarget/tag-blue.png differ diff --git a/photonlib-cpp-examples/aimattarget/tag-green.png b/photonlib-cpp-examples/aimattarget/tag-green.png new file mode 100644 index 0000000000..63029fcf20 Binary files /dev/null and b/photonlib-cpp-examples/aimattarget/tag-green.png differ