Skip to content

Commit

Permalink
Fixed cpp sim apriltag layout and cleaned up cpp sim example (#1190)
Browse files Browse the repository at this point in the history 
* Fixed cpp sim apriltag layout and cleaned up cpp sim example

* changed layout for photoncamerasim

---------

Co-authored-by: Drew Williams <[email protected]>
  • Loading branch information
@r4stered
r4stered and Drew Williams committed Jan 22, 2024
1 parent 939283d commit a3e1dda
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 36 deletions.
2 changes: 1 addition & 1 deletion photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -435,7 +435,7 @@ class PhotonCameraSim {
double minTargetAreaPercent;

frc::AprilTagFieldLayout tagLayout{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)};
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)};

cs::CvSource videoSimRaw;
cv::Mat videoSimFrameRaw{};
Expand Down
60 changes: 30 additions & 30 deletions photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,48 +35,48 @@

namespace constants {
namespace Vision {
static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
static const frc::Transform3d kRobotToCam{
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}};
static const frc::AprilTagFieldLayout kTagLayout{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)};
inline const frc::AprilTagFieldLayout kTagLayout{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)};

static const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
static const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
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 {

static constexpr units::meter_t kTrackWidth{18.5_in};
static constexpr units::meter_t kTrackLength{18.5_in};
static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
static constexpr units::meter_t kWheelDiameter{4_in};
static constexpr units::meter_t kWheelCircumference{kWheelDiameter *
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};

static constexpr double kDriveGearRatio = 6.75;
static constexpr double kSteerGearRatio = 12.8;
inline constexpr double kDriveGearRatio = 6.75;
inline constexpr double kSteerGearRatio = 12.8;

static constexpr units::meter_t kDriveDistPerPulse =
inline constexpr units::meter_t kDriveDistPerPulse =
kWheelCircumference / 1024.0 / kDriveGearRatio;
static constexpr units::radian_t kSteerRadPerPulse =
inline constexpr units::radian_t kSteerRadPerPulse =
units::radian_t{2 * std::numbers::pi} / 1024.0;

static constexpr double kDriveKP = 1.0;
static constexpr double kDriveKI = 0.0;
static constexpr double kDriveKD = 0.0;
inline constexpr double kDriveKP = 1.0;
inline constexpr double kDriveKI = 0.0;
inline constexpr double kDriveKD = 0.0;

static constexpr double kSteerKP = 20.0;
static constexpr double kSteerKI = 0.0;
static constexpr double kSteerKD = 0.25;
inline constexpr double kSteerKP = 20.0;
inline constexpr double kSteerKI = 0.0;
inline constexpr double kSteerKD = 0.25;

static const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};

static const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
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 {
Expand Down Expand Up @@ -106,13 +106,13 @@ struct ModuleConstants {
centerOffset(frc::Translation2d{xOffset, yOffset}) {}
};

static const ModuleConstants FL_CONSTANTS{
inline const ModuleConstants FL_CONSTANTS{
1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
static const ModuleConstants FR_CONSTANTS{
inline const ModuleConstants FR_CONSTANTS{
2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
static const ModuleConstants BL_CONSTANTS{
inline const ModuleConstants BL_CONSTANTS{
3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
static const ModuleConstants BR_CONSTANTS{
inline const ModuleConstants BR_CONSTANTS{
4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
} // namespace Swerve
} // namespace constants
8 changes: 3 additions & 5 deletions photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class Vision {
cameraSim = std::make_shared<photon::PhotonCameraSim>(camera.get(),
*cameraProp.get());

visionSim->AddCamera(cameraSim.get(), robotToCam);
visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
cameraSim->EnableDrawWireframe(true);
}
}
Expand Down Expand Up @@ -138,12 +138,10 @@ class Vision {
frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }

private:
frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m},
frc::Rotation3d{}};
photon::PhotonPoseEstimator photonEstimator{
LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
constants::Vision::kTagLayout,
photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
photon::PhotonCamera{"photonvision"}, robotToCam};
photon::PhotonCamera{"photonvision"}, constants::Vision::kRobotToCam};
std::shared_ptr<photon::PhotonCamera> camera{photonEstimator.GetCamera()};
std::unique_ptr<photon::VisionSystemSim> visionSim;
std::unique_ptr<photon::SimCameraProperties> cameraProp;
Expand Down

0 comments on commit a3e1dda

Please sign in to comment.