mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Examples Clean-Up (#1408)
This commit is contained in:
Chris Gerth
@@ -11,18 +11,12 @@ repositories {
|
||||
jcenter()
|
||||
}
|
||||
|
||||
wpi.maven.useDevelopment = true
|
||||
wpi.versions.wpilibVersion = "2024.3.2"
|
||||
wpi.versions.wpimathVersion = "2024.3.2"
|
||||
|
||||
apply from: "${rootDir}/../shared/examples_common.gradle"
|
||||
|
||||
ext {
|
||||
wpilibVersion = "2025.0.0-alpha-1"
|
||||
wpimathVersion = wpilibVersion
|
||||
openCVversion = "4.8.0-2"
|
||||
}
|
||||
|
||||
wpi.getVersions().getOpencvVersion().convention(openCVversion);
|
||||
wpi.getVersions().getWpilibVersion().convention(wpilibVersion);
|
||||
wpi.getVersions().getWpimathVersion().convention(wpimathVersion);
|
||||
|
||||
// Define my targets (RoboRIO) and artifacts (deployable files)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
@@ -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)
|
||||
|
||||
1
photonlib-cpp-examples/aimattarget/networktables.json
Normal file
1
photonlib-cpp-examples/aimattarget/networktables.json
Normal file
@@ -0,0 +1 @@
|
||||
[]
|
||||
@@ -11,10 +11,13 @@
|
||||
"incKey": 83
|
||||
},
|
||||
{
|
||||
"decKey": 69,
|
||||
"decayRate": 0.0,
|
||||
"incKey": 82,
|
||||
"keyRate": 0.009999999776482582
|
||||
},
|
||||
{},
|
||||
{
|
||||
"decKey": 81,
|
||||
"incKey": 69
|
||||
}
|
||||
],
|
||||
"axisCount": 6,
|
||||
|
||||
@@ -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": "336,158"
|
||||
},
|
||||
"###Joysticks": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "250,465",
|
||||
"Size": "796,155"
|
||||
},
|
||||
"###Keyboard 0 Settings": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "10,50",
|
||||
"Size": "300,560"
|
||||
"Pos": "278,685",
|
||||
"Size": "976,219"
|
||||
},
|
||||
"###NetworkTables": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "250,277",
|
||||
"Size": "750,185"
|
||||
"Pos": "353,479",
|
||||
"Size": "830,620"
|
||||
},
|
||||
"###NetworkTables Info": {
|
||||
"Collapsed": "0",
|
||||
"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"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,50 +24,98 @@
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <photon/PhotonUtils.h>
|
||||
#include <iostream>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <units/time.h>
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
void Robot::RobotPeriodic() {
|
||||
photon::PhotonCamera::SetVersionCheckEnabled(false);
|
||||
drivetrain.Periodic();
|
||||
drivetrain.Log();
|
||||
}
|
||||
|
||||
auto start = frc::Timer::GetFPGATimestamp();
|
||||
photon::PhotonPipelineResult result = camera.GetLatestResult();
|
||||
auto end = frc::Timer::GetFPGATimestamp();
|
||||
void Robot::DisabledInit() {}
|
||||
|
||||
std::printf("DT is %.2f uS for %i targets\n",
|
||||
units::microsecond_t(end - start).to<double>(),
|
||||
result.GetTargets().size());
|
||||
void Robot::DisabledPeriodic() { drivetrain.Stop(); }
|
||||
|
||||
void Robot::DisabledExit() {}
|
||||
|
||||
void Robot::AutonomousInit() {}
|
||||
|
||||
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<double>());
|
||||
// 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;
|
||||
|
||||
bool targetVisible = false;
|
||||
double targetYaw = 0.0;
|
||||
auto results = camera.GetAllUnreadResults();
|
||||
if (results.size() > 0) {
|
||||
// Camera processed a new frame since last
|
||||
// Get the last one in the list.
|
||||
auto result = results[results.size() - 1];
|
||||
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;
|
||||
// At least one AprilTag was seen by the camera
|
||||
for (auto& target : result.GetTargets()) {
|
||||
if (target.GetFiducialId() == 7) {
|
||||
// Found Tag 7, record its information
|
||||
targetYaw = target.GetYaw();
|
||||
targetVisible = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Manual Driver Mode
|
||||
rotationSpeed = xboxController.GetLeftX();
|
||||
}
|
||||
|
||||
// Use our forward/turn speeds to control the drivetrain
|
||||
drive.ArcadeDrive(forwardSpeed, rotationSpeed);
|
||||
// Auto-align when requested
|
||||
if (controller.GetAButton() && targetVisible) {
|
||||
// Driver wants auto-alignment to tag 7
|
||||
// And, tag 7 is in sight, so we can turn toward it.
|
||||
// Override the driver's turn command with an automatic one that turns
|
||||
// toward the tag.
|
||||
turn =
|
||||
-1.0 * targetYaw * VISION_TURN_kP * 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
|
||||
|
||||
@@ -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 <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
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<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&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<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> 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<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> 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<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> 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<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Heading",
|
||||
pose.Rotation().Degrees().to<double>());
|
||||
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
module.Log();
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> 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<units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<units::ampere_t, 4> 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; }
|
||||
@@ -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 <iostream>
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.kV.to<double>() / driveFF.kA.to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 1.0 / driveFF.kA.to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{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<double>() / steerFF.kA.to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 1.0 / steerFF.kA.to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{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<numModules>& 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<units::volt_t, numModules>& 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<units::volt_t, numModules>& 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<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& 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<double>(), kS.to<double>());
|
||||
|
||||
nextStateVel += discB(1, 0) * ksSystemEffect;
|
||||
inputToStop = nextStateVel / -discB(1, 0);
|
||||
double signToStop = sgn(inputToStop);
|
||||
double inputSign = sgn(input.to<double>());
|
||||
double ksInputEffect = 0;
|
||||
|
||||
if (std::abs(ksSystemEffect) < kS.to<double>()) {
|
||||
double absInput = std::abs(input.to<double>());
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
} else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
|
||||
double absInput = std::abs(input.to<double>() - inputToStop);
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
}
|
||||
|
||||
auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
|
||||
ksInputEffect};
|
||||
auto Bu = discB * sF;
|
||||
auto retVal = Ax + Bu;
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(units::second_t dt) {
|
||||
Eigen::Matrix<double, 2, 2> driveDiscA;
|
||||
Eigen::Matrix<double, 2, 1> driveDiscB;
|
||||
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> 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<double, 2, 1>{0, 0};
|
||||
steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
|
||||
}
|
||||
omega = 0_rad_per_s;
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleSteerStates) {
|
||||
this->pose = pose;
|
||||
driveStates = moduleDriveStates;
|
||||
steerStates = moduleSteerStates;
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::SwerveModulePosition, numModules> 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<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> 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<frc::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::SwerveModuleState, numModules> 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<Eigen::Matrix<double, 2, 1>, numModules>
|
||||
SwerveDriveSim::GetDriveStates() const {
|
||||
return driveStates;
|
||||
}
|
||||
|
||||
std::array<Eigen::Matrix<double, 2, 1>, 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<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> 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<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> 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;
|
||||
}
|
||||
@@ -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 <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
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<double>());
|
||||
steerEncoder.SetDistancePerPulse(
|
||||
constants::Swerve::kSteerRadPerPulse.to<double>());
|
||||
steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
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<double>())};
|
||||
}
|
||||
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<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
steerCurrentSim.to<double>());
|
||||
}
|
||||
|
||||
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<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
|
||||
steerEncoderSim.SetRate(steerEncoderRate.to<double>());
|
||||
steerCurrentSim = steerCurrent;
|
||||
}
|
||||
@@ -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.
|
||||
118
photonlib-cpp-examples/aimattarget/src/main/include/Constants.h
Normal file
118
photonlib-cpp-examples/aimattarget/src/main/include/Constants.h
Normal file
@@ -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 <numbers>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <units/length.h>
|
||||
|
||||
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, -30_deg, 0_rad}};
|
||||
inline const frc::AprilTagFieldLayout kTagLayout{
|
||||
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)};
|
||||
|
||||
inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
|
||||
inline const Eigen::Matrix<double, 3, 1> 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<units::meters> kDriveFF{
|
||||
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::radians> 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
|
||||
@@ -28,27 +28,33 @@
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/motorcontrol/PWMVictorSPX.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/length.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "VisionSim.h"
|
||||
#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{constants::Vision::kCameraName};
|
||||
SwerveDrive drivetrain{};
|
||||
VisionSim vision{&camera};
|
||||
frc::XboxController controller{0};
|
||||
static constexpr double VISION_TURN_kP = 0.01;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* 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 <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonPoseEstimator.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/simulation/VisionSystemSim.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
#include <photon/targeting/PhotonPipelineResult.h>
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class VisionSim {
|
||||
public:
|
||||
explicit VisionSim(photon::PhotonCamera* camera) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
visionSim = std::make_unique<photon::VisionSystemSim>("main");
|
||||
|
||||
visionSim->AddAprilTags(constants::Vision::kTagLayout);
|
||||
|
||||
cameraProp = std::make_unique<photon::SimCameraProperties>();
|
||||
|
||||
cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibError(.35, .10);
|
||||
cameraProp->SetFPS(70_Hz);
|
||||
cameraProp->SetAvgLatency(30_ms);
|
||||
cameraProp->SetLatencyStdDev(15_ms);
|
||||
|
||||
cameraSim =
|
||||
std::make_shared<photon::PhotonCameraSim>(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<photon::VisionSystemSim> visionSim;
|
||||
std::unique_ptr<photon::SimCameraProperties> cameraProp;
|
||||
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
|
||||
|
||||
// The most recent result, cached for calculating std devs
|
||||
photon::PhotonPipelineResult m_latestResult;
|
||||
};
|
||||
@@ -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 <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/SPI.h>
|
||||
#include <frc/estimator/SwerveDrivePoseEstimator.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
||||
|
||||
#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<frc::SwerveModuleState, 4>& 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<frc::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<frc::Pose2d, 4> GetModulePoses() const;
|
||||
void Log();
|
||||
void SimulationPeriodic();
|
||||
frc::Pose2d GetSimPose() const;
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::array<SwerveModule, 4> 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};
|
||||
};
|
||||
@@ -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 <random>
|
||||
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
static constexpr int numModules{4};
|
||||
|
||||
class SwerveDriveSim {
|
||||
public:
|
||||
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& 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<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
static Eigen::Matrix<double, 2, 1> CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& 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<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleSteerStates);
|
||||
frc::Pose2d GetPose() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
|
||||
units::meter_t driveStdDev, units::radian_t steerStdDev);
|
||||
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
|
||||
std::array<Eigen::Matrix<double, 2, 1>, 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<units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
units::ampere_t GetTotalCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::random_device rd{};
|
||||
std::mt19937 generator{rd()};
|
||||
std::normal_distribution<double> 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<numModules> kinematics;
|
||||
std::array<units::volt_t, numModules> driveInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
|
||||
std::array<units::volt_t, numModules> steerInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
|
||||
frc::Pose2d pose{frc::Pose2d{}};
|
||||
units::radians_per_second_t omega{0};
|
||||
};
|
||||
@@ -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 <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <units/current.h>
|
||||
|
||||
#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};
|
||||
};
|
||||
BIN
photonlib-cpp-examples/aimattarget/swerve_module.png
Normal file
BIN
photonlib-cpp-examples/aimattarget/swerve_module.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
BIN
photonlib-cpp-examples/aimattarget/tag-blue.png
Normal file
BIN
photonlib-cpp-examples/aimattarget/tag-blue.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
BIN
photonlib-cpp-examples/aimattarget/tag-green.png
Normal file
BIN
photonlib-cpp-examples/aimattarget/tag-green.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
Reference in New Issue
Block a user