Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Start on updating examples for 2027

Browse Source 
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
This commit is contained in:
Jade Turner
2025-06-24 10:17:44 +08:00
committed by samfreund
parent ed1b31cb7f
commit a5f9a0b673
16 changed files with 100 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp
View File

@@ -32,7 +32,7 @@
SwerveDrive::SwerveDrive()
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
gyroSim(gyro),
// gyroSim(gyro),
swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
constants::Swerve::kDriveGearRatio,
constants::Swerve::kWheelDiameter / 2,
@@ -82,8 +82,8 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
for (int i = 0; i < swerveMods.size(); i++) {
swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
}
gyroSim.SetAngle(-pose.Rotation().Degrees());
gyroSim.SetRate(0_rad_per_s);
// gyroSim.SetAngle(-pose.Rotation().Degrees());
// gyroSim.SetRate(0_rad_per_s);
}
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
@@ -190,8 +190,8 @@ void SwerveDrive::SimulationPeriodic() {
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
steerPos, steerRate, steerCurrents[i]);
}
gyroSim.SetRate(-swerveDriveSim.GetOmega());
gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
// gyroSim.SetRate(-swerveDriveSim.GetOmega());
// gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
}
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }

9
photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h
View File

@@ -24,11 +24,9 @@
#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 <frc/OboardIMU.h>
#include "SwerveDriveSim.h"
#include "SwerveModule.h"
@@ -70,11 +68,12 @@ class SwerveDrive {
swerveMods[2].GetModuleConstants().centerOffset,
swerveMods[3].GetModuleConstants().centerOffset,
};
frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
frc::OnboardIMU gyro{MountOrientation::kFlat};
frc::SwerveDrivePoseEstimator<4> poseEstimator;
frc::ChassisSpeeds targetChassisSpeeds{};
frc::sim::ADXRS450_GyroSim gyroSim;
//TODO(Jade) onboard imu doesn't have sim yet
// frc::sim::ADXRS450_GyroSim gyroSim;
SwerveDriveSim swerveDriveSim;
units::ampere_t totalCurrentDraw{0};
};