Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -32,7 +32,8 @@ Drivetrain::Drivetrain() {
wpi::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
}
void Drivetrain::SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
const double leftOutput = m_leftPIDController.Calculate(
@@ -75,12 +76,15 @@ wpi::math::Pose3d Drivetrain::ObjectToRobotPose(
std::vector<double> val{cameraToObjectEntry.Get()};
// Reconstruct cameraToObject Transform3D from networktables.
wpi::math::Translation3d translation{wpi::units::meter_t{val[0]}, wpi::units::meter_t{val[1]},
wpi::units::meter_t{val[2]}};
wpi::math::Rotation3d rotation{wpi::math::Quaternion{val[3], val[4], val[5], val[6]}};
wpi::math::Translation3d translation{wpi::units::meter_t{val[0]},
wpi::units::meter_t{val[1]},
wpi::units::meter_t{val[2]}};
wpi::math::Rotation3d rotation{
wpi::math::Quaternion{val[3], val[4], val[5], val[6]}};
wpi::math::Transform3d cameraToObject{translation, rotation};
return wpi::math::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera);
return wpi::math::ObjectToRobotPose(objectInField, cameraToObject,
robotToCamera);
}
void Drivetrain::UpdateOdometry() {
@@ -98,8 +102,8 @@ void Drivetrain::UpdateOdometry() {
wpi::math::Pose3d visionMeasurement3d = ObjectToRobotPose(
m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef);
// Convert robot's pose from wpi::math::Pose3d to wpi::math::Pose2d needed to apply vision
// measurements.
// Convert robot's pose from wpi::math::Pose3d to wpi::math::Pose2d needed to
// apply vision measurements.
wpi::math::Pose2d visionMeasurement2d = visionMeasurement3d.ToPose2d();
// Apply vision measurements. For simulation purposes only, we don't input a

21
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp
View File

@@ -106,9 +106,9 @@ class Drivetrain {
* @param cameraToObjectEntry The networktables entry publishing and querying
* example computer vision measurements.
*/
wpi::math::Pose3d ObjectToRobotPose(wpi::math::Pose3d objectInField,
wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry);
wpi::math::Pose3d ObjectToRobotPose(
wpi::math::Pose3d objectInField, wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry);
private:
static constexpr wpi::units::meter_t kTrackwidth = 0.381_m * 2;
@@ -120,9 +120,11 @@ class Drivetrain {
wpi::math::Transform3d m_robotToCamera{
wpi::math::Translation3d{1_m, 1_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}}};
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi / 2}}};
wpi::nt::NetworkTableInstance m_inst{wpi::nt::NetworkTableInstance::GetDefault()};
wpi::nt::NetworkTableInstance m_inst{
wpi::nt::NetworkTableInstance::GetDefault()};
wpi::nt::DoubleArrayTopic m_cameraToObjectTopic{
m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")};
wpi::nt::DoubleArrayEntry m_cameraToObjectEntry =
@@ -130,8 +132,10 @@ class Drivetrain {
wpi::nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
wpi::apriltag::AprilTagFieldLayout m_aprilTagFieldLayout{
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo)};
wpi::math::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo)};
wpi::math::Pose3d m_objectInField{
m_aprilTagFieldLayout.GetTagPose(0).value()};
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
@@ -161,7 +165,8 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
1_V, 3_V / 1_mps};
// Simulation classes
wpi::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};

9
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

@@ -16,9 +16,10 @@ class ExampleGlobalMeasurementSensor {
static wpi::math::Pose2d GetEstimatedGlobalPose(
const wpi::math::Pose2d& estimatedRobotPose) {
auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return wpi::math::Pose2d{estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
return wpi::math::Pose2d{
estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
}
};