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(); }