mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-09 00:28:06 -06:00
astatin3
126 lines
4.8 KiB
C++
126 lines
4.8 KiB
C++
/*
|
|
* 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/acceleration.h>
|
|
#include <units/angular_acceleration.h>
|
|
#include <units/angular_velocity.h>
|
|
#include <units/length.h>
|
|
#include <units/velocity.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::AprilTagFieldLayout::LoadField(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 {
|
|
using namespace units;
|
|
|
|
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;
|
|
|
|
using namespace units;
|
|
|
|
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
|