From a052ea5e16c118a3927771a7033ed718024a7679 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Thu, 29 Aug 2024 17:17:55 -0700 Subject: [PATCH 01/12] Adding persistent 'trim' --- src/y2024/cpp/FunkyRobot.cc | 23 -------------- src/y2024/cpp/control_triggers.cc | 30 +++++++++++++++++++ .../cpp/subsystems/abstract/control_input.cc | 8 +++++ .../subsystems/abstract/control_input.h | 2 ++ .../subsystems/abstract/super_structure.h | 11 ++++++- 5 files changed, 50 insertions(+), 24 deletions(-) diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index 5525b35..5b6b4b4 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -78,29 +78,6 @@ void FunkyRobot::InitTeleop() { container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); container_.bracer_.SetDefaultCommand(BracerCommand{container_}); - frc2::Trigger drivetrain_zero_bearing_trigger{ - [&] { return container_.control_input_.GetReadings().zero_bearing; }}; - - drivetrain_zero_bearing_trigger.WhileTrue( - frc2::InstantCommand([this] { - container_.drivetrain_.SetBearing( - frc846::util::ShareTables::GetBoolean("is_red_side") ? 0_deg - : 180_deg); - }).ToPtr()); - - frc2::Trigger on_piece_trigger{[&] { - return frc846::util::ShareTables::GetBoolean("scorer_has_piece"); - }}; - - on_piece_trigger.OnTrue( - frc2::InstantCommand( - [&] { container_.control_input_.SetTarget({true, false}); }) - .WithTimeout(1_s) - .AndThen(frc2::WaitCommand(1_s).ToPtr()) - .AndThen(frc2::InstantCommand([&] { - container_.control_input_.SetTarget({false, false}); - }).ToPtr())); - frc2::Trigger on_coast_trigger{[&] { return coasting_switch_.Get(); }}; on_coast_trigger.OnTrue(frc2::InstantCommand([&] { diff --git a/src/y2024/cpp/control_triggers.cc b/src/y2024/cpp/control_triggers.cc index ebc434f..4cdd595 100644 --- a/src/y2024/cpp/control_triggers.cc +++ b/src/y2024/cpp/control_triggers.cc @@ -18,6 +18,29 @@ #include "commands/complex/super_amp_command.h" void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { + frc2::Trigger drivetrain_zero_bearing_trigger{ + [&] { return container.control_input_.GetReadings().zero_bearing; }}; + + drivetrain_zero_bearing_trigger.WhileTrue( + frc2::InstantCommand([&] { + container.drivetrain_.SetBearing( + frc846::util::ShareTables::GetBoolean("is_red_side") ? 0_deg + : 180_deg); + }).ToPtr()); + + frc2::Trigger on_piece_trigger{[&] { + return frc846::util::ShareTables::GetBoolean("scorer_has_piece"); + }}; + + on_piece_trigger.OnTrue( + frc2::InstantCommand( + [&] { container.control_input_.SetTarget({true, false}); }) + .WithTimeout(1_s) + .AndThen(frc2::WaitCommand(1_s).ToPtr()) + .AndThen(frc2::InstantCommand([&] { + container.control_input_.SetTarget({false, false}); + }).ToPtr())); + frc2::Trigger amp_command_trigger{[&container] { return container.control_input_.GetReadings().running_amp; }}; @@ -106,4 +129,11 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { }}; wrist_zero_trigger.OnTrue(WristZeroCommand{container}.ToPtr()); + + frc2::Trigger wrist_trim_save_trigger{ + [&] { return container.control_input_.GetReadings().save_trim; }}; + + wrist_trim_save_trigger.OnTrue(frc2::InstantCommand([&] { + container.super_structure_.SaveAdjustments(); + }).ToPtr()); } \ No newline at end of file diff --git a/src/y2024/cpp/subsystems/abstract/control_input.cc b/src/y2024/cpp/subsystems/abstract/control_input.cc index 7230f02..9d3ea3a 100644 --- a/src/y2024/cpp/subsystems/abstract/control_input.cc +++ b/src/y2024/cpp/subsystems/abstract/control_input.cc @@ -98,6 +98,11 @@ ControlInputReadings ControlInputSubsystem::ReadFromHardware() { readings.home_wrist ? 1 : 0); } + if (readings.save_trim != previous_readings_.save_trim) { + Log("ControlInput [Save Trim] state changed to {}", + readings.save_trim ? 1 : 0); + } + if (std::abs(readings.pivot_manual_adjust) > 0.01 && !(std::abs(previous_readings_.pivot_manual_adjust) > 0.01)) { Log("ControlInput [Pivot Manual Adjustment]"); @@ -191,6 +196,9 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() { ci_readings_.home_wrist = dr_readings.b_button; ci_readings_.zero_bearing = dr_readings.back_button; + // SAVE TRIM + ci_readings_.save_trim = op_readings.x_button; + previous_operator_ = op_readings; previous_driver_ = dr_readings; diff --git a/src/y2024/include/subsystems/abstract/control_input.h b/src/y2024/include/subsystems/abstract/control_input.h index d3c48d0..253473b 100644 --- a/src/y2024/include/subsystems/abstract/control_input.h +++ b/src/y2024/include/subsystems/abstract/control_input.h @@ -34,6 +34,8 @@ struct ControlInputReadings { bool home_wrist; bool zero_bearing; + + bool save_trim; }; struct ControlInputTarget { diff --git a/src/y2024/include/subsystems/abstract/super_structure.h b/src/y2024/include/subsystems/abstract/super_structure.h index 256d39d..9bbde47 100644 --- a/src/y2024/include/subsystems/abstract/super_structure.h +++ b/src/y2024/include/subsystems/abstract/super_structure.h @@ -53,6 +53,9 @@ class SuperStructureSubsystem PTWSetpoint currentSetpoint; PTWSetpoint manualAdjustments; + PTWSetpoint lastAdjustments; + PTWSetpoint savedAdjustments; + public: PivotSubsystem* pivot_; WristSubsystem* wrist_; @@ -92,6 +95,7 @@ class SuperStructureSubsystem currentSetpoint.wrist != newSetpoint.wrist || currentSetpoint.telescope != newSetpoint.telescope) { currentSetpoint = newSetpoint; + lastAdjustments = manualAdjustments; ClearAdjustments(); } } @@ -134,7 +138,12 @@ class SuperStructureSubsystem } void ClearAdjustments() { - manualAdjustments = PTWSetpoint{0.0_deg, 0.0_in, 0.0_deg}; + manualAdjustments = {savedAdjustments.pivot, savedAdjustments.telescope, + savedAdjustments.wrist}; + } + + void SaveAdjustments() { + savedAdjustments = {0.0_deg, 0.0_in, lastAdjustments.wrist}; } bool hasReachedSetpoint(PTWSetpoint setpoint) { From ac72dc65be9fd7dce7dc5bb89f47b6d6e8a4e2a1 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Tue, 10 Sep 2024 17:22:49 -0700 Subject: [PATCH 02/12] Arch math update --- .../{util/math.cc => math/collection.cc} | 12 +- .../{other => ntinf}/sendable_callback.cc | 13 +- .../cpp/frc846/other/swerve_odometry.cc | 64 ++-- .../cpp/frc846/other/trajectory_generator.cc | 77 +++-- src/frc846/cpp/frc846/robot/GenericRobot.cc | 4 +- src/frc846/cpp/frc846/robot/RobotState.cc | 25 ++ src/frc846/include/frc846/base/fserver.h | 1 + src/frc846/include/frc846/control/motion.h | 18 +- src/frc846/include/frc846/math/calculator.h | 38 +++ src/frc846/include/frc846/math/collection.h | 32 ++ src/frc846/include/frc846/math/constants.h | 16 + src/frc846/include/frc846/math/fieldpoints.h | 71 +++++ src/frc846/include/frc846/math/vectors.h | 282 +++++++++++++++--- .../sendable_callback.h => ntinf/ntaction.h} | 10 +- .../include/frc846/other/swerve_odometry.h | 17 +- .../frc846/other/trajectory_generator.h | 21 +- .../include/frc846/robot/GenericCommand.h | 2 +- src/frc846/include/frc846/robot/RobotState.h | 65 ++++ src/frc846/include/frc846/util/math.h | 187 ------------ src/y2024/cpp/FunkyRobot.cc | 52 ++-- src/y2024/cpp/autos/drive_auto.cc | 23 +- src/y2024/cpp/autos/five_piece_auto.cc | 65 ++-- src/y2024/cpp/autos/one_piece.cc | 15 +- .../calculators/arm_kinematics_calculator.cc | 59 ++++ .../cpp/calculators/shooting_calculator.cc | 44 +++ .../commands/auto_intake_and_shoot_command.cc | 4 +- .../basic/prepare_auto_shoot_command.cc | 12 - .../commands/basic/prepare_shoot_command.cc | 28 +- src/y2024/cpp/commands/basic/shoot_command.cc | 12 +- .../complex/close_drive_amp_command.cc | 16 +- .../cpp/commands/complex/super_amp_command.cc | 10 +- .../cpp/commands/follow_trajectory_command.cc | 76 ++--- .../cpp/commands/teleop/drive_command.cc | 34 +-- src/y2024/cpp/constants.cc | 35 --- src/y2024/cpp/control_triggers.cc | 36 ++- src/y2024/cpp/field.cc | 52 ++-- src/y2024/cpp/main.cpp | 6 +- .../cpp/subsystems/abstract/control_input.cc | 15 +- src/y2024/cpp/subsystems/abstract/driver.cc | 5 +- .../subsystems/abstract/super_structure.cc | 22 +- src/y2024/cpp/subsystems/abstract/vision.cc | 32 +- .../cpp/subsystems/hardware/drivetrain.cc | 99 +++--- src/y2024/include/autos/drive_auto.h | 1 - src/y2024/include/autos/five_piece_auto.h | 1 - src/y2024/include/autos/one_piece_auto.h | 1 - .../calculators/arm_kinematics_calculator.h | 43 +++ .../include/calculators/shooting_calculator.h | 40 +++ .../commands/auto_intake_and_shoot_command.h | 7 +- .../include/commands/basic/amp_command.h | 1 - .../basic/auto_deploy_intake_command.h | 1 - .../basic/prepare_auto_shoot_command.h | 6 +- .../commands/basic/prepare_shoot_command.h | 3 +- .../include/commands/basic/shoot_command.h | 3 + .../commands/complex/super_amp_command.h | 1 - .../commands/follow_trajectory_command.h | 11 +- .../include/commands/teleop/drive_command.h | 1 + src/y2024/include/constants.h | 193 ------------ src/y2024/include/field.h | 148 +++++---- .../subsystems/abstract/control_input.h | 3 +- .../include/subsystems/abstract/driver.h | 1 - .../include/subsystems/abstract/operator.h | 1 - .../subsystems/abstract/super_structure.h | 50 ++-- .../include/subsystems/hardware/drivetrain.h | 15 +- src/y2024/include/subsystems/hardware/pivot.h | 3 +- .../subsystems/hardware/swerve_module.h | 1 - src/y2024/include/subsystems/hardware/wrist.h | 37 +-- src/y2024/resources/preferences_backup.nform | 33 +- .../resources/preferences_cl_backup.nform | Bin 18417 -> 20041 bytes 68 files changed, 1264 insertions(+), 1048 deletions(-) rename src/frc846/cpp/frc846/{util/math.cc => math/collection.cc} (89%) rename src/frc846/cpp/frc846/{other => ntinf}/sendable_callback.cc (54%) create mode 100644 src/frc846/cpp/frc846/robot/RobotState.cc create mode 100644 src/frc846/include/frc846/math/calculator.h create mode 100644 src/frc846/include/frc846/math/collection.h create mode 100644 src/frc846/include/frc846/math/constants.h create mode 100644 src/frc846/include/frc846/math/fieldpoints.h rename src/frc846/include/frc846/{other/sendable_callback.h => ntinf/ntaction.h} (55%) create mode 100644 src/frc846/include/frc846/robot/RobotState.h delete mode 100644 src/frc846/include/frc846/util/math.h create mode 100644 src/y2024/cpp/calculators/arm_kinematics_calculator.cc create mode 100644 src/y2024/cpp/calculators/shooting_calculator.cc delete mode 100644 src/y2024/cpp/constants.cc create mode 100644 src/y2024/include/calculators/arm_kinematics_calculator.h create mode 100644 src/y2024/include/calculators/shooting_calculator.h delete mode 100644 src/y2024/include/constants.h diff --git a/src/frc846/cpp/frc846/util/math.cc b/src/frc846/cpp/frc846/math/collection.cc similarity index 89% rename from src/frc846/cpp/frc846/util/math.cc rename to src/frc846/cpp/frc846/math/collection.cc index 0a55558..242d043 100644 --- a/src/frc846/cpp/frc846/util/math.cc +++ b/src/frc846/cpp/frc846/math/collection.cc @@ -1,6 +1,12 @@ -#include "frc846/util/math.h" +#include "frc846/math/collection.h" -namespace frc846::util { +#include + +namespace frc846::math { + +bool DEquals(double x, double y, double epsilon) { + return std::abs(x - y) < epsilon; +} double HorizontalDeadband(double input, double x_intercept, double max, double exponent, double sensitivity) { @@ -56,4 +62,4 @@ units::degree_t CoterminalSum(units::degree_t angle, } } -} // namespace frc846::util \ No newline at end of file +} // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/cpp/frc846/other/sendable_callback.cc b/src/frc846/cpp/frc846/ntinf/sendable_callback.cc similarity index 54% rename from src/frc846/cpp/frc846/other/sendable_callback.cc rename to src/frc846/cpp/frc846/ntinf/sendable_callback.cc index e56e5cd..f744a64 100644 --- a/src/frc846/cpp/frc846/other/sendable_callback.cc +++ b/src/frc846/cpp/frc846/ntinf/sendable_callback.cc @@ -1,12 +1,11 @@ -#include "frc846/other/sendable_callback.h" - #include -namespace frc846::other { +#include "frc846/ntinf/ntaction.h" + +namespace frc846::ntinf { -SendableCallback::SendableCallback(std::function callback) - : callback_(callback) {} -void SendableCallback::InitSendable(wpi::SendableBuilder& builder) { +NTAction::NTAction(std::function callback) : callback_(callback) {} +void NTAction::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("Command"); builder.AddStringProperty(".name", [] { return "Run"; }, nullptr); builder.AddBooleanProperty( @@ -18,4 +17,4 @@ void SendableCallback::InitSendable(wpi::SendableBuilder& builder) { }); } -} // namespace frc846::other \ No newline at end of file +} // namespace frc846::ntinf \ No newline at end of file diff --git a/src/frc846/cpp/frc846/other/swerve_odometry.cc b/src/frc846/cpp/frc846/other/swerve_odometry.cc index bc0b8fa..7222232 100644 --- a/src/frc846/cpp/frc846/other/swerve_odometry.cc +++ b/src/frc846/cpp/frc846/other/swerve_odometry.cc @@ -3,75 +3,47 @@ #include #include -#include "frc846/util/math.h" -#include "frc846/util/share_tables.h" +#include "frc846/math/vectors.h" namespace frc846 { -SwerveOdometry::SwerveOdometry(util::Position initial_pose) - : pose_(initial_pose) {} +SwerveOdometry::SwerveOdometry( + frc846::math::VectorND initial_position) + : position_{initial_position[0], initial_position[1]} {} void SwerveOdometry::Update( - std::array, kModuleCount> wheel_vecs, + std::array, kModuleCount> + wheel_vecs, units::radian_t bearing) { // change in distance from the last odometry update for (int i = 0; i < kModuleCount; i++) { - units::foot_t wheel_dist = wheel_vecs[i].Magnitude(); + units::foot_t wheel_dist = wheel_vecs[i].magnitude(); units::foot_t delta_distance = wheel_dist - prev_wheel_distances_[i]; units::foot_t dx = - delta_distance * units::math::cos(wheel_vecs[i].Bearing() + bearing); + delta_distance * units::math::sin(wheel_vecs[i].angle(true) + bearing); units::foot_t dy = - delta_distance * units::math::sin(wheel_vecs[i].Bearing() + bearing); + delta_distance * units::math::cos(wheel_vecs[i].angle(true) + bearing); prev_wheel_distances_[i] = wheel_dist; - wheel_vecs[i] = {dx, dy}; + wheel_vecs[i][0] = dx; + wheel_vecs[i][1] = dy; } - // get the distance components of each of the module, accounting for the robot - // bearing - std::array, kModuleCount> xy_comps; + frc846::math::VectorND relative_displacement{0_ft, 0_ft}; for (int i = 0; i < kModuleCount; i++) { - xy_comps[i] = { - wheel_vecs[i].Magnitude() * units::math::sin(wheel_vecs[i].Bearing()), - wheel_vecs[i].Magnitude() * units::math::cos(wheel_vecs[i].Bearing()), - }; + relative_displacement += wheel_vecs[i] / kModuleCount; } - // the distance travelled by each "side" of the robot - units::foot_t top = (xy_comps[0].x + xy_comps[1].x) / 2; - units::foot_t bottom = (xy_comps[3].x + xy_comps[2].x) / 2; - units::foot_t left = (xy_comps[0].y + xy_comps[2].y) / 2; - units::foot_t right = (xy_comps[1].y + xy_comps[3].y) / 2; - - units::radian_t theta = bearing - pose_.bearing; - - auto sin_theta = units::math::sin(theta); - auto cos_theta = units::math::cos(theta); - - auto top_bottom = util::Vector2D{ - (left + right) * sin_theta, - (left + right) * cos_theta, - }; - auto left_right = util::Vector2D{ - (top + bottom) * cos_theta, - (top + bottom) * -sin_theta, - }; - - pose_.point.x += (top_bottom.y + left_right.y) / 2; - pose_.point.y += (top_bottom.x + left_right.x) / 2; - - pose_.bearing = bearing; + position_ += relative_displacement.rotate(bearing, true); } -void SwerveOdometry::SetPoint(util::Vector2D point) { - pose_.point = point; +void SwerveOdometry::SetPoint(frc846::math::VectorND point) { + position_[0] = point[0]; + position_[1] = point[1]; } -void SwerveOdometry::Zero() { - SetPoint({0_ft, 0_ft}); - pose_.bearing = 0_deg; -} +void SwerveOdometry::Zero() { SetPoint({0_ft, 0_ft}); } } // namespace frc846 \ No newline at end of file diff --git a/src/frc846/cpp/frc846/other/trajectory_generator.cc b/src/frc846/cpp/frc846/other/trajectory_generator.cc index 2819390..f0cb89c 100644 --- a/src/frc846/cpp/frc846/other/trajectory_generator.cc +++ b/src/frc846/cpp/frc846/other/trajectory_generator.cc @@ -3,27 +3,27 @@ #include #include "frc846/base/loggable.h" -#include "frc846/util/math.h" namespace frc846 { -std::vector InterpolatePoints( - util::Vector2D start, util::Vector2D end, - units::degree_t start_bearing, units::degree_t end_bearing, - units::foot_t cut) { - auto distance = (end - start).Magnitude(); +std::vector InterpolatePoints( + frc846::math::VectorND start, + frc846::math::VectorND end, units::degree_t start_bearing, + units::degree_t end_bearing, units::foot_t cut) { + auto distance = (end - start).magnitude(); int n = std::max(units::math::ceil(distance / cut).to(), 1); - std::vector points(n); + std::vector points(n); for (int i = 0; i < n; ++i) { double weight = (double)(i) / n; double bearingWeightMultiplier = 1.15; points[i] = {{ - start.x * (1 - weight) + end.x * weight, - start.y * (1 - weight) + end.y * weight, + start[0] * (1 - weight) + end[0] * weight, + start[1] * (1 - weight) + end[1] * weight, }, start_bearing * (1 - (weight * bearingWeightMultiplier)) + - end_bearing * (weight * bearingWeightMultiplier)}; + end_bearing * (weight * bearingWeightMultiplier), + {0.0_fps, 0.0_fps}}; if (start_bearing <= end_bearing) { points[i].bearing = units::math::min(end_bearing, points[i].bearing); @@ -36,8 +36,7 @@ std::vector InterpolatePoints( } Trajectory GenerateTrajectory( - std::vector input_points, - units::feet_per_second_t robot_max_v, + std::vector input_points, units::feet_per_second_t robot_max_v, units::feet_per_second_squared_t robot_max_acceleration, units::feet_per_second_squared_t robot_max_deceleration, units::inch_t cut) { @@ -59,42 +58,33 @@ Trajectory GenerateTrajectory( input_points[i].pos.bearing, input_points[i - 1].pos.bearing, cut); interpolated_points.erase(interpolated_points.begin()); - trajectory.push_back({ - input_points[i].pos, - input_points[i].v_max.value_or(robot_max_v), - }); + trajectory.push_back({input_points[i].pos}); for (auto point : interpolated_points) { - trajectory.push_back({ - point, - robot_max_v, - }); + point.velocity[0] = robot_max_v; + trajectory.push_back({point}); } } - trajectory.push_back({ - input_points[0].pos, - input_points[0].v_max.value_or(robot_max_v), - }); + trajectory.push_back({input_points[0].pos}); for (unsigned int i = 1; i < trajectory.size(); ++i) { auto delta_pos = - (trajectory[i].pos.point - trajectory[i - 1].pos.point).Magnitude(); + (trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude(); // v₂² = v₁² + 2aΔx // 2aΔx = v₂² - v₁² // a = (v₂² - v₁²) / (2Δx) - auto deceleration = (units::math::pow<2>(trajectory[i].v) - - units::math::pow<2>(trajectory[i - 1].v)) / - (2 * delta_pos); + auto deceleration = + (units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) - + units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) / + (2 * delta_pos); if (deceleration > robot_max_deceleration) { // v₂² = v₁² + 2aΔx // v₂² = sqrt(v₁² + 2aΔx) - // this code was replaced with calculateW because it makes the incorrect - // assumption that the values are positive - trajectory[i].v = - units::math::sqrt(units::math::pow<2>(trajectory[i - 1].v) + - 2 * robot_max_deceleration * delta_pos); + trajectory[i].pos.velocity[0] = units::math::sqrt( + units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) + + 2 * robot_max_deceleration * delta_pos); } } @@ -102,20 +92,21 @@ Trajectory GenerateTrajectory( for (unsigned int i = 1; i < trajectory.size(); ++i) { auto delta_pos = - (trajectory[i].pos.point - trajectory[i - 1].pos.point).Magnitude(); + (trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude(); // v₂² = v₁² + 2aΔx // 2aΔx = v₂² - v₁² // a = (v₂² - v₁²) / (2Δx) - auto acceleration = (units::math::pow<2>(trajectory[i].v) - - units::math::pow<2>(trajectory[i - 1].v)) / - (2 * delta_pos); + auto acceleration = + (units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) - + units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) / + (2 * delta_pos); if (acceleration > robot_max_acceleration) { // v₂² = v₁² + 2aΔx // v₂² = sqrt(v₁² + 2aΔx) - trajectory[i].v = - units::math::sqrt(units::math::pow<2>(trajectory[i - 1].v) + - 2 * robot_max_acceleration * delta_pos); + trajectory[i].pos.velocity[0] = units::math::sqrt( + units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) + + 2 * robot_max_acceleration * delta_pos); // trajectory[i].v = // units::velocity::feet_per_second_t(Find_Vsub2(units::unit_cast(trajectory[i].v), @@ -126,8 +117,10 @@ Trajectory GenerateTrajectory( // If any point has 0 speed, just set it to the previous speed. for (unsigned int i = 0; i < trajectory.size(); ++i) { - if (trajectory[i].v == 0_fps) { - trajectory[i].v = i == 0 ? trajectory[1].v : trajectory[i - 1].v; + if (trajectory[i].pos.velocity.magnitude() == 0_fps) { + trajectory[i].pos.velocity.magnitude() = + i == 0 ? trajectory[1].pos.velocity.magnitude() + : trajectory[i - 1].pos.velocity.magnitude(); } } diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc index b4a0611..eaa203f 100644 --- a/src/frc846/cpp/frc846/robot/GenericRobot.cc +++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc @@ -15,7 +15,7 @@ #include "frc2/command/ParallelDeadlineGroup.h" #include "frc2/command/WaitCommand.h" #include "frc846/base/loggable.h" -#include "frc846/other/sendable_callback.h" +#include "frc846/ntinf/ntaction.h" #include "frc846/robot/GenericCommand.h" #include "frc846/wpilib/time.h" @@ -57,7 +57,7 @@ void GenericRobot::StartCompetition() { frc::SmartDashboard::PutData( "verify_hardware", - new frc846::other::SendableCallback([this] { VerifyHardware(); })); + new frc846::ntinf::NTAction([this] { VerifyHardware(); })); // Verify robot hardware VerifyHardware(); diff --git a/src/frc846/cpp/frc846/robot/RobotState.cc b/src/frc846/cpp/frc846/robot/RobotState.cc new file mode 100644 index 0000000..90d9626 --- /dev/null +++ b/src/frc846/cpp/frc846/robot/RobotState.cc @@ -0,0 +1,25 @@ +#include "frc846/robot/RobotState.h" + +namespace frc846::robot { + +RSTable::RSTable(std::string table_name) + : frc846::base::Loggable{table_name}, table_{} {} + +std::vector RSTable::ListKeys() { + std::vector keys; + for (const auto& [key, value] : table_) { + keys.push_back(key); + } + return keys; +} + +std::unordered_map RobotState::tables_{}; + +RSTable* RobotState::getTable(std::string table_name) { + if (tables_.find(table_name) == tables_.end()) { + tables_.insert({table_name, new RSTable{table_name}}); + } + return tables_[table_name]; +} + +}; // namespace frc846::robot \ No newline at end of file diff --git a/src/frc846/include/frc846/base/fserver.h b/src/frc846/include/frc846/base/fserver.h index 51f8e32..06c02d3 100644 --- a/src/frc846/include/frc846/base/fserver.h +++ b/src/frc846/include/frc846/base/fserver.h @@ -95,6 +95,7 @@ class LoggingServer { msg_mtx.lock(); auto msg = messages.front(); + messages.pop(); msg_mtx.unlock(); cli_mtx.lock(); diff --git a/src/frc846/include/frc846/control/motion.h b/src/frc846/include/frc846/control/motion.h index 775dca9..e997d79 100644 --- a/src/frc846/include/frc846/control/motion.h +++ b/src/frc846/include/frc846/control/motion.h @@ -100,15 +100,23 @@ class BrakingPositionDyFPID { public: BrakingPositionDyFPID(frc846::base::Loggable& parent, std::function prop_ff_function, - CurrentControlSettings current_control_settings) + CurrentControlSettings current_control_settings, + frc846::control::HardLimitsConfigHelper* hard_limits) : prop_ff_function_{prop_ff_function}, - current_control_{parent, current_control_settings} {} + current_control_{parent, current_control_settings}, + hard_limits_{hard_limits} {} double calculate(X target_pos, X current_pos, double current_velocity_percentage, // Only using dyF, P, D frc846::control::Gains g) { - double error = - target_pos.template to() - current_pos.template to(); + auto hard_limits_vals = hard_limits_->get(); + + double target_pos_capped = + std::max(std::min(hard_limits_vals.forward.template to(), + target_pos.template to()), + hard_limits_vals.reverse.template to()); + + double error = target_pos_capped - current_pos.template to(); double target_output = g.kF * prop_ff_function_(current_pos) + g.kP * error + g.kD * current_velocity_percentage; @@ -121,6 +129,8 @@ class BrakingPositionDyFPID { std::function prop_ff_function_; CurrentControl current_control_; + + frc846::control::HardLimitsConfigHelper* hard_limits_; }; struct GenericMotionWaypoint { diff --git a/src/frc846/include/frc846/math/calculator.h b/src/frc846/include/frc846/math/calculator.h new file mode 100644 index 0000000..9a76521 --- /dev/null +++ b/src/frc846/include/frc846/math/calculator.h @@ -0,0 +1,38 @@ +#pragma once + +namespace frc846::math { + +template +class Calculator { + public: + void setConstants(C constants) { constants_ = constants; } + + virtual O calculate(I input) = 0; + + protected: + C constants_; +}; + +template +class IterativeCalculator : public Calculator { + protected: + virtual O calculateIteration(I input, O prev_output) = 0; + + public: + O calculate(I input) override final { + O output = O{}; + for (int i = 0; i < max_iterations; i++) { + output = calculateIteration(input, output); + } + return output; + } + + void setMaxIterations(int max_iterations) { + this->max_iterations = max_iterations; + } + + protected: + int max_iterations = 7; +}; + +}; // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/math/collection.h b/src/frc846/include/frc846/math/collection.h new file mode 100644 index 0000000..f99b717 --- /dev/null +++ b/src/frc846/include/frc846/math/collection.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include + +namespace frc846::math { + +// Double comparison using an epsilon value. Default epsilon value is 1e-9. +bool DEquals(double x, double y, double epsilon = 1e-9); + +// Find the circumference of a circle given radius. +constexpr units::inch_t Circumference(units::meter_t radius) { + return 2 * units::constants::pi * radius; +} + +double HorizontalDeadband(double input, double x_intercept, double max, + double exponent = 1, double sensitivity = 1); + +double VerticalDeadband(double input, double y_intercept, double max, + double exponent = 1, double sensitivity = 1); + +// Returns the smallest difference between two angles +units::degree_t CoterminalDifference(units::degree_t angle, + units::degree_t other_angle); + +// Returns the smallest sum between two angles. +units::degree_t CoterminalSum(units::degree_t angle, + units::degree_t other_angle); + +} // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/math/constants.h b/src/frc846/include/frc846/math/constants.h new file mode 100644 index 0000000..da72d7f --- /dev/null +++ b/src/frc846/include/frc846/math/constants.h @@ -0,0 +1,16 @@ +#pragma once + +#include + +namespace frc846::math { + +struct constants { + struct physics { + static constexpr units::feet_per_second_squared_t g{32.17405}; + }; + struct geometry { + static constexpr double pi = 3.14159265; + }; +}; + +} // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/math/fieldpoints.h b/src/frc846/include/frc846/math/fieldpoints.h new file mode 100644 index 0000000..b8efe08 --- /dev/null +++ b/src/frc846/include/frc846/math/fieldpoints.h @@ -0,0 +1,71 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "frc846/math/vectors.h" +#include "frc846/ntinf/pref.h" + +namespace frc846::math { + +struct FieldPoint { + VectorND point; + units::degree_t bearing; + + VectorND velocity; + + // Returns a FieldPoint that is 'mirrored' across the field + FieldPoint mirror(bool shouldMirror = true) const { + if (shouldMirror) { + return FieldPoint{{field_size_x - point[0], field_size_y - point[1]}, + 180_deg + bearing, + {-velocity[0], -velocity[1]}}; + } + return FieldPoint{ + {point[0], point[1]}, bearing, {velocity[0], velocity[1]}}; + } + + // Returns a FieldPoint that is 'mirrored' across the centerline of the field + FieldPoint mirrorOnlyY(bool shouldMirror = true) const { + if (shouldMirror) { + return FieldPoint{{point[0], field_size_y - point[1]}, + 180_deg - bearing, + {velocity[0], -velocity[1]}}; + } + return mirror(false); + } + + private: + static constexpr units::inch_t field_size_x = 651.25_in; + static constexpr units::inch_t field_size_y = 315.5_in; +}; + +class FieldPointPreference { + public: + FieldPointPreference(std::string name, FieldPoint backup) + : x{table_name_, name + "_x", backup.point[0]}, + y{table_name_, name + "_y", backup.point[1]}, + bearing{table_name_, name + "_deg", backup.bearing}, + v_x{table_name_, name + "_v_x", backup.velocity[0]}, + v_y{table_name_, name + "_v_y", backup.velocity[1]} {} + + FieldPoint get() { + return { + {x.value(), y.value()}, bearing.value(), {v_x.value(), v_y.value()}}; + } + + private: + frc846::ntinf::Pref x; + frc846::ntinf::Pref y; + frc846::ntinf::Pref bearing; + frc846::ntinf::Pref v_x; + frc846::ntinf::Pref v_y; + + const std::string table_name_ = "Preferences/field_points"; +}; + +} // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h index eb75357..77669e9 100644 --- a/src/frc846/include/frc846/math/vectors.h +++ b/src/frc846/include/frc846/math/vectors.h @@ -1,37 +1,245 @@ -// #pragma once - -// #include -// #include -// #include -// #include - -// #include - -// namespace frc846::math { - -// template -// class VectorND { -// static_assert(N > 0, -// "VectorND can not be created with a dimension less than 1"); -// static_assert(units::traits::is_unit_t(), -// "VectorND can only be created with unit types"); - -// public: -// // Constructs an N-dimensional vector given displacement of vector in each -// // dimension -// VectorND(const T (&arr)[N]) : dims{} { -// for (int i = 0; i < N; i++) { -// dims[i] = arr[i]; -// } -// } - -// // Constructs an 2-dimensional vector from polar arguments (r, theta) -// // template > -// // VectorND(units::degree_t theta, T r) {} - -// private: -// T dims[N]; -// }; - -// }; // namespace frc846::math \ No newline at end of file +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "frc846/math/collection.h" + +namespace frc846::math { +template +class VectorND { + static_assert(N > 0, + "VectorND can not be created with less than one dimension."); + static_assert(units::traits::is_unit_t(), + "VectorND can only be created with unit types."); + + private: + std::vector data; + + public: + // Default constructor initializes with zeros + VectorND() : data{N, T()} {} + + // Constructs an N-dimensional vector from N unit-type values + VectorND(std::initializer_list dims) : VectorND() { + assert(dims.size() == N && + "VectorND must be constructed with N dimensions."); + std::copy(dims.begin(), dims.end(), data.begin()); + } + + // Constructs a 2D vector from magnitude and angle + // If angleIsBearing is true, 0_deg is +y and angles are measured clockwise + // Otherwise, 0_deg is +x and angles are measured counter-clockwise + VectorND(T magnitude, units::degree_t theta, bool angleIsBearing = false) + : VectorND() { + assert(N == 2 && "Polar constructor can only be used with 2D vectors."); + if (angleIsBearing) { + theta = 90_deg - theta; + } + data[0] = magnitude * units::math::cos(theta); + data[1] = magnitude * units::math::sin(theta); + } + + // Constructs 2D vector from a pair + VectorND(const std::pair vec) : VectorND() { + assert(N == 2 && "Pair constructor can only be used with 2D vectors."); + data = {vec.first, vec.second}; + } + + // Copy constructor for VectorND + VectorND(const VectorND& other) : VectorND() { + for (size_t i = 0; i < N; ++i) { + data[i] = other[i]; + } + } + + // Adds VectorND to this and returns result + VectorND operator+(const VectorND& other) const { + VectorND result; + for (size_t i = 0; i < N; ++i) { + result[i] = data[i] + other[i]; + } + return result; + } + + // Subtracts VectorND from this and returns result + VectorND operator-(const VectorND& other) const { + VectorND result; + for (size_t i = 0; i < N; ++i) { + result[i] = data[i] - other[i]; + } + return result; + } + + VectorND operator*(const double scalar) const { + VectorND result; + for (size_t i = 0; i < N; ++i) { + result[i] = data[i] * scalar; + } + return result; + } + + friend double operator*(double lhs, const VectorND& rhs) { + return rhs * lhs; + } + + VectorND operator/(const double scalar) const { + VectorND result; + for (size_t i = 0; i < N; ++i) { + result[i] = data[i] / scalar; + } + return result; + } + + VectorND& operator+=(const VectorND& other) { + for (size_t i = 0; i < N; ++i) { + data[i] += other[i]; + } + return *this; + } + + VectorND& operator-=(const VectorND& other) { + for (size_t i = 0; i < N; ++i) { + data[i] -= other[i]; + } + return *this; + } + + VectorND& operator*=(const double scalar) { + for (size_t i = 0; i < N; ++i) { + data[i] *= scalar; + } + return *this; + } + + VectorND& operator/=(const double scalar) { + for (size_t i = 0; i < N; ++i) { + data[i] /= scalar; + } + return *this; + } + + void operator=(const VectorND& other) { + for (size_t i = 0; i < N; ++i) { + data[i] = other[i]; + } + } + + // Uses 'safe' double comparison + bool operator==(const VectorND& other) const { + for (size_t i = 0; i < N; ++i) { + if (!frc846::math::DEquals(data[i], other[i])) { + return false; + } + } + return true; + } + + // Returns a vector rotated by a given angle. Default is clockwise rotation. + VectorND rotate(units::degree_t angle, bool clockwise = true) const { + static_assert(N == 2, "Rotation is only defined for 2D vectors."); + if (clockwise) { + angle = -angle; + } + return { + data[0] * units::math::cos(angle) - data[1] * units::math::sin(angle), + data[0] * units::math::sin(angle) + data[1] * units::math::cos(angle)}; + } + + // Returns the dot product of this vector and another + T dot(const VectorND& other) const { + T result = T{}; + for (size_t i = 0; i < N; ++i) { + result += data[i] * other[i]; + } + return result; + } + + // Returns the cross product of this vector and another + // Cross product is only defined for 3D vectors + VectorND cross(const VectorND& other) const { + static_assert(N == 3, "Cross product is only defined for 3D vectors."); + return {data[1] * other[2] - data[2] * other[1], + data[2] * other[0] - data[0] * other[2], + data[0] * other[1] - data[1] * other[0]}; + } + + // Returns the magnitude of this vector + T magnitude() const { + auto result = data[0] * data[0]; + for (size_t i = 1; i < N; ++i) { + result += data[i] * data[i]; + } + return units::math::sqrt(result); + } + + // Returns the unit vector of this vector + VectorND unit() const { + return *this / magnitude().template to(); + } + + // Projects this vector onto another and returns + VectorND projectOntoAnother(const VectorND& other) const { + return other.unit() * dot(other.unit()).template to(); + } + + // Projects another vector onto this and returns + VectorND projectOntoThis(const VectorND& other) const { + return unit() * other.dot(*this); + } + + // Returns the angle of this vector + // If angleIsBearing is true, 0_deg is +y and angles are measured clockwise + // Otherwise, 0_deg is +x and angles are measured counter-clockwise + units::degree_t angle(bool angleIsBearing = false) const { + assert(N == 2 && "Angle can only be calculated for 2D vectors."); + if (angleIsBearing) { + return units::math::atan2(data[0], data[1]); + } + return units::math::atan2(data[1], data[0]); + } + + // Returns the angle between this vector and another + units::degree_t angleTo(const VectorND& other, + bool angleIsBearing = false) const { + return other.angle(angleIsBearing) - angle(angleIsBearing); + } + + // Const and non-const accessors for vector elements + const T& operator[](size_t i) const { return data[i]; } + T& operator[](size_t i) { return data[i]; } + + // Returns a pair representation of a 2D vector + std::pair toPair() { + static_assert(N == 2 && "toPair can only be used with 2D vectors."); + return {data[0], data[1]}; + } + + // Returns string representation of this vector + std::string toString() const { + std::string output = "<"; + for (size_t i = 0; i < N; ++i) { + output += data[i]; + if (i < N - 1) output += ", "; + } + output += ">"; + return output; + } +}; + +// Commonly used vector types + +// 1D vector, units::inch_t +using Vector1D = VectorND; +// 2D vector, units::inch_t +using Vector2D = VectorND; +// 3D vector, units::inch_t +using Vector3D = VectorND; + +}; // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/other/sendable_callback.h b/src/frc846/include/frc846/ntinf/ntaction.h similarity index 55% rename from src/frc846/include/frc846/other/sendable_callback.h rename to src/frc846/include/frc846/ntinf/ntaction.h index 82c41fc..e64ac90 100644 --- a/src/frc846/include/frc846/other/sendable_callback.h +++ b/src/frc846/include/frc846/ntinf/ntaction.h @@ -4,15 +4,13 @@ #include -namespace frc846::other { +namespace frc846::ntinf { -// Like an instant command but works when robot is disabled. -// // Use with SmartDashboard::PutData to have buttons call functions on // Shuffleboard. -class SendableCallback : public wpi::Sendable { +class NTAction : public wpi::Sendable { public: - SendableCallback(std::function callback); + NTAction(std::function callback); void InitSendable(wpi::SendableBuilder& builder); @@ -20,4 +18,4 @@ class SendableCallback : public wpi::Sendable { std::function callback_; }; -} // namespace frc846::other +} // namespace frc846::ntinf diff --git a/src/frc846/include/frc846/other/swerve_odometry.h b/src/frc846/include/frc846/other/swerve_odometry.h index dfa7e38..52f3e9e 100644 --- a/src/frc846/include/frc846/other/swerve_odometry.h +++ b/src/frc846/include/frc846/other/swerve_odometry.h @@ -2,7 +2,8 @@ #include -#include "frc846/util/math.h" +#include "frc846/math/collection.h" +#include "frc846/math/vectors.h" #include "units/base.h" namespace frc846 { @@ -12,20 +13,20 @@ class SwerveOdometry { static constexpr int kModuleCount = 4; public: - SwerveOdometry(util::Position initial_pose = {{0_ft, 0_ft}, 0_deg}); + SwerveOdometry(frc846::math::VectorND initial_position); - util::Position pose() const { return pose_; } + frc846::math::VectorND& position() { return position_; } - void Update( - std::array, kModuleCount> wheel_vecs, - units::radian_t omega); + void Update(std::array, kModuleCount> + wheel_vecs, + units::radian_t omega); - void SetPoint(util::Vector2D point); + void SetPoint(frc846::math::VectorND point); void Zero(); private: - util::Position pose_; + frc846::math::VectorND position_; std::array prev_wheel_distances_ = {}; }; diff --git a/src/frc846/include/frc846/other/trajectory_generator.h b/src/frc846/include/frc846/other/trajectory_generator.h index da54f6b..4e2447d 100644 --- a/src/frc846/include/frc846/other/trajectory_generator.h +++ b/src/frc846/include/frc846/other/trajectory_generator.h @@ -3,30 +3,23 @@ #include #include -#include "frc846/util/math.h" +#include "frc846/math/fieldpoints.h" namespace frc846 { -struct InputWaypoint { - frc846::util::Position pos; - std::optional v_max; -}; - struct Waypoint { - frc846::util::Position pos; - units::feet_per_second_t v; + frc846::math::FieldPoint pos; }; using Trajectory = std::vector; -std::vector InterpolatePoints( - util::Vector2D start, util::Vector2D end, - units::degree_t start_bearing, units::degree_t end_bearing, - units::foot_t cut); +std::vector InterpolatePoints( + frc846::math::VectorND start, + frc846::math::VectorND end, units::degree_t start_bearing, + units::degree_t end_bearing, units::foot_t cut); Trajectory GenerateTrajectory( - std::vector input_points, - units::feet_per_second_t robot_max_v, + std::vector input_points, units::feet_per_second_t robot_max_v, units::feet_per_second_squared_t robot_max_acceleration, units::feet_per_second_squared_t robot_max_deceleration, units::inch_t cut = 2_in); diff --git a/src/frc846/include/frc846/robot/GenericCommand.h b/src/frc846/include/frc846/robot/GenericCommand.h index 34d61fe..d55409a 100644 --- a/src/frc846/include/frc846/robot/GenericCommand.h +++ b/src/frc846/include/frc846/robot/GenericCommand.h @@ -5,7 +5,7 @@ #include #include -#include "frc846/util/math.h" +#include "frc846/base/Loggable.h" #include "frc846/wpilib/time.h" namespace frc846::robot { diff --git a/src/frc846/include/frc846/robot/RobotState.h b/src/frc846/include/frc846/robot/RobotState.h new file mode 100644 index 0000000..25f5ef4 --- /dev/null +++ b/src/frc846/include/frc846/robot/RobotState.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "frc846/base/Loggable.h" +#include "frc846/ntinf/grapher.h" + +namespace frc846::robot { + +class RSTable : frc846::base::Loggable { + private: + std::unordered_map table_; + + public: + RSTable(std::string table_name); + virtual ~RSTable() = default; + + template + void Set(std::string key, T value) { + table_[key] = value; + + if (units::traits::is_unit_t()) { + frc::SmartDashboard::PutData(name(), value.template to()); + } else if (std::is_same_v) { + frc::SmartDashboard::PutBoolean(name(), value); + } else if (std::is_same_v) { + frc::SmartDashboard::PutString(name(), value); + } else if (std::is_same_v) { + frc::SmartDashboard::PutNumber(name(), value); + } else if (std::is_same_v) { + frc::SmartDashboard::PutNumber(name(), value); + } else { + Warn("Unable to graph RSTable entry of type {} with key {}.", + typeid(T).name(), key); + } + } + + template + T Get(std::string key) { + try { + return std::any_cast(table_[key]); + } catch (const std::bad_any_cast& exc) { + Error("Incorrect type specified when accessing RSTable key {}.", key); + } catch (const std::out_of_range& exc) { + Warn("Key {} not found in RSTable.", key); + } + return T{}; + } + + std::vector ListKeys(); +}; + +class RobotState { + static std::unordered_map tables_; + + public: + static RSTable* getTable(std::string table_name); +}; + +}; // namespace frc846::robot \ No newline at end of file diff --git a/src/frc846/include/frc846/util/math.h b/src/frc846/include/frc846/util/math.h deleted file mode 100644 index c83d6dd..0000000 --- a/src/frc846/include/frc846/util/math.h +++ /dev/null @@ -1,187 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -#include "frc846/ntinf/pref.h" - -namespace frc846::util { - -// Find the circumference of a circle given radius. -constexpr units::inch_t Circumference(units::meter_t radius) { - return 2 * units::constants::pi * radius; -} - -double HorizontalDeadband(double input, double x_intercept, double max, - double exponent = 1, double sensitivity = 1); - -double VerticalDeadband(double input, double y_intercept, double max, - double exponent = 1, double sensitivity = 1); - -// Returns the smallest difference between an element of C(θ₁) and an element of -// C(θ₂). -units::degree_t CoterminalDifference(units::degree_t angle, - units::degree_t other_angle); - -// Returns the smallest sum between two angles. -units::degree_t CoterminalSum(units::degree_t angle, - units::degree_t other_angle); - -// A 2D vector. -// -// TODO only calculate magnitude and bearing once -template -struct Vector2D { - static_assert(units::traits::is_unit_t(), "must be a unit"); - - T x; - T y; - - // Magnitude of the vector. - T Magnitude() const { return units::math::sqrt(x * x + y * y); } - - // Bearing of the vector. - // - // x and y are intentionally swapped in atan2. - units::radian_t Bearing() const { return units::math::atan2(x, y); } - - // Returns a new vector rotate CLOCKWISE by theta. - Vector2D Rotate(units::degree_t theta) const { - return { - x * units::math::cos(theta) + y * units::math::sin(theta), - x * -units::math::sin(theta) + y * units::math::cos(theta), - }; - } - - Vector2D Extrapolate(Vector2D from, units::inch_t by) const { - auto bearing = units::math::atan2(x - from.x, y - from.y); - return {x + by * units::math::sin(bearing), - y + by * units::math::cos(bearing)}; - } - - Vector2D operator+(const Vector2D& other) const { - return {x + other.x, y + other.y}; - } - - Vector2D operator-(const Vector2D& other) const { - return {x - other.x, y - other.y}; - } - - Vector2D operator*(double scalar) const { - return {x * scalar, y * scalar}; - } - - Vector2D operator/(double scalar) const { - return {x / scalar, y / scalar}; - } - - bool operator==(const Vector2D& other) const { - return x == other.x && y == other.y; - } - - // VectorPolar toVectorPolar() { return {Magnitude(), Bearing()}; } -}; - -// template -// struct VectorPolar { -// static_assert(units::traits::is_unit_t(), "must be a unit"); - -// T magnitude; // Length of vector -// units::degree_t theta; // Angle from vertical - -// Vector2D toVector2D() { -// return { -// magnitude *units::math::sin(theta), magnitude *units::math::cos(theta); -// } -// } - -// VectorPolar operator+(const VectorPolar& other) const { -// return (toVector2D() + other.toVector2D()).toVectorPolar(); -// } -// }; - -struct Position { - Vector2D point; - units::degree_t bearing; -}; - -class FieldPoint { - public: - static Position to_position(FieldPoint point) { - return Position{{point.x.value(), point.y.value()}, point.bearing.value()}; - } - - static Position flipOnlyY(FieldPoint point, bool should_flip) { - if (should_flip) { - return {{point.x.value(), field_size_y - point.y.value()}, - 180_deg - point.bearing.value()}; - } - return {{point.x.value(), point.y.value()}, point.bearing.value()}; - } - - static Position flipOnlyY(Position point, bool should_flip) { - if (should_flip) { - return {{point.point.x, field_size_y - point.point.y}, - 180_deg - point.bearing}; - } - return point; - } - - static Position flip(FieldPoint point, bool should_flip, bool onlyY = false) { - if (onlyY) return flipOnlyY(point, should_flip); - - if (should_flip) { - return Position{{-point.x.value(), field_size_y - point.y.value()}, - 180_deg + point.bearing.value()}; - } else { - return Position{{point.x.value(), point.y.value()}, - point.bearing.value()}; - } - } - - static Position flip(Position point, bool should_flip, bool onlyY = false) { - if (onlyY) return flipOnlyY(point, should_flip); - if (should_flip) { - return Position{ - {field_size_x - point.point.x, field_size_y - point.point.y}, - 180_deg + point.bearing}; - } else { - return Position{{point.point.x, point.point.y}, point.bearing}; - } - } - - Position flip(bool should_flip, bool onlyY = false) { - return flip(*this, should_flip, onlyY); - } - - frc846::base::Loggable& point_names_ = - *(new frc846::base::Loggable("Preferences/field_points")); - - FieldPoint(std::string name) - : x{point_names_, name + "_x", 0.0_in}, - y{point_names_, name + "_y", 0.0_in}, - bearing{point_names_, name + "_deg", 0.0_deg}, - name_(name) {} - - FieldPoint(std::string name, units::inch_t x, units::inch_t y, - units::degree_t deg) - : x{point_names_, name + "_x", x}, - y{point_names_, name + "_y", y}, - bearing{point_names_, name + "_deg", deg}, - name_(name) {} - - frc846::ntinf::Pref x; - frc846::ntinf::Pref y; - frc846::ntinf::Pref bearing; - std::string name_; - - private: - static constexpr units::inch_t field_size_x = 651.25_in; - static constexpr units::inch_t field_size_y = 315.5_in; -}; - -} // namespace frc846::util diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index 5b6b4b4..19000a0 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -22,62 +22,43 @@ #include "commands/teleop/operator_control.h" #include "commands/teleop/stow_command.h" #include "control_triggers.h" -#include "frc846/other/sendable_callback.h" +#include "frc846/ntinf/ntaction.h" FunkyRobot::FunkyRobot() : GenericRobot{&container_} {} void FunkyRobot::OnInitialize() { // Add dashboard buttons frc::SmartDashboard::PutData( - "zero_modules", new frc846::other::SendableCallback( + "zero_modules", new frc846::ntinf::NTAction( [this] { container_.drivetrain_.ZeroModules(); })); frc::SmartDashboard::PutData("zero_bearing", - new frc846::other::SendableCallback([this] { + new frc846::ntinf::NTAction([this] { container_.drivetrain_.SetBearing(0_deg); })); frc::SmartDashboard::PutData( - "zero_odometry", new frc846::other::SendableCallback( + "zero_odometry", new frc846::ntinf::NTAction( [this] { container_.drivetrain_.ZeroOdometry(); })); frc::SmartDashboard::PutData("zero_subsystems", - new frc846::other::SendableCallback([this] { + new frc846::ntinf::NTAction([this] { container_.super_structure_.ZeroSubsystem(); })); frc::SmartDashboard::PutData("coast_subsystems", - new frc846::other::SendableCallback([this] { + new frc846::ntinf::NTAction([this] { container_.pivot_.Coast(); container_.telescope_.Coast(); container_.wrist_.Coast(); })); frc::SmartDashboard::PutData("brake_subsystems", - new frc846::other::SendableCallback([this] { + new frc846::ntinf::NTAction([this] { container_.pivot_.Brake(); container_.telescope_.Brake(); container_.wrist_.Brake(); })); - AddAutos(five_piece_auto_red.get(), - {five_piece_auto_blue.get(), one_piece_auto_0.get(), - one_piece_auto_1.get(), one_piece_auto_2.get(), - one_piece_auto_3.get(), drive_auto_.get()}); -} - -void FunkyRobot::InitTeleop() { - container_.pivot_.Brake(); - container_.telescope_.Brake(); - container_.wrist_.Brake(); - - container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); - container_.control_input_.SetDefaultCommand( - OperatorControlCommand{container_}); - container_.super_structure_.SetDefaultCommand(StowCommand{container_}); - container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_}); - container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); - container_.bracer_.SetDefaultCommand(BracerCommand{container_}); - frc2::Trigger on_coast_trigger{[&] { return coasting_switch_.Get(); }}; on_coast_trigger.OnTrue(frc2::InstantCommand([&] { @@ -99,6 +80,25 @@ void FunkyRobot::InitTeleop() { container_.telescope_.ZeroSubsystem(); }).ToPtr()); + AddAutos(five_piece_auto_red.get(), + {five_piece_auto_blue.get(), one_piece_auto_0.get(), + one_piece_auto_1.get(), one_piece_auto_2.get(), + one_piece_auto_3.get(), drive_auto_.get()}); +} + +void FunkyRobot::InitTeleop() { + container_.pivot_.Brake(); + container_.telescope_.Brake(); + container_.wrist_.Brake(); + + container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); + container_.control_input_.SetDefaultCommand( + OperatorControlCommand{container_}); + container_.super_structure_.SetDefaultCommand(StowCommand{container_}); + container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_}); + container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); + container_.bracer_.SetDefaultCommand(BracerCommand{container_}); + ControlTriggerInitializer::InitTeleopTriggers(container_); } diff --git a/src/y2024/cpp/autos/drive_auto.cc b/src/y2024/cpp/autos/drive_auto.cc index afa3410..5c24688 100644 --- a/src/y2024/cpp/autos/drive_auto.cc +++ b/src/y2024/cpp/autos/drive_auto.cc @@ -9,7 +9,6 @@ #include "field.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" -#include "frc846/util/math.h" DriveAuto::DriveAuto(RobotContainer& container) : frc846::robot::GenericCommandGroup()); - }}, - PrepareAutoShootCommand{container}, AutoShootCommand{container}, - frc2::WaitCommand{ - container.super_structure_.post_shoot_wait_.value()} + // frc2::InstantCommand{[&, s = start_angle] { + // container.drivetrain_.SetBearing(s); + // Log("OP: Zeroing to {}", s.to()); + // }}, + // PrepareAutoShootCommand{container}, + // AutoShootCommand{container}, + // frc2::WaitCommand{ + // container.super_structure_.post_shoot_wait_.value()} }} {} diff --git a/src/y2024/cpp/calculators/arm_kinematics_calculator.cc b/src/y2024/cpp/calculators/arm_kinematics_calculator.cc new file mode 100644 index 0000000..ed0d4b1 --- /dev/null +++ b/src/y2024/cpp/calculators/arm_kinematics_calculator.cc @@ -0,0 +1,59 @@ +#include "calculators/arm_kinematics_calculator.h" + +#include + +#include "frc846/math/constants.h" +#include "frc846/math/vectors.h" + +ArmKinematicsCalculator::ArmKinematicsCalculator() { + AKCalculatorConstants constants{}; + + constants.pivot_offset = {-9.25_in, 16.25_in}; + constants.pivot_to_wrist_joint = {20.5_in, -4.25_in}; + constants.wrist_joint_to_shooter = {0.0_in, 10_in}; + constants.shooter_to_intake = {12.0_in, 0.0_in}; + + constants.max_extension_from_center = + 14_in + 1_ft; // Half robot length + 1ft + + constants.max_height = 4_ft; + + constants_ = constants; + + setConstants(constants); +} + +AKCalculatorOutput ArmKinematicsCalculator::calculate(AKCalculatorInput input) { + frc846::math::Vector2D arm_vector_reference = + constants_.pivot_to_wrist_joint + + frc846::math::Vector2D{input.telescope_extension, 0_in}; + auto arm_vector = arm_vector_reference.rotate(input.pivot_angle, false); + + auto joint_position = constants_.pivot_offset + arm_vector; + + auto shooter_position = + joint_position + constants_.wrist_joint_to_shooter.rotate( + input.pivot_angle - input.wrist_angle, false); + + auto intake_position = + shooter_position + constants_.shooter_to_intake.rotate( + input.pivot_angle + input.wrist_angle, false); + + return {shooter_position, intake_position}; +} + +bool ArmKinematicsCalculator::WithinBounds( + AKCalculatorOutput end_effector_positions) { + return end_effector_positions.shooter_position[0] < + constants_.max_extension_from_center && + end_effector_positions.shooter_position[0] > + -constants_.max_extension_from_center && + end_effector_positions.shooter_position[1] < constants_.max_height && + end_effector_positions.shooter_position[1] > 0_in && + end_effector_positions.intake_position[0] < + constants_.max_extension_from_center && + end_effector_positions.intake_position[0] > + -constants_.max_extension_from_center && + end_effector_positions.intake_position[1] < constants_.max_height && + end_effector_positions.intake_position[1] > 0_in; +} \ No newline at end of file diff --git a/src/y2024/cpp/calculators/shooting_calculator.cc b/src/y2024/cpp/calculators/shooting_calculator.cc new file mode 100644 index 0000000..dbde205 --- /dev/null +++ b/src/y2024/cpp/calculators/shooting_calculator.cc @@ -0,0 +1,44 @@ +#include "calculators/shooting_calculator.h" + +#include "frc846/math/constants.h" +#include "frc846/math/vectors.h" + +ShootingCalculatorOutput ShootingCalculator::calculateIteration( + ShootingCalculatorInput input, ShootingCalculatorOutput prev_output) { + if (input.shot_distance <= 4.0_ft) { + return {input.default_setpoint, 0.0_deg}; + } + + /* + * rv_o = the portion of the robot's velocity is perpendicular to the line + * between the robot and the speaker. + * + * Finding the amount that the robot has to 'twist' to cancel out rv_o + */ + units::degree_t twist_angle = + units::math::asin(input.rv_o / input.shot_speed); + + /* + * Finding the time it takes for the shot to reach the speaker + * + * t = time for shot to reach speaker + * shot_distance = distance to speaker along ground + * + * t = shot_distance / ground_speed + * ground_speed = robot_velocity + shot_speed_along_ground + */ + auto t = input.shot_distance / + (input.rv_i + input.shot_speed * units::math::cos(twist_angle) * + units::math::cos(prev_output.launch_angle)); + + auto height_difference = constants_.speaker_height - input.shooter_height; + + /* + * Finding the angle the shot has to be launched at to reach the speaker + */ + auto launch_angle = units::math::asin( + (height_difference + 0.5 * frc846::math::constants::physics::g * t * t) / + (input.shot_speed * t)); + + return {launch_angle, twist_angle}; +} \ No newline at end of file diff --git a/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc b/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc index e854036..26d2397 100644 --- a/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc +++ b/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc @@ -13,8 +13,8 @@ #include "frc2/command/WaitCommand.h" AutoIntakeAndShootCommand::AutoIntakeAndShootCommand( - RobotContainer& container, frc846::InputWaypoint intake_point, - frc846::InputWaypoint mid_point, frc846::InputWaypoint shoot_point) + RobotContainer& container, frc846::Waypoint intake_point, + frc846::Waypoint mid_point, frc846::Waypoint shoot_point) : frc846::robot::GenericCommandGroup{ diff --git a/src/y2024/cpp/commands/basic/prepare_auto_shoot_command.cc b/src/y2024/cpp/commands/basic/prepare_auto_shoot_command.cc index 16ecb3d..33bb54c 100644 --- a/src/y2024/cpp/commands/basic/prepare_auto_shoot_command.cc +++ b/src/y2024/cpp/commands/basic/prepare_auto_shoot_command.cc @@ -14,16 +14,6 @@ void PrepareAutoShootCommand::Periodic() { auto defaultShootSetpoint{container_.super_structure_.getAutoShootSetpoint()}; - // auto theta = - // shooting_calculator_ - // .calculateLaunchAngles( - // shooter_.shooting_exit_velocity_.value(), - // vis_readings_.est_dist_from_speaker.to() + - // super_.auto_shooter_x_.value().to() / 12.0, - // 0.0, 0.0, super_.auto_shooter_height_.value().to() - // / 12.0, defaultShootSetpoint.wrist.to()) - // .launch_angle; - container_.intake_.SetTarget({IntakeState::kHold}); container_.shooter_.SetTarget({ShooterState::kRun}); @@ -33,8 +23,6 @@ void PrepareAutoShootCommand::Periodic() { defaultShootSetpoint.wrist += defaultShootSetpoint.pivot; - // Log("Theta {}", theta); - container_.super_structure_.SetTargetSetpoint(defaultShootSetpoint); } diff --git a/src/y2024/cpp/commands/basic/prepare_shoot_command.cc b/src/y2024/cpp/commands/basic/prepare_shoot_command.cc index d337b15..cc84b52 100644 --- a/src/y2024/cpp/commands/basic/prepare_shoot_command.cc +++ b/src/y2024/cpp/commands/basic/prepare_shoot_command.cc @@ -7,6 +7,9 @@ PrepareShootCommand::PrepareShootCommand(RobotContainer& container, "prepare_shoot_command"}, super_shot_(super_shot) { AddRequirements({&container_.super_structure_}); + + ShootingCalculatorConstants sc_constants{78.0_in}; + shooting_calculator_.setConstants(sc_constants); } void PrepareShootCommand::OnInit() {} @@ -20,21 +23,16 @@ void PrepareShootCommand::Periodic() { // container_.shooter_.SetTarget({ShooterState::kRun}); if (super_shot_) { - auto theta = - shooting_calculator_ - .calculateLaunchAngles( - container_.shooter_.shooting_exit_velocity_.value(), - vis_readings_.est_dist_from_speaker.to() + - container_.super_structure_.teleop_shooter_x_.value() - .to() / - 12.0, - vis_readings_.velocity_in_component, - vis_readings_.velocity_orth_component, - container_.super_structure_.teleop_shooter_height_.value() - .to() / - 12.0, - shootSetpoint.wrist.to()) - .launch_angle; + ShootingCalculatorInput sc_input = { + vis_readings_.est_dist_from_speaker + + container_.super_structure_.teleop_shooter_x_.value(), + vis_readings_.velocity_in_component * 1_fps, + vis_readings_.velocity_orth_component * 1_fps, + container_.super_structure_.teleop_shooter_height_.value(), + shootSetpoint.wrist, + container_.shooter_.shooting_exit_velocity_.value() * 1_fps}; + + auto theta = shooting_calculator_.calculate(sc_input).launch_angle; shootSetpoint.wrist = theta + shootSetpoint.pivot; } else { diff --git a/src/y2024/cpp/commands/basic/shoot_command.cc b/src/y2024/cpp/commands/basic/shoot_command.cc index e5a865a..ad2e698 100644 --- a/src/y2024/cpp/commands/basic/shoot_command.cc +++ b/src/y2024/cpp/commands/basic/shoot_command.cc @@ -6,7 +6,7 @@ ShootCommand::ShootCommand(RobotContainer& container) AddRequirements({&container_.intake_, &container_.shooter_}); } -void ShootCommand::OnInit() {} +void ShootCommand::OnInit() { num_loops_ = 0; } void ShootCommand::Periodic() { container_.intake_.SetTarget({IntakeState::kFeed}); @@ -17,4 +17,12 @@ void ShootCommand::OnEnd(bool interrupted) { frc846::util::ShareTables::SetBoolean("ready_to_shoot", false); } -bool ShootCommand::IsFinished() { return false; } \ No newline at end of file +bool ShootCommand::IsFinished() { + if (!container_.intake_.GetHasPiece()) { + num_loops_ += 1; + } else { + num_loops_ = 0; + } + + return num_loops_ > 20; +} \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/close_drive_amp_command.cc b/src/y2024/cpp/commands/complex/close_drive_amp_command.cc index f2b81c7..c95bd0d 100644 --- a/src/y2024/cpp/commands/complex/close_drive_amp_command.cc +++ b/src/y2024/cpp/commands/complex/close_drive_amp_command.cc @@ -19,16 +19,16 @@ void CloseDriveAmpCommand::Periodic() { drivetrain_target.control = kClosedLoop; auto amp_drive_dist = - (field::points::kPreAmpNoFlip().point - amp_point.point).Magnitude(); + (field::points::kPreAmpNoFlip().point - amp_point.point).magnitude(); auto max_speed = container_.drivetrain_.close_drive_amp_max_speed_.value(); - drivetrain_target.v_x = - (amp_point.point.x - container_.drivetrain_.GetReadings().pose.point.x) / - amp_drive_dist * max_speed; - drivetrain_target.v_y = - (amp_point.point.y - container_.drivetrain_.GetReadings().pose.point.y) / - amp_drive_dist * max_speed; + drivetrain_target.v_x = (amp_point.point[0] - + container_.drivetrain_.GetReadings().pose.point[0]) / + amp_drive_dist * max_speed; + drivetrain_target.v_y = (amp_point.point[1] - + container_.drivetrain_.GetReadings().pose.point[1]) / + amp_drive_dist * max_speed; drivetrain_target.v_x = units::math::max( -max_speed, units::math::min(max_speed, drivetrain_target.v_x)); @@ -43,5 +43,5 @@ void CloseDriveAmpCommand::OnEnd(bool interrupted) {} bool CloseDriveAmpCommand::IsFinished() { return (field::points::kAmpNoFlip().point - container_.drivetrain_.GetReadings().pose.point) - .Magnitude() <= 2_in; + .magnitude() <= 2_in; } \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/super_amp_command.cc b/src/y2024/cpp/commands/complex/super_amp_command.cc index ac6187a..e651783 100644 --- a/src/y2024/cpp/commands/complex/super_amp_command.cc +++ b/src/y2024/cpp/commands/complex/super_amp_command.cc @@ -14,7 +14,6 @@ #include "field.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" -#include "frc846/util/math.h" SuperAmpCommand::SuperAmpCommand(RobotContainer& container, bool is_red_side) : frc846::robot::GenericCommandGroup input_points) + RobotContainer& container, std::vector input_points) : frc846::robot::GenericCommand< RobotContainer, FollowTrajectoryCommand>{container, "follow_trajectory_command"}, @@ -11,13 +11,13 @@ FollowTrajectoryCommand::FollowTrajectoryCommand( void FollowTrajectoryCommand::OnInit() { Log("Starting Trajectory"); - for (frc846::InputWaypoint i : input_points_) { - Log("points x{} y{} bearing {}", i.pos.point.x, i.pos.point.y, + for (frc846::Waypoint i : input_points_) { + Log("points x{} y{} bearing {}", i.pos.point[0], i.pos.point[1], i.pos.bearing); } Log("initial pose x{}, y{}, bearing {}", - container_.drivetrain_.GetReadings().pose.point.x, - container_.drivetrain_.GetReadings().pose.point.y, + container_.drivetrain_.GetReadings().pose.point[0], + container_.drivetrain_.GetReadings().pose.point[1], container_.drivetrain_.GetReadings().pose.bearing); target_idx_ = 1; is_done_ = false; @@ -25,8 +25,7 @@ void FollowTrajectoryCommand::OnInit() { start_time_ = frc846::wpilib::CurrentFPGATime(); auto points = input_points_; - points.insert(points.begin(), 1, - {container_.drivetrain_.GetReadings().pose, 0_fps}); + points.insert(points.begin(), 1, {container_.drivetrain_.GetReadings().pose}); trajectory_ = frc846::GenerateTrajectory( points, container_.drivetrain_.auto_max_speed_.value(), @@ -37,9 +36,12 @@ void FollowTrajectoryCommand::OnInit() { Error("trajectory size ({}) is less than 4 - ending!", trajectory_.size()); is_done_ = true; } else { - current_extrapolated_point_ = trajectory_[1].pos.point.Extrapolate( - trajectory_[0].pos.point, - container_.drivetrain_.extrapolation_distance_.value()); + double extrapolation_distance = + container_.drivetrain_.extrapolation_distance_.value() / 1_ft; + current_extrapolated_point_ = + trajectory_[1].pos.point - trajectory_[0].pos.point; + current_extrapolated_point_ = + current_extrapolated_point_.unit() * extrapolation_distance; } } @@ -61,19 +63,23 @@ void FollowTrajectoryCommand::Periodic() { return; } + double extrapolation_distance = + container_.drivetrain_.extrapolation_distance_.value() / 1_ft; + current_extrapolated_point_ = trajectory_[target_idx_].pos.point - + trajectory_[target_idx_ - 1].pos.point; current_extrapolated_point_ = - trajectory_[target_idx_].pos.point.Extrapolate( - trajectory_[target_idx_ - 1].pos.point, - container_.drivetrain_.extrapolation_distance_.value()); + current_extrapolated_point_.unit() * extrapolation_distance; } auto delta_pos = current_extrapolated_point_ - container_.drivetrain_.GetReadings().pose.point; - auto direction = units::math::atan2(delta_pos.y, delta_pos.x); + auto direction = units::math::atan2(delta_pos[1], delta_pos[0]); DrivetrainTarget target; - target.v_x = trajectory_[target_idx_].v * units::math::cos(direction); - target.v_y = trajectory_[target_idx_].v * units::math::sin(direction); + target.v_x = trajectory_[target_idx_].pos.velocity.magnitude() * + units::math::cos(direction); + target.v_y = trajectory_[target_idx_].pos.velocity.magnitude() * + units::math::sin(direction); target.translation_reference = DrivetrainTranslationReference::kField; target.rotation = DrivetrainRotationPosition(trajectory_[target_idx_].pos.bearing); @@ -92,8 +98,8 @@ bool FollowTrajectoryCommand::IsFinished() { return is_done_; } // https://math.stackexchange.com/questions/274712/calculate-on-which-side-of-a-straight-line-is-a-given-point-located bool FollowTrajectoryCommand::HasCrossedWaypoint( frc846::Waypoint current_waypoint, frc846::Waypoint prev_waypoint, - frc846::util::Vector2D pos, - frc846::util::Vector2D extrapolated_point) { + frc846::math::VectorND pos, + frc846::math::VectorND extrapolated_point) { // fmt::print( // "\ncurrent_waypoint x {}, current_waypoint y {}, prev_waypoint x {} y " // "{}, pos x{} y{}, extrap x{}, y{}\n", @@ -101,12 +107,12 @@ bool FollowTrajectoryCommand::HasCrossedWaypoint( // prev_waypoint.pos.point.x, prev_waypoint.pos.point.y, pos.x, pos.y, // extrapolated_point.x, extrapolated_point.y); - auto d = [](frc846::util::Vector2D target, - frc846::util::Vector2D p1, - frc846::util::Vector2D p2) { - double x = - ((target.x - p1.x) * (p2.y - p1.y) - (target.y - p1.y) * (p2.x - p1.x)) - .to(); + auto d = [](frc846::math::VectorND target, + frc846::math::VectorND p1, + frc846::math::VectorND p2) { + double x = ((target[0] - p1[0]) * (p2[1] - p1[1]) - + (target[1] - p1[1]) * (p2[0] - p1[0])) + .to(); if (x > 0) { return 1; } else if (x < 0) { @@ -115,20 +121,22 @@ bool FollowTrajectoryCommand::HasCrossedWaypoint( return 0; }; - auto delta_y = current_waypoint.pos.point.y - prev_waypoint.pos.point.y; - auto delta_x = current_waypoint.pos.point.x - prev_waypoint.pos.point.x; + auto delta_y = current_waypoint.pos.point[1] - prev_waypoint.pos.point[1]; + auto delta_x = current_waypoint.pos.point[0] - prev_waypoint.pos.point[0]; auto theta = units::math::atan(-delta_x / delta_y); double cos_theta = units::math::cos(theta); double sin_theta = units::math::sin(theta); - auto p1 = current_waypoint.pos.point - frc846::util::Vector2D{ - 1_ft * cos_theta, - 1_ft * sin_theta, - }; - auto p2 = current_waypoint.pos.point + frc846::util::Vector2D{ - 1_ft * cos_theta, - 1_ft * sin_theta, - }; + auto p1 = + current_waypoint.pos.point - frc846::math::VectorND{ + 1_ft * cos_theta, + 1_ft * sin_theta, + }; + auto p2 = + current_waypoint.pos.point + frc846::math::VectorND{ + 1_ft * cos_theta, + 1_ft * sin_theta, + }; return d(pos, p1, p2) == d(extrapolated_point, p1, p2); } \ No newline at end of file diff --git a/src/y2024/cpp/commands/teleop/drive_command.cc b/src/y2024/cpp/commands/teleop/drive_command.cc index 5459cf2..7077c3e 100644 --- a/src/y2024/cpp/commands/teleop/drive_command.cc +++ b/src/y2024/cpp/commands/teleop/drive_command.cc @@ -2,10 +2,9 @@ #include -#include "constants.h" #include "field.h" #include "frc846/base/loggable.h" -#include "frc846/util/math.h" +#include "frc846/math/collection.h" #include "frc846/util/share_tables.h" #include "subsystems/hardware/drivetrain.h" #include "subsystems/hardware/swerve_module.h" @@ -14,6 +13,9 @@ DriveCommand::DriveCommand(RobotContainer& container) : frc846::robot::GenericCommand{ container, "drive_command"} { AddRequirements({&container_.drivetrain_}); + + ShootingCalculatorConstants sc_constants{78.0_in}; + shooting_calculator.setConstants(sc_constants); } void DriveCommand::OnInit() { driver_adjust_ = 0.0; } @@ -36,11 +38,11 @@ void DriveCommand::Periodic() { // -----TRANSLATION CONTROL----- - double translate_x = frc846::util::HorizontalDeadband( + double translate_x = frc846::math::HorizontalDeadband( container_.control_input_.GetReadings().translate_x, container_.control_input_.driver_.translation_deadband_.value(), 1, container_.control_input_.driver_.translation_exponent_.value(), 1); - double translate_y = frc846::util::HorizontalDeadband( + double translate_y = frc846::math::HorizontalDeadband( container_.control_input_.GetReadings().translate_y, container_.control_input_.driver_.translation_deadband_.value(), 1, container_.control_input_.driver_.translation_exponent_.value(), 1); @@ -71,7 +73,7 @@ void DriveCommand::Periodic() { // -----STEER CONTROL----- - double steer_x = frc846::util::HorizontalDeadband( + double steer_x = frc846::math::HorizontalDeadband( container_.control_input_.GetReadings().rotation, container_.control_input_.driver_.steer_deadband_.value(), 1, container_.control_input_.driver_.steer_exponent_.value(), 1); @@ -88,22 +90,18 @@ void DriveCommand::Periodic() { if (prep_align_speaker) { VisionReadings vision_readings = container_.vision_.GetReadings(); - double shooting_dist = vision_readings.est_dist_from_speaker.to(); - shooting_dist = - shooting_dist + - container_.super_structure_.teleop_shooter_x_.value().to() / - 12.0; + ShootingCalculatorInput sc_input = { + vision_readings.est_dist_from_speaker + + container_.super_structure_.teleop_shooter_x_.value(), + vision_readings.velocity_in_component * 1_fps, + vision_readings.velocity_orth_component * 1_fps, + container_.super_structure_.teleop_shooter_height_.value(), + 0_deg, + container_.shooter_.shooting_exit_velocity_.value() * 1_fps}; units::degree_t theta_adjust = - shooting_calculator - .calculateLaunchAngles( - container_.shooter_.shooting_exit_velocity_.value(), - shooting_dist, vision_readings.velocity_in_component, - vision_readings.velocity_orth_component, - container_.super_structure_.teleop_shooter_height_.value() - .to()) - .turning_offset_angle; + shooting_calculator.calculate(sc_input).twist_angle; if (units::math::abs(vision_readings.est_dist_from_speaker_x) > 0.5_ft || units::math::abs(vision_readings.est_dist_from_speaker_y) > 0.5_ft) { diff --git a/src/y2024/cpp/constants.cc b/src/y2024/cpp/constants.cc deleted file mode 100644 index 1469805..0000000 --- a/src/y2024/cpp/constants.cc +++ /dev/null @@ -1,35 +0,0 @@ -#include "constants.h" - -CoordinatePositions ArmKinematics::calculateCoordinatePosition( - RawPositions pos_raw, bool intakePoint) { - Vector2D ground_to_pivot_vector{pivotToCenter, pivotToGround}; - - Vector2D pivot_first_segment_vector{0.0_in, - pivotToWristOffset}; - - Vector2D arm_length_vector{pivotToWrist + pos_raw.extension, - 0.0_in}; - - // Rotate is CW - auto pivot_first_segment_vector_rotated = - pivot_first_segment_vector.Rotate(-pos_raw.pivot_angle); - - auto arm_length_vector_rotated = - arm_length_vector.Rotate(-pos_raw.pivot_angle); - - Vector2D joint_to_endpoint_vector{ - intakePoint ? shooterToIntake : 0.0_in, wristToShooter}; - - auto joint_to_endpoint_vector_rotated = joint_to_endpoint_vector.Rotate( - -pos_raw.pivot_angle + - pos_raw.wrist_angle); // Wrist is measured off of pivot, in opposite - // direction - - units::degree_t shootingAngle = pos_raw.wrist_angle - pos_raw.pivot_angle; - - Vector2D endpoint = - ground_to_pivot_vector + pivot_first_segment_vector_rotated + - arm_length_vector_rotated + joint_to_endpoint_vector_rotated; - - return {shootingAngle, endpoint.x, endpoint.y}; -} \ No newline at end of file diff --git a/src/y2024/cpp/control_triggers.cc b/src/y2024/cpp/control_triggers.cc index 4cdd595..da24e6c 100644 --- a/src/y2024/cpp/control_triggers.cc +++ b/src/y2024/cpp/control_triggers.cc @@ -28,9 +28,8 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { : 180_deg); }).ToPtr()); - frc2::Trigger on_piece_trigger{[&] { - return frc846::util::ShareTables::GetBoolean("scorer_has_piece"); - }}; + frc2::Trigger on_piece_trigger{ + [&] { return container.intake_.GetHasPiece(); }}; on_piece_trigger.OnTrue( frc2::InstantCommand( @@ -41,6 +40,20 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { container.control_input_.SetTarget({false, false}); }).ToPtr())); + frc2::Trigger shooter_spinup_trigger{[&] { + return container.shooter_.GetReadings().error_percent <= + container.shooter_.shooter_speed_tolerance_.value(); + }}; + + shooter_spinup_trigger.OnTrue( + frc2::InstantCommand( + [&] { container.control_input_.SetTarget({true, false}); }) + .WithTimeout(1_s) + .AndThen(frc2::WaitCommand(1_s).ToPtr()) + .AndThen(frc2::InstantCommand([&] { + container.control_input_.SetTarget({false, false}); + }).ToPtr())); + frc2::Trigger amp_command_trigger{[&container] { return container.control_input_.GetReadings().running_amp; }}; @@ -112,7 +125,7 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { frc2::Trigger shoot_trigger{ [&container] { return container.control_input_.GetReadings().shooting; }}; - shoot_trigger.WhileTrue(ShootCommand{container}.ToPtr()); + shoot_trigger.OnTrue(ShootCommand{container}.ToPtr()); for (int i = 0; i <= 6; i++) { frc2::Trigger trap_stage_trigger{[&container, i] { @@ -130,10 +143,17 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { wrist_zero_trigger.OnTrue(WristZeroCommand{container}.ToPtr()); - frc2::Trigger wrist_trim_save_trigger{ - [&] { return container.control_input_.GetReadings().save_trim; }}; + frc2::Trigger wrist_trim_up_trigger{ + [&] { return container.control_input_.GetReadings().trim_up; }}; + + wrist_trim_up_trigger.OnTrue(frc2::InstantCommand([&] { + container.super_structure_.TrimUp(); + }).ToPtr()); + + frc2::Trigger wrist_trim_down_trigger{ + [&] { return container.control_input_.GetReadings().trim_down; }}; - wrist_trim_save_trigger.OnTrue(frc2::InstantCommand([&] { - container.super_structure_.SaveAdjustments(); + wrist_trim_down_trigger.OnTrue(frc2::InstantCommand([&] { + container.super_structure_.TrimDown(); }).ToPtr()); } \ No newline at end of file diff --git a/src/y2024/cpp/field.cc b/src/y2024/cpp/field.cc index 92fe0b2..0af1fe3 100644 --- a/src/y2024/cpp/field.cc +++ b/src/y2024/cpp/field.cc @@ -1,27 +1,31 @@ #include "field.h" -frc846::util::FieldPoint field::points::testing_origin{"testing_origin", 0_in, - 0_in, 0_deg}; -frc846::util::FieldPoint field::points::testing_point{"testing_point", 0_in, - 100_in, 0_deg}; +// frc846::util::FieldPoint field::points::testing_origin{"testing_origin", +// 0_in, +// 0_in, 0_deg}; +// frc846::util::FieldPoint field::points::testing_point{"testing_point", 0_in, +// 100_in, 0_deg}; -frc846::util::FieldPoint field::points::five_piece_origin{"5p_origin", 217.5_in, - 49_in, 0_deg}; -frc846::util::FieldPoint field::points::five_piece_intake_one{ - "5p_intake_one", 217.5_in, 112_in, 0_deg}; -frc846::util::FieldPoint field::points::five_piece_mid_one{"5p_mid_one", 292_in, - 324_in, 20_deg}; -frc846::util::FieldPoint field::points::five_piece_shoot_one{ - "5p_shoot_one", 217.5_in, 112_in, 0_deg}; -frc846::util::FieldPoint field::points::five_piece_intake_two{ - "5p_intake_two", 292_in, 324_in, 20_deg}; -frc846::util::FieldPoint field::points::five_piece_mid_two{"5p_mid_two", 292_in, - 324_in, 20_deg}; -frc846::util::FieldPoint field::points::five_piece_shoot_two{ - "5p_shoot_two", 217.5_in, 53_in, 0_deg}; -frc846::util::FieldPoint field::points::five_piece_intake_three{ - "5p_intake_three", 160.5_in, 112_in, -40_deg}; -frc846::util::FieldPoint field::points::five_piece_mid_three{ - "5p_mid_three", 292_in, 324_in, 20_deg}; -frc846::util::FieldPoint field::points::five_piece_shoot_three{ - "5p_shoot_three", 217.5_in, 53_in, 0_deg}; \ No newline at end of file +// frc846::util::FieldPoint field::points::five_piece_origin{"5p_origin", +// 217.5_in, +// 49_in, 0_deg}; +// frc846::util::FieldPoint field::points::five_piece_intake_one{ +// "5p_intake_one", 217.5_in, 112_in, 0_deg}; +// frc846::util::FieldPoint field::points::five_piece_mid_one{"5p_mid_one", +// 292_in, +// 324_in, 20_deg}; +// frc846::util::FieldPoint field::points::five_piece_shoot_one{ +// "5p_shoot_one", 217.5_in, 112_in, 0_deg}; +// frc846::util::FieldPoint field::points::five_piece_intake_two{ +// "5p_intake_two", 292_in, 324_in, 20_deg}; +// frc846::util::FieldPoint field::points::five_piece_mid_two{"5p_mid_two", +// 292_in, +// 324_in, 20_deg}; +// frc846::util::FieldPoint field::points::five_piece_shoot_two{ +// "5p_shoot_two", 217.5_in, 53_in, 0_deg}; +// frc846::util::FieldPoint field::points::five_piece_intake_three{ +// "5p_intake_three", 160.5_in, 112_in, -40_deg}; +// frc846::util::FieldPoint field::points::five_piece_mid_three{ +// "5p_mid_three", 292_in, 324_in, 20_deg}; +// frc846::util::FieldPoint field::points::five_piece_shoot_three{ +// "5p_shoot_three", 217.5_in, 53_in, 0_deg}; \ No newline at end of file diff --git a/src/y2024/cpp/main.cpp b/src/y2024/cpp/main.cpp index 8dfc501..f92c0da 100644 --- a/src/y2024/cpp/main.cpp +++ b/src/y2024/cpp/main.cpp @@ -3,7 +3,7 @@ #include #include "FunkyRobot.h" -// #include "frc846/math/vectors.h" + // #include // #include // #include @@ -126,10 +126,6 @@ int main() { } }*/ - // frc846::math::VectorND v1{{1_in, 2_in, 3_in}}; - // frc846::math::VectorND v2{5_deg, 1_in}; - // frc846::math::VectorND v3{5_deg, 1_in}; - std::cout << "Starting robot code [2024]..." << std::endl; return frc::StartRobot(); } \ No newline at end of file diff --git a/src/y2024/cpp/subsystems/abstract/control_input.cc b/src/y2024/cpp/subsystems/abstract/control_input.cc index 9d3ea3a..81dda79 100644 --- a/src/y2024/cpp/subsystems/abstract/control_input.cc +++ b/src/y2024/cpp/subsystems/abstract/control_input.cc @@ -98,9 +98,13 @@ ControlInputReadings ControlInputSubsystem::ReadFromHardware() { readings.home_wrist ? 1 : 0); } - if (readings.save_trim != previous_readings_.save_trim) { - Log("ControlInput [Save Trim] state changed to {}", - readings.save_trim ? 1 : 0); + if (readings.trim_up != previous_readings_.trim_up) { + Log("ControlInput [Trim Up] state changed to {}", readings.trim_up ? 1 : 0); + } + + if (readings.trim_down != previous_readings_.trim_down) { + Log("ControlInput [Trim Down] state changed to {}", + readings.trim_up ? 1 : 0); } if (std::abs(readings.pivot_manual_adjust) > 0.01 && @@ -196,8 +200,9 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() { ci_readings_.home_wrist = dr_readings.b_button; ci_readings_.zero_bearing = dr_readings.back_button; - // SAVE TRIM - ci_readings_.save_trim = op_readings.x_button; + // TRIM + ci_readings_.trim_up = op_readings.y_button; + ci_readings_.trim_down = op_readings.a_button; previous_operator_ = op_readings; previous_driver_ = dr_readings; diff --git a/src/y2024/cpp/subsystems/abstract/driver.cc b/src/y2024/cpp/subsystems/abstract/driver.cc index 55dd106..1963e4b 100644 --- a/src/y2024/cpp/subsystems/abstract/driver.cc +++ b/src/y2024/cpp/subsystems/abstract/driver.cc @@ -29,10 +29,7 @@ DriverReadings DriverSubsystem::ReadFromHardware() { void DriverSubsystem::WriteToHardware(DriverTarget target) { target_rumble_graph_.Graph(target.rumble); - auto rumble = - (target.rumble || frc846::util::ShareTables::GetBoolean("ready_to_shoot")) - ? rumble_strength_.value() - : 0; + auto rumble = (target.rumble) ? rumble_strength_.value() : 0; xbox_.SetRumble(frc::XboxController::RumbleType::kLeftRumble, rumble); xbox_.SetRumble(frc::XboxController::RumbleType::kRightRumble, rumble); diff --git a/src/y2024/cpp/subsystems/abstract/super_structure.cc b/src/y2024/cpp/subsystems/abstract/super_structure.cc index 3fdea1e..6dc956e 100644 --- a/src/y2024/cpp/subsystems/abstract/super_structure.cc +++ b/src/y2024/cpp/subsystems/abstract/super_structure.cc @@ -39,16 +39,14 @@ void SuperStructureSubsystem::WriteToHardware(SuperStructureTarget target) { wrist_home_counter = 0; } - CoordinatePositions targetPosIntake{ - ArmKinematics::calculateCoordinatePosition( - {targetPos.pivot, targetPos.telescope, targetPos.wrist}, true)}; - CoordinatePositions targetPosShooter{ - ArmKinematics::calculateCoordinatePosition( - {targetPos.pivot, targetPos.telescope, targetPos.wrist}, false)}; - - intake_point_x_graph_.Graph(targetPosIntake.forward_axis); - intake_point_y_graph_.Graph(targetPosIntake.upward_axis); - - shooter_point_x_graph_.Graph(targetPosShooter.forward_axis); - shooter_point_y_graph_.Graph(targetPosShooter.upward_axis); + auto target_end_effector_positions = arm_kinematics_calculator.calculate( + {targetPos.pivot, targetPos.telescope, targetPos.wrist}); + + intake_point_x_graph_.Graph(target_end_effector_positions.intake_position[0]); + intake_point_y_graph_.Graph(target_end_effector_positions.intake_position[1]); + + shooter_point_x_graph_.Graph( + target_end_effector_positions.shooter_position[0]); + shooter_point_y_graph_.Graph( + target_end_effector_positions.shooter_position[1]); } diff --git a/src/y2024/cpp/subsystems/abstract/vision.cc b/src/y2024/cpp/subsystems/abstract/vision.cc index d47d86c..47691fc 100644 --- a/src/y2024/cpp/subsystems/abstract/vision.cc +++ b/src/y2024/cpp/subsystems/abstract/vision.cc @@ -4,7 +4,7 @@ #include #include "field.h" -#include "frc846/util/math.h" +#include "frc846/math/vectors.h" #include "frc846/util/share_tables.h" VisionSubsystem::VisionSubsystem(bool init) @@ -94,17 +94,17 @@ VisionReadings VisionSubsystem::ReadFromHardware() { // {{robot_x, robot_y}, bearing_}, // !frc846::util::ShareTables::GetBoolean("is_red_side"), true); - // robot_y = flipped.point.y; - // robot_x = flipped.point.x; + // robot_y = flipped.point[1]; + // robot_x = flipped.point[0]; // bearing_ = flipped.bearing; - frc846::util::Vector2D local_tag_vec{ + frc846::math::VectorND local_tag_vec{ newReadings.local_x_dist, newReadings.local_y_dist}; - auto tag_vec = local_tag_vec.Rotate(bearing_); + auto tag_vec = local_tag_vec.rotate(bearing_); - units::inch_t tag_x_dist = tag_vec.x; + units::inch_t tag_x_dist = tag_vec[0]; - units::inch_t tag_y_dist = tag_vec.y; + units::inch_t tag_y_dist = tag_vec[1]; y_correction = (april_tag.y_pos - tag_y_dist) - (robot_y - latency * 1_s * velocity_y); @@ -121,28 +121,26 @@ VisionReadings VisionSubsystem::ReadFromHardware() { newReadings.est_dist_from_speaker_x = robot_x - field::points::kSpeaker( - !frc846::util::ShareTables::GetBoolean("is_red_side")) - .x; + !frc846::util::ShareTables::GetBoolean("is_red_side"))[0]; newReadings.est_dist_from_speaker_y = robot_y - field::points::kSpeaker( - !frc846::util::ShareTables::GetBoolean("is_red_side")) - .y; + !frc846::util::ShareTables::GetBoolean("is_red_side"))[1]; - auto point_target = frc846::util::Vector2D{ + auto point_target = frc846::math::VectorND{ -newReadings.est_dist_from_speaker_x, -newReadings.est_dist_from_speaker_y}; newReadings.velocity_in_component = - ((velocity_x * point_target.x + velocity_y * point_target.y) / - point_target.Magnitude()) + ((velocity_x * point_target[0] + velocity_y * point_target[1]) / + point_target.magnitude()) .to(); - point_target = point_target.Rotate(90_deg); + point_target = point_target.rotate(90_deg); newReadings.velocity_orth_component = - ((velocity_x * point_target.x + velocity_y * point_target.y) / - point_target.Magnitude()) + ((velocity_x * point_target[0] + velocity_y * point_target[1]) / + point_target.magnitude()) .to(); newReadings.est_dist_from_speaker = units::math::sqrt( diff --git a/src/y2024/cpp/subsystems/hardware/drivetrain.cc b/src/y2024/cpp/subsystems/hardware/drivetrain.cc index 801bd53..b846886 100644 --- a/src/y2024/cpp/subsystems/hardware/drivetrain.cc +++ b/src/y2024/cpp/subsystems/hardware/drivetrain.cc @@ -11,8 +11,10 @@ double vel_readings_composite_x; double vel_readings_composite_y; DrivetrainSubsystem::DrivetrainSubsystem(bool initialize) - : frc846::robot::GenericSubsystem{ - "drivetrain", initialize} { + : frc846::robot::GenericSubsystem{"drivetrain", + initialize}, + odometry_{frc846::math::VectorND{0.0_ft, 0.0_ft}} { bearing_offset_ = 0_deg; ZeroOdometry(); frc::SmartDashboard::PutData("Field", &m_field); @@ -74,10 +76,10 @@ void DrivetrainSubsystem::ZeroOdometry() { } void DrivetrainSubsystem::SetPoint( - frc846::util::Vector2D point) { + frc846::math::VectorND point) { if (!is_initialized()) return; - Log("set point x {} y {}", point.x, point.y); + Log("set point x {} y {}", point[0], point[1]); odometry_.SetPoint(point); } @@ -89,15 +91,15 @@ void DrivetrainSubsystem::SetBearing(units::degree_t bearing) { Log("Zeroed bearing {}", bearing); } -std::array, +std::array, DrivetrainSubsystem::kModuleCount> DrivetrainSubsystem::SwerveControl( - frc846::util::Vector2D translation, + frc846::math::VectorND translation, units::degrees_per_second_t rotation_speed, units::inch_t width, units::inch_t height, units::inch_t radius, units::feet_per_second_t max_speed) { // Locations of each module - static constexpr frc846::util::Vector2D + static frc846::math::VectorND kModuleLocationSigns[DrivetrainSubsystem::kModuleCount] = { {-1, +1}, // fl {+1, +1}, // fr @@ -105,16 +107,16 @@ DrivetrainSubsystem::SwerveControl( {+1, -1}, // br }; - std::array, + std::array, DrivetrainSubsystem::kModuleCount> module_targets; units::feet_per_second_t max_magnitude = 0_fps; for (int i = 0; i < kModuleCount; ++i) { // Location of the module relaive to the center - frc846::util::Vector2D location{ - kModuleLocationSigns[i].x * width / 2, - kModuleLocationSigns[i].y * height / 2, + frc846::math::VectorND location{ + kModuleLocationSigns[i][0] * width / 2, + kModuleLocationSigns[i][1] * height / 2, }; // Target direction for the module - angle from center of robot to @@ -122,18 +124,18 @@ DrivetrainSubsystem::SwerveControl( // // x and y inputs in atan2 are swapped to return a bearing units::degree_t direction = - units::math::atan2(location.x, location.y) + 90_deg; + units::math::atan2(location[0], location[1]) + 90_deg; // do 90 - direction to convert bearing to cartesian angle - frc846::util::Vector2D rotation{ + frc846::math::VectorND rotation{ rotation_speed * units::math::cos(90_deg - direction) * radius / 1_rad, rotation_speed * units::math::sin(90_deg - direction) * radius / 1_rad, }; module_targets[i] = translation + rotation; - if (module_targets[i].Magnitude() <= max_speed * 4) { + if (module_targets[i].magnitude() <= max_speed * 4) { max_magnitude = - units::math::max(max_magnitude, module_targets[i].Magnitude()); + units::math::max(max_magnitude, module_targets[i].magnitude()); } } @@ -142,8 +144,8 @@ DrivetrainSubsystem::SwerveControl( if (max_magnitude > max_speed) { auto scale = max_speed / max_magnitude; for (auto& t : module_targets) { - t.x *= scale; - t.y *= scale; + t[0] *= scale; + t[1] *= scale; } } @@ -184,32 +186,32 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { readings.is_gyro_connected = gyro_.IsConnected(); - readings.pose.bearing = units::degree_t(gyro_.GetYaw()) + bearing_offset_; + auto bearing = units::degree_t(gyro_.GetYaw()) + bearing_offset_; readings.angular_velocity = units::degrees_per_second_t(gyro_.GetRate()); auto pitch_initial = units::degree_t(gyro_.GetPitch()); auto roll_initial = units::degree_t(gyro_.GetRoll()); - readings.tilt = - units::degree_t{pitch_initial * units::math::cos(readings.pose.bearing) + - roll_initial * units::math::sin(readings.pose.bearing)}; + readings.tilt = units::degree_t{pitch_initial * units::math::cos(bearing) + + roll_initial * units::math::sin(bearing)}; // auto current_time_ = frc846::wpilib::CurrentFPGATime(); // Gets the position difference vector for each module, to update odometry // with - std::array, kModuleCount> module_outs; + std::array, kModuleCount> + module_outs; for (int i = 0; i < kModuleCount; ++i) { modules_all_[i]->UpdateReadings(); auto d = modules_all_[i]->GetReadings().distance; units::radian_t a = modules_all_[i]->GetReadings().direction; - frc846::util::Vector2D vec{ + frc846::math::VectorND vec{ d * units::math::sin(a), d * units::math::cos(a), }; module_outs[i] = vec; } - odometry_.Update(module_outs, readings.pose.bearing); + odometry_.Update(module_outs, bearing); units::feet_per_second_t total_x = 0_fps, total_y = 0_fps; for (auto module : modules_all_) { @@ -219,39 +221,40 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { units::math::sin(90_deg - module->GetReadings().direction); } - frc846::util::Vector2D unfiltered_velocity = { + frc846::math::VectorND unfiltered_velocity = { total_x / kModuleCount, total_y / kModuleCount}; - readings.velocity = unfiltered_velocity; + readings.pose = frc846::math::FieldPoint(odometry_.position(), bearing, + unfiltered_velocity); - pose_x_graph_.Graph(odometry_.pose().point.x); - pose_y_graph_.Graph(odometry_.pose().point.y); - pose_bearing_graph.Graph(odometry_.pose().bearing); - v_x_graph_.Graph(readings.velocity.x); - v_y_graph_.Graph(readings.velocity.y); + pose_x_graph_.Graph(odometry_.position()[0]); + pose_y_graph_.Graph(odometry_.position()[1]); + // pose_bearing_graph.Graph(odometry_.position().bearing); + v_x_graph_.Graph(readings.pose.velocity[0]); + v_y_graph_.Graph(readings.pose.velocity[1]); frc846::util::ShareTables::SetDouble("robot_bearing_", readings.pose.bearing.to()); frc846::util::ShareTables::SetDouble("odometry_x_", - odometry_.pose().point.x.to()); + odometry_.position()[0].to()); frc846::util::ShareTables::SetDouble("odometry_y_", - odometry_.pose().point.y.to()); + odometry_.position()[1].to()); frc846::util::ShareTables::SetDouble("velocity_x_", - readings.velocity.x.to()); + readings.pose.velocity[0].to()); frc846::util::ShareTables::SetDouble("velocity_y_", - readings.velocity.y.to()); - - readings.pose.point = odometry_.pose().point; + readings.pose.velocity[1].to()); SetMap(); - vel_readings_composite_x = units::unit_cast(readings.velocity.x); + vel_readings_composite_x = + units::unit_cast(readings.pose.velocity[0]); if (vel_readings_composite_x < 0.0) { vel_readings_composite_x = -vel_readings_composite_x; } - vel_readings_composite_y = units::unit_cast(readings.velocity.y); + vel_readings_composite_y = + units::unit_cast(readings.pose.velocity[1]); if (vel_readings_composite_y < 0.0) { vel_readings_composite_y = -vel_readings_composite_y; @@ -266,9 +269,9 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { void DrivetrainSubsystem::SetMap() { // set odometry - m_field.SetRobotPose(frc::Pose2d(odometry_.pose().point.y, - -odometry_.pose().point.x, - odometry_.pose().bearing)); + m_field.SetRobotPose(frc::Pose2d(odometry_.position()[1], + -odometry_.position()[0], + GetReadings().pose.bearing)); } void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) { @@ -293,22 +296,22 @@ void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) { throw std::runtime_error{"unhandled case"}; } - frc846::util::Vector2D target_translation = { + frc846::math::VectorND target_translation = { target.v_x, target.v_y}; // rotate translation vector if field oriented if (target.translation_reference == DrivetrainTranslationReference::kField) { units::degree_t offset = GetReadings().pose.bearing; - target_translation = target_translation.Rotate(-offset); + target_translation = target_translation.rotate(-offset); } - target_translation.x = vx_ramp_rate_.Calculate(target_translation.x); - target_translation.y = vy_ramp_rate_.Calculate(target_translation.y); + target_translation[0] = vx_ramp_rate_.Calculate(target_translation[0]); + target_translation[1] = vy_ramp_rate_.Calculate(target_translation[1]); units::degrees_per_second_t target_omega; if (auto* theta = std::get_if(&target.rotation)) { // position control auto p_error = - frc846::util::CoterminalDifference(*theta, GetReadings().pose.bearing); + frc846::math::CoterminalDifference(*theta, GetReadings().pose.bearing); auto d_error = GetReadings().angular_velocity; target_omega = units::degrees_per_second_t( @@ -340,7 +343,7 @@ void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) { for (int i = 0; i < kModuleCount; ++i) { modules_all_[i]->SetTarget( - {targets[i].Magnitude(), targets[i].Bearing(), target.control}); + {targets[i].magnitude(), targets[i].angle(true), target.control}); modules_all_[i]->UpdateHardware(); } } \ No newline at end of file diff --git a/src/y2024/include/autos/drive_auto.h b/src/y2024/include/autos/drive_auto.h index d4b76e4..897ad1b 100644 --- a/src/y2024/include/autos/drive_auto.h +++ b/src/y2024/include/autos/drive_auto.h @@ -3,7 +3,6 @@ #include "commands/follow_trajectory_command.h" #include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class DriveAuto diff --git a/src/y2024/include/autos/five_piece_auto.h b/src/y2024/include/autos/five_piece_auto.h index 9fa424b..dd93762 100644 --- a/src/y2024/include/autos/five_piece_auto.h +++ b/src/y2024/include/autos/five_piece_auto.h @@ -3,7 +3,6 @@ #include "commands/follow_trajectory_command.h" #include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class FivePieceAuto diff --git a/src/y2024/include/autos/one_piece_auto.h b/src/y2024/include/autos/one_piece_auto.h index 6893277..6e9aef8 100644 --- a/src/y2024/include/autos/one_piece_auto.h +++ b/src/y2024/include/autos/one_piece_auto.h @@ -3,7 +3,6 @@ #include "commands/follow_trajectory_command.h" #include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class OnePieceAuto diff --git a/src/y2024/include/calculators/arm_kinematics_calculator.h b/src/y2024/include/calculators/arm_kinematics_calculator.h new file mode 100644 index 0000000..50031de --- /dev/null +++ b/src/y2024/include/calculators/arm_kinematics_calculator.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include "frc846/math/calculator.h" +#include "frc846/math/vectors.h" + +struct AKCalculatorInput { + units::degree_t pivot_angle; + units::inch_t telescope_extension; + units::degree_t wrist_angle; +}; + +struct AKCalculatorOutput { + frc846::math::Vector2D shooter_position; + frc846::math::Vector2D intake_position; +}; + +struct AKCalculatorConstants { + frc846::math::Vector2D pivot_to_wrist_joint; + frc846::math::Vector2D wrist_joint_to_shooter; + frc846::math::Vector2D shooter_to_intake; + + frc846::math::Vector2D pivot_offset; + + units::inch_t max_extension_from_center; + units::inch_t max_height; +}; + +class ArmKinematicsCalculator + : public frc846::math::Calculator { + public: + ArmKinematicsCalculator(); + + AKCalculatorOutput calculate(AKCalculatorInput input) override; + + bool WithinBounds(AKCalculatorOutput end_effector_positions); + + private: + AKCalculatorConstants constants_; +}; \ No newline at end of file diff --git a/src/y2024/include/calculators/shooting_calculator.h b/src/y2024/include/calculators/shooting_calculator.h new file mode 100644 index 0000000..b4b586b --- /dev/null +++ b/src/y2024/include/calculators/shooting_calculator.h @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include +#include + +#include "frc846/math/calculator.h" + +struct ShootingCalculatorInput { + units::foot_t shot_distance; + units::feet_per_second_t rv_i; + units::feet_per_second_t rv_o; + + units::foot_t shooter_height; + + units::degree_t + default_setpoint; // Not in constants so that these can be changed live + // (without restarting robot code) + units::feet_per_second_t shot_speed; +}; + +struct ShootingCalculatorOutput { + units::degree_t launch_angle; + units::degree_t twist_angle; +}; + +struct ShootingCalculatorConstants { + units::foot_t speaker_height; +}; + +class ShootingCalculator + : public frc846::math::IterativeCalculator { + public: + ShootingCalculatorOutput calculateIteration( + ShootingCalculatorInput input, + ShootingCalculatorOutput prev_output) override; +}; \ No newline at end of file diff --git a/src/y2024/include/commands/auto_intake_and_shoot_command.h b/src/y2024/include/commands/auto_intake_and_shoot_command.h index ab8c861..5fec77f 100644 --- a/src/y2024/include/commands/auto_intake_and_shoot_command.h +++ b/src/y2024/include/commands/auto_intake_and_shoot_command.h @@ -3,7 +3,6 @@ #include "commands/follow_trajectory_command.h" #include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class AutoIntakeAndShootCommand @@ -12,7 +11,7 @@ class AutoIntakeAndShootCommand frc2::SequentialCommandGroup> { public: AutoIntakeAndShootCommand(RobotContainer& container, - frc846::InputWaypoint intake_point, - frc846::InputWaypoint mid_point, - frc846::InputWaypoint shoot_point); + frc846::Waypoint intake_point, + frc846::Waypoint mid_point, + frc846::Waypoint shoot_point); }; diff --git a/src/y2024/include/commands/basic/amp_command.h b/src/y2024/include/commands/basic/amp_command.h index aea8ca7..7bb3627 100644 --- a/src/y2024/include/commands/basic/amp_command.h +++ b/src/y2024/include/commands/basic/amp_command.h @@ -1,7 +1,6 @@ #pragma once #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/abstract/super_structure.h" #include "subsystems/robot_container.h" diff --git a/src/y2024/include/commands/basic/auto_deploy_intake_command.h b/src/y2024/include/commands/basic/auto_deploy_intake_command.h index e4bf55d..cf65fa7 100644 --- a/src/y2024/include/commands/basic/auto_deploy_intake_command.h +++ b/src/y2024/include/commands/basic/auto_deploy_intake_command.h @@ -1,7 +1,6 @@ #pragma once #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class AutoDeployIntakeCommand diff --git a/src/y2024/include/commands/basic/prepare_auto_shoot_command.h b/src/y2024/include/commands/basic/prepare_auto_shoot_command.h index 5700355..84b742b 100644 --- a/src/y2024/include/commands/basic/prepare_auto_shoot_command.h +++ b/src/y2024/include/commands/basic/prepare_auto_shoot_command.h @@ -1,8 +1,7 @@ #pragma once -#include "constants.h" +#include "calculators/shooting_calculator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class PrepareAutoShootCommand @@ -18,7 +17,4 @@ class PrepareAutoShootCommand void OnEnd(bool interrupted) override; bool IsFinished() override; - - private: - ShootingCalculator shooting_calculator_; }; diff --git a/src/y2024/include/commands/basic/prepare_shoot_command.h b/src/y2024/include/commands/basic/prepare_shoot_command.h index 122c653..3e001aa 100644 --- a/src/y2024/include/commands/basic/prepare_shoot_command.h +++ b/src/y2024/include/commands/basic/prepare_shoot_command.h @@ -1,8 +1,7 @@ #pragma once -#include "constants.h" +#include "calculators/shooting_calculator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class PrepareShootCommand diff --git a/src/y2024/include/commands/basic/shoot_command.h b/src/y2024/include/commands/basic/shoot_command.h index daf0671..92dc580 100644 --- a/src/y2024/include/commands/basic/shoot_command.h +++ b/src/y2024/include/commands/basic/shoot_command.h @@ -15,4 +15,7 @@ class ShootCommand void OnEnd(bool interrupted) override; bool IsFinished() override; + + private: + int num_loops_; }; diff --git a/src/y2024/include/commands/complex/super_amp_command.h b/src/y2024/include/commands/complex/super_amp_command.h index f2f8578..45c3fa2 100644 --- a/src/y2024/include/commands/complex/super_amp_command.h +++ b/src/y2024/include/commands/complex/super_amp_command.h @@ -5,7 +5,6 @@ #include "commands/follow_trajectory_command.h" #include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class SuperAmpCommand diff --git a/src/y2024/include/commands/follow_trajectory_command.h b/src/y2024/include/commands/follow_trajectory_command.h index 51905a2..941d44e 100644 --- a/src/y2024/include/commands/follow_trajectory_command.h +++ b/src/y2024/include/commands/follow_trajectory_command.h @@ -2,7 +2,6 @@ #include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" -#include "frc846/util/math.h" #include "subsystems/robot_container.h" class FollowTrajectoryCommand @@ -10,7 +9,7 @@ class FollowTrajectoryCommand FollowTrajectoryCommand> { public: FollowTrajectoryCommand(RobotContainer& container, - std::vector input_points); + std::vector input_points); void OnInit() override; @@ -21,17 +20,17 @@ class FollowTrajectoryCommand bool IsFinished() override; private: - std::vector input_points_; + std::vector input_points_; frc846::Trajectory trajectory_; unsigned int target_idx_ = 1; bool is_done_ = false; - frc846::util::Vector2D current_extrapolated_point_; + frc846::math::VectorND current_extrapolated_point_; units::second_t start_time_; static bool HasCrossedWaypoint( frc846::Waypoint current_waypoint, frc846::Waypoint prev_waypoint, - frc846::util::Vector2D pos, - frc846::util::Vector2D test_target); + frc846::math::VectorND pos, + frc846::math::VectorND test_target); }; diff --git a/src/y2024/include/commands/teleop/drive_command.h b/src/y2024/include/commands/teleop/drive_command.h index 1da3208..9136ab6 100644 --- a/src/y2024/include/commands/teleop/drive_command.h +++ b/src/y2024/include/commands/teleop/drive_command.h @@ -1,5 +1,6 @@ #pragma once +#include "calculators/shooting_calculator.h" #include "frc846/robot/GenericCommand.h" #include "subsystems/robot_container.h" diff --git a/src/y2024/include/constants.h b/src/y2024/include/constants.h deleted file mode 100644 index 4ea36e5..0000000 --- a/src/y2024/include/constants.h +++ /dev/null @@ -1,193 +0,0 @@ -#pragma once - -#include - -#include "frc846/base/loggable.h" -#include "frc846/util/math.h" - -using namespace frc846::util; - -struct ShootingAngles { - units::degree_t launch_angle; - units::degree_t turning_offset_angle; -}; - -class ShootingCalculator { - private: - static double pow(double base, int exponent) { - for (int i = 0; i < exponent; i++) { - base *= base; - } - return base; - } - - static constexpr double pi = 3.14159265; - - static double radians(double degs) { return degs * pi / 180; } - - static double degs(double radians) { return radians * 180 / pi; } - - static constexpr double g = 32.0; - - static constexpr double h_speaker = 78.0 / 12.0; - - double lastAngle = 0.0; - - public: - ShootingAngles calculateLaunchAngles(double v, double d, double r_v, - double r_o, double h_shooter, - double angle_setpoint = 55.0, - double max_iterations = 7) { - if (d <= 4.0) { - return {1_deg * angle_setpoint, 0.0_deg}; - } - - try { - double t = 0.0; - for (int i = 0; i < max_iterations; i++) { - t = d / (r_v + v * std::cos(std::asin(r_o / v)) * std::cos(lastAngle)); - lastAngle = std::asin((h_speaker - h_shooter + 1.0 / 2.0 * g * t * t) / - (v * t)); - } - - if (v * std::sin(lastAngle) / g >= t) { - return {units::radian_t(lastAngle), - units::radian_t(std::asin(r_o / v))}; - } - } catch (std::exception& exc) { - (void)exc; - } - - // std::cout << "[Shooting calculator] out of range"; - - lastAngle = radians(0.0); - return {0.0_deg, 0.0_deg}; - } -}; - -struct RawPositions { - units::degree_t pivot_angle; - units::inch_t extension; - units::degree_t wrist_angle; -}; - -struct CoordinatePositions { - units::degree_t shooting_angle; - units::inch_t forward_axis; - units::inch_t upward_axis; -}; - -static constexpr double radians(double degs) { - return degs * 3.141592658979 / 180; -} - -static constexpr double degs(double radians) { - return radians * 180 / 3.141592658979; -} - -// FIX adding/removing home_offsets -class ArmKinematics { - private: - static constexpr units::inch_t pivotToWrist = 20.5_in; - static constexpr units::inch_t pivotToWristOffset = -4.25_in; - - static constexpr units::inch_t wristToShooter = 10_in; - static constexpr units::inch_t shooterToIntake = 12.0_in; - - static constexpr double wristToIntakeOtherAngle = radians(36.5); - - static constexpr units::inch_t pivotToGround = 16.25_in; - - static constexpr units::inch_t pivotToCenter = -9.25_in; - - static constexpr units::inch_t robotWidth = 28_in; - - static double pow(double base, int exponent) { - for (int i = 1; i < exponent; i++) { - base *= base; - } - return base; - } - - public: - /* default position calculated is for the center of the shooter*/ - static CoordinatePositions calculateCoordinatePosition(RawPositions pos_raw, - bool intakePoint); - - static CoordinatePositions calculateRawPosition(RawPositions pos_raw, - bool intakePoint); - - static bool withinBounds(CoordinatePositions pos) { - return (pos.forward_axis < robotWidth / 2.0 + 12.0_in && - pos.upward_axis < 47.5_in); - } - - static double sumOutOfBounds(CoordinatePositions pos) { - units::inch_t sumOut = 0.0_in; - if (pos.forward_axis >= (robotWidth / 2.0 + 12.0_in)) { - sumOut += pos.forward_axis - (robotWidth / 2.0 + 12.0_in); - } - if (pos.upward_axis >= 48.0_in) { - sumOut += pos.upward_axis - 48.0_in; - } - return sumOut.to(); - } - - /*static RawPositions toRaw(CoordinatePositions pos, int point = 0) { - RawPositions raw{}; - - if (point == 0) { - double pivotForwardComponent = - (pos.forward_axis - (wristToIntake)*std::cos(pos.shooting_angle)) + - pivotToCenter; - - double pivotUpwardComponent = - (pos.upward_axis - (wristToIntake)*std::sin(pos.shooting_angle)) - - pivotToGround; - - raw.extension = - std::sqrt(pow(pivotForwardComponent, 2) + - pow(pivotUpwardComponent, 2) - pow(pivotToWristOffset, 2)) - - pivotToWrist; - - double pivotTotalAngle = - (std::atan2(pivotUpwardComponent, pivotForwardComponent)); - - double pivotAddedAngle = - (std::atan2(pivotToWristOffset, pivotToWrist + raw.extension)); - - raw.pivot_angle = (pivotTotalAngle - pivotAddedAngle + radians(17.0)); - - raw.wrist_angle = -(((pos.shooting_angle - raw.pivot_angle + - radians(17.0) - radians(47))) - - wristToIntakeOtherAngle); - - } else if (point == 1) { - double pivotForwardComponent = - (pos.forward_axis - (wristToFlywheels)*std::cos(pos.shooting_angle) + - pivotToCenter); - - double pivotUpwardComponent = - (pos.upward_axis - (wristToFlywheels)*std::sin(pos.shooting_angle) - - pivotToGround); - - raw.extension = - std::sqrt(pow(pivotForwardComponent, 2) + - pow(pivotUpwardComponent, 2) - pow(pivotToWristOffset, 2)) - - pivotToWrist; - - double pivotTotalAngle = - (std::atan2(pivotUpwardComponent, pivotForwardComponent)); - - double pivotAddedAngle = - (std::atan2(pivotToWristOffset, pivotToWrist + raw.extension)); - - raw.pivot_angle = (pivotTotalAngle - pivotAddedAngle + radians(17.0)); - - raw.wrist_angle = -(((pos.shooting_angle - raw.pivot_angle + - radians(17.0) - radians(47)))); - } - - return raw; - }*/ -}; diff --git a/src/y2024/include/field.h b/src/y2024/include/field.h index 5ea2999..0c44f65 100644 --- a/src/y2024/include/field.h +++ b/src/y2024/include/field.h @@ -2,94 +2,92 @@ #include -#include "frc846/util/math.h" +#include "frc846/math/fieldpoints.h" // Field has blue alliance far right corner as origin struct field { struct points { - static frc846::util::Position Origin() { - return frc846::util::Position( - frc846::util::Vector2D(0_in, 0_in), 0_deg); - }; + static frc846::math::FieldPoint Origin() { + return {{0_in, 0_in}, 0_deg, {0_fps, 0_fps}}; + } - static frc846::util::Vector2D kSpeaker(bool flip = false) { + static frc846::math::VectorND kSpeaker( + bool flip = false) { if (!flip) { - return frc846::util::Vector2D(217.5_in, -4_in); + return {217.5_in, -4_in}; } else { - return frc846::util::Vector2D(217.5_in, 655.8_in); + return {217.5_in, 655.8_in}; } } - static frc846::util::Position kAmpNoFlip() { - return frc846::util::Position( - frc846::util::Vector2D(0_in, 0_in), 90_deg); + static frc846::math::FieldPoint kAmpNoFlip() { + return {{0_in, 0_in}, 90_deg, {0_fps, 0_fps}}; } - static frc846::util::Position kPreAmpNoFlip() { - return frc846::util::Position( - frc846::util::Vector2D(-2_ft, 0_in), 90_deg); + static frc846::math::FieldPoint kPreAmpNoFlip() { + return {{-2_ft, 0_in}, 90_deg, {0_fps, 0_fps}}; } - // TESTING - static frc846::util::FieldPoint testing_origin; - static frc846::util::Position kTestingOrigin(bool should_flip) { - return testing_origin.flip(should_flip); - }; - - static frc846::util::FieldPoint testing_point; - static frc846::util::Position kTestingPoint(bool should_flip) { - return testing_point.flip(should_flip); - }; - - // FIVE PIECE AUTO - static frc846::util::FieldPoint five_piece_origin; - static frc846::util::Position kFivePieceOrigin(bool should_flip) { - return five_piece_origin.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_intake_one; - static frc846::util::Position kFivePieceIntakeOne(bool should_flip) { - return five_piece_intake_one.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_mid_one; - static frc846::util::Position kFivePieceMidOne(bool should_flip) { - return five_piece_mid_one.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_shoot_one; - static frc846::util::Position kFivePieceShootOne(bool should_flip) { - return five_piece_shoot_one.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_intake_two; - static frc846::util::Position kFivePieceIntakeTwo(bool should_flip) { - return five_piece_intake_two.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_mid_two; - static frc846::util::Position kFivePieceMidTwo(bool should_flip) { - return five_piece_mid_two.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_shoot_two; - static frc846::util::Position kFivePieceShootTwo(bool should_flip) { - return five_piece_shoot_two.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_intake_three; - static frc846::util::Position kFivePieceIntakeThree(bool should_flip) { - return five_piece_intake_three.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_mid_three; - static frc846::util::Position kFivePieceMidThree(bool should_flip) { - return five_piece_mid_three.flip(should_flip, true); - }; - - static frc846::util::FieldPoint five_piece_shoot_three; - static frc846::util::Position kFivePieceShootThree(bool should_flip) { - return five_piece_shoot_three.flip(should_flip, true); - }; + // // TESTING + // static frc846::util::FieldPoint testing_origin; + // static frc846::util::Position kTestingOrigin(bool should_flip) { + // return testing_origin.flip(should_flip); + // }; + + // static frc846::util::FieldPoint testing_point; + // static frc846::util::Position kTestingPoint(bool should_flip) { + // return testing_point.flip(should_flip); + // }; + + // // FIVE PIECE AUTO + // static frc846::util::FieldPoint five_piece_origin; + // static frc846::util::Position kFivePieceOrigin(bool should_flip) { + // return five_piece_origin.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_intake_one; + // static frc846::util::Position kFivePieceIntakeOne(bool should_flip) { + // return five_piece_intake_one.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_mid_one; + // static frc846::util::Position kFivePieceMidOne(bool should_flip) { + // return five_piece_mid_one.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_shoot_one; + // static frc846::util::Position kFivePieceShootOne(bool should_flip) { + // return five_piece_shoot_one.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_intake_two; + // static frc846::util::Position kFivePieceIntakeTwo(bool should_flip) { + // return five_piece_intake_two.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_mid_two; + // static frc846::util::Position kFivePieceMidTwo(bool should_flip) { + // return five_piece_mid_two.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_shoot_two; + // static frc846::util::Position kFivePieceShootTwo(bool should_flip) { + // return five_piece_shoot_two.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_intake_three; + // static frc846::util::Position kFivePieceIntakeThree(bool should_flip) { + // return five_piece_intake_three.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_mid_three; + // static frc846::util::Position kFivePieceMidThree(bool should_flip) { + // return five_piece_mid_three.flip(should_flip, true); + // }; + + // static frc846::util::FieldPoint five_piece_shoot_three; + // static frc846::util::Position kFivePieceShootThree(bool should_flip) { + // return five_piece_shoot_three.flip(should_flip, true); + // }; }; }; diff --git a/src/y2024/include/subsystems/abstract/control_input.h b/src/y2024/include/subsystems/abstract/control_input.h index 253473b..bc4f999 100644 --- a/src/y2024/include/subsystems/abstract/control_input.h +++ b/src/y2024/include/subsystems/abstract/control_input.h @@ -35,7 +35,8 @@ struct ControlInputReadings { bool home_wrist; bool zero_bearing; - bool save_trim; + bool trim_up; + bool trim_down; }; struct ControlInputTarget { diff --git a/src/y2024/include/subsystems/abstract/driver.h b/src/y2024/include/subsystems/abstract/driver.h index f9f7c12..27d5763 100644 --- a/src/y2024/include/subsystems/abstract/driver.h +++ b/src/y2024/include/subsystems/abstract/driver.h @@ -4,7 +4,6 @@ #include "frc846/ntinf/pref.h" #include "frc846/other/xbox.h" #include "frc846/robot/GenericSubsystem.h" -#include "frc846/util/math.h" #include "ports.h" using DriverReadings = frc846::XboxReadings; diff --git a/src/y2024/include/subsystems/abstract/operator.h b/src/y2024/include/subsystems/abstract/operator.h index d4bdae0..02a8e94 100644 --- a/src/y2024/include/subsystems/abstract/operator.h +++ b/src/y2024/include/subsystems/abstract/operator.h @@ -4,7 +4,6 @@ #include "frc846/ntinf/pref.h" #include "frc846/other/xbox.h" #include "frc846/robot/GenericSubsystem.h" -#include "frc846/util/math.h" #include "ports.h" using OperatorReadings = frc846::XboxReadings; diff --git a/src/y2024/include/subsystems/abstract/super_structure.h b/src/y2024/include/subsystems/abstract/super_structure.h index 9bbde47..e64785a 100644 --- a/src/y2024/include/subsystems/abstract/super_structure.h +++ b/src/y2024/include/subsystems/abstract/super_structure.h @@ -1,6 +1,6 @@ #pragma once -#include "constants.h" +#include "calculators/arm_kinematics_calculator.h" #include "frc846/control/motion.h" #include "frc846/ntinf/pref.h" #include "frc846/robot/GenericSubsystem.h" @@ -53,8 +53,7 @@ class SuperStructureSubsystem PTWSetpoint currentSetpoint; PTWSetpoint manualAdjustments; - PTWSetpoint lastAdjustments; - PTWSetpoint savedAdjustments; + units::degree_t wrist_trim_amt = 0_deg; public: PivotSubsystem* pivot_; @@ -95,29 +94,17 @@ class SuperStructureSubsystem currentSetpoint.wrist != newSetpoint.wrist || currentSetpoint.telescope != newSetpoint.telescope) { currentSetpoint = newSetpoint; - lastAdjustments = manualAdjustments; ClearAdjustments(); } } bool CheckValidAdjustment(PTWSetpoint adjusted) { - auto intakeWithinBounds = - ArmKinematics::withinBounds(ArmKinematics::calculateCoordinatePosition( + return true; + return arm_kinematics_calculator.WithinBounds( + arm_kinematics_calculator.calculate( {(currentSetpoint.pivot + adjusted.pivot), (currentSetpoint.telescope + adjusted.telescope), - (currentSetpoint.wrist + adjusted.wrist)}, - true)); - - if (!intakeWithinBounds) return false; - - auto shooterInBounds = - ArmKinematics::withinBounds(ArmKinematics::calculateCoordinatePosition( - {(currentSetpoint.pivot + adjusted.pivot), - (currentSetpoint.telescope + adjusted.telescope), - (currentSetpoint.wrist + adjusted.wrist)}, - false)); - - return shooterInBounds; + (currentSetpoint.wrist + adjusted.wrist)})); }; void AdjustSetpoint(PTWSetpoint newAdj) { @@ -138,12 +125,7 @@ class SuperStructureSubsystem } void ClearAdjustments() { - manualAdjustments = {savedAdjustments.pivot, savedAdjustments.telescope, - savedAdjustments.wrist}; - } - - void SaveAdjustments() { - savedAdjustments = {0.0_deg, 0.0_in, lastAdjustments.wrist}; + manualAdjustments = {0.0_deg, 0.0_in, wrist_trim_amt}; } bool hasReachedSetpoint(PTWSetpoint setpoint) { @@ -152,6 +134,22 @@ class SuperStructureSubsystem telescope_->WithinTolerance(setpoint.telescope); } + /* + */ + + frc846::ntinf::Pref wrist_trim_inc{ + *this, "wrist_trim_increment", 0.5_deg}; + + void TrimUp() { + wrist_trim_amt += wrist_trim_inc.value(); + manualAdjustments.wrist += wrist_trim_inc.value(); + } + + void TrimDown() { + wrist_trim_amt -= wrist_trim_inc.value(); + manualAdjustments.wrist -= wrist_trim_inc.value(); + } + /* */ @@ -239,6 +237,8 @@ class SuperStructureSubsystem bool homing_wrist = false; int wrist_home_counter = 0; + ArmKinematicsCalculator arm_kinematics_calculator; + SuperStructureReadings ReadFromHardware() override; void WriteToHardware(SuperStructureTarget target) override; diff --git a/src/y2024/include/subsystems/hardware/drivetrain.h b/src/y2024/include/subsystems/hardware/drivetrain.h index e185545..2cae8ee 100644 --- a/src/y2024/include/subsystems/hardware/drivetrain.h +++ b/src/y2024/include/subsystems/hardware/drivetrain.h @@ -11,21 +11,20 @@ #include #include +#include "frc846/math/fieldpoints.h" #include "frc846/ntinf/grapher.h" #include "frc846/ntinf/pref.h" #include "frc846/other/swerve_odometry.h" #include "frc846/robot/GenericSubsystem.h" -#include "frc846/util/math.h" #include "frc846/wpilib/time.h" #include "ports.h" #include "subsystems/hardware/swerve_module.h" struct DrivetrainReadings { bool is_gyro_connected; - frc846::util::Position pose; + frc846::math::FieldPoint pose; units::degrees_per_second_t angular_velocity; units::degree_t tilt; - frc846::util::Vector2D velocity; }; // Robot vs field oriented translation control. @@ -77,7 +76,7 @@ class DrivetrainSubsystem void ZeroOdometry(); // Set odometry point. - void SetPoint(frc846::util::Vector2D point); + void SetPoint(frc846::math::VectorND point); // Set bearing. void SetBearing(units::degree_t bearing); @@ -159,9 +158,9 @@ class DrivetrainSubsystem // Convert a translation vector and the drivetrain angular velocity to the // individual module outputs. - static std::array, + static std::array, kModuleCount> - SwerveControl(frc846::util::Vector2D translation, + SwerveControl(frc846::math::VectorND translation, units::degrees_per_second_t rotation_speed, units::inch_t width, units::inch_t height, units::inch_t radius, units::feet_per_second_t max_speed); @@ -171,8 +170,6 @@ class DrivetrainSubsystem bool VerifyHardware() override; private: - int lastRelocalize = 0; - // Drivetrain dimensions. frc846::ntinf::Pref width_{*this, "width", 21.75_in}; frc846::ntinf::Pref height_{*this, "height", 26.75_in}; @@ -239,7 +236,7 @@ class DrivetrainSubsystem drive_esc_loggable_, {false, (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0) * - frc846::util::Circumference(wheel_radius_.value()).to() / + frc846::math::Circumference(wheel_radius_.value()).to() / 12.0, frc846::control::MotorIdleMode::kDefaultBrake, {80_A}}, diff --git a/src/y2024/include/subsystems/hardware/pivot.h b/src/y2024/include/subsystems/hardware/pivot.h index 6d16702..e62785a 100644 --- a/src/y2024/include/subsystems/hardware/pivot.h +++ b/src/y2024/include/subsystems/hardware/pivot.h @@ -142,5 +142,6 @@ class PivotSubsystem return std::abs( units::math::cos(GetReadings().pivot_position).to()); }, - {30_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}}; + {30_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}, + &hard_limits_}; }; diff --git a/src/y2024/include/subsystems/hardware/swerve_module.h b/src/y2024/include/subsystems/hardware/swerve_module.h index 05ca529..5640af9 100644 --- a/src/y2024/include/subsystems/hardware/swerve_module.h +++ b/src/y2024/include/subsystems/hardware/swerve_module.h @@ -14,7 +14,6 @@ #include "frc846/ntinf/grapher.h" #include "frc846/ntinf/pref.h" #include "frc846/robot/GenericSubsystem.h" -#include "frc846/util/math.h" // FRC846_CTRE_NAMESPACE() diff --git a/src/y2024/include/subsystems/hardware/wrist.h b/src/y2024/include/subsystems/hardware/wrist.h index cb80fec..34848b6 100644 --- a/src/y2024/include/subsystems/hardware/wrist.h +++ b/src/y2024/include/subsystems/hardware/wrist.h @@ -126,25 +126,28 @@ class WristSubsystem frc846::motion::BrakingPositionDyFPID dyFPID{ dyFPID_loggable, - [this](units::degree_t pos) -> double { + [&](units::degree_t pos) -> double { return std::abs( units::math::cos( - 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") + - GetReadings().wrist_position - wrist_cg_offset_.value()) + 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") - + GetReadings().wrist_position + wrist_cg_offset_.value()) .to()); }, - {30_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}}; - - frc846::base::Loggable close_dyFPID_loggable{*this, "CloseDynamicFPID"}; - - frc846::motion::BrakingPositionDyFPID dyFPIDClose{ - close_dyFPID_loggable, - [this](units::degree_t pos) -> double { - return std::abs( - units::math::cos( - 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") + - GetReadings().wrist_position - wrist_cg_offset_.value()) - .to()); - }, - {15_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}}; + {30_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}, + &hard_limits_}; + + // frc846::base::Loggable close_dyFPID_loggable{*this, "CloseDynamicFPID"}; + + // frc846::motion::BrakingPositionDyFPID dyFPIDClose{ + // close_dyFPID_loggable, + // [this](units::degree_t pos) -> double { + // return std::abs( + // units::math::cos( + // 1_deg * + // frc846::util::ShareTables::GetDouble("pivot_position") - + // GetReadings().wrist_position + wrist_cg_offset_.value()) + // .to()); + // }, + // {15_A, frc846::control::DefaultSpecifications::stall_current_neo, + // 0.3}}; }; diff --git a/src/y2024/resources/preferences_backup.nform b/src/y2024/resources/preferences_backup.nform index 554d1f7..f3c18b4 100644 --- a/src/y2024/resources/preferences_backup.nform +++ b/src/y2024/resources/preferences_backup.nform @@ -1,7 +1,8 @@ Default.int | 0 Preferences/MotionTargets/Pivot/amp_position (deg).double | 70.000000 Preferences/MotionTargets/Pivot/auto_shoot_position (deg).double | 10.000000 -Preferences/MotionTargets/Pivot/intake_position (deg).double | -11.000000 +Preferences/MotionTargets/Pivot/intake_position (deg).double | -17.000000 +Preferences/MotionTargets/Pivot/pass_position (deg).double | 70.000000 Preferences/MotionTargets/Pivot/preclimb_position (deg).double | 83.000000 Preferences/MotionTargets/Pivot/prescore_position (deg).double | 80.000000 Preferences/MotionTargets/Pivot/shoot_position (deg).double | 70.000000 @@ -11,13 +12,14 @@ Preferences/MotionTargets/Pivot/trapscore_position (deg).double | 80.000000 Preferences/MotionTargets/Telescope/amp_position (in).double | 4.000000 Preferences/MotionTargets/Telescope/trapscore_position (in).double | 3.000000 Preferences/MotionTargets/Wrist/amp_position (deg).double | 120.000000 -Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 45.500000 -Preferences/MotionTargets/Wrist/intake_position (deg).double | 47.500000 +Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 50.500000 +Preferences/MotionTargets/Wrist/intake_position (deg).double | 32.000000 +Preferences/MotionTargets/Wrist/pass_position (deg).double | 80.000000 Preferences/MotionTargets/Wrist/preclimb_position (deg).double | -30.000000 Preferences/MotionTargets/Wrist/prescore_position (deg).double | 40.000000 -Preferences/MotionTargets/Wrist/shoot_position (deg).double | 40.600000 +Preferences/MotionTargets/Wrist/shoot_position (deg).double | 34.300000 Preferences/MotionTargets/Wrist/source_position (deg).double | -10.000000 -Preferences/MotionTargets/Wrist/stow_position (deg).double | -30.000000 +Preferences/MotionTargets/Wrist/stow_position (deg).double | -40.000000 Preferences/MotionTargets/Wrist/trapscore_position (deg).double | 80.000000 Preferences/field_points/5p_intake_one_y (in).double | 94.000000 Preferences/field_points/5p_intake_three_deg (deg).double | 0.000000 @@ -45,7 +47,7 @@ SuperStructure/auto/pre_shoot_wait (s).double | 0.100000 SuperStructure/manual_control_deadband.double | 0.150000 SuperStructure/shooting/auto_shooter_height (in).double | 23.000000 SuperStructure/shooting/auto_shooter_x (in).double | 15.000000 -SuperStructure/shooting/teleop_shooter_height (in).double | 39.500000 +SuperStructure/shooting/teleop_shooter_height (in).double | 39.000000 drivetrain/Configs/CurrentLimiting/target_threshold (A).double | 40.000000 drivetrain/Configs/default_brake_mode.bool | false drivetrain/Configs/gear_ratio.double | 16.800000 @@ -65,6 +67,7 @@ drivetrain/module_FL/cancoder_offset (deg).double | 100.400000 drivetrain/module_FR/cancoder_offset (deg).double | 24.400000 drivetrain/percent_max_omega.double | 0.450000 drivetrain/pov_control_speed_.double | 10.000000 +drivetrain/tolerated_skid.double | 1.500000 drivetrain/wheel_radius (in).double | 2.000000 intake/Configs/ramp_time (s).double | 1.000000 intake/Configs/use_ramp_rate.bool | true @@ -99,20 +102,24 @@ shooter/Gains/kF.double | 0.008000 shooter/Gains/kP.double | 0.030000 shooter/shooter_speed (fps).double | 100.000000 shooter/shooter_spin.double | 0.000000 -shooter/shooting_exit_velocity.double | 36.500000 +shooter/shooting_exit_velocity.double | 36.000000 shooter/vFPID/smart_current_limit (A).double | 15.000000 telescope/Gains/kD.double | -0.100000 vision/camera_x_offset (in).double | -7.000000 vision/camera_y_offset (in).double | -10.000000 +vision/can_bus_latency (ms).double | 20.000000 vision/default_is_red_side.bool | true +wrist/CloseDynamicFPID/smart_current_limit (A).double | 15.000000 wrist/Configs/HardLimits/forward (deg).double | 130.000000 -wrist/Configs/HardLimits/reverse (deg).double | -30.000000 +wrist/Configs/HardLimits/reverse (deg).double | -42.000000 wrist/Configs/HardLimits/use_position_limits.bool | true +wrist/Configs/default_brake_mode.bool | false wrist/Configs/invert.bool | true -wrist/DynamicFPID/smart_current_limit (A).double | 40.000000 -wrist/Gains/kD.double | -1.200000 -wrist/Gains/kF.double | 0.070000 -wrist/Gains/kP.double | 0.020000 -wrist/home_offset_wrist (deg).double | -30.000000 +wrist/DynamicFPID/smart_current_limit (A).double | 50.000000 +wrist/Gains/kD.double | -1.650000 +wrist/Gains/kF.double | -0.065000 +wrist/Gains/kP.double | 0.033000 +wrist/cg_offset_wrist (deg).double | 155.000000 +wrist/home_offset_wrist (deg).double | -42.000000 wrist/homing_speed.double | -0.350000 wrist/homing_velocity_tolerance (deg_per_s).double | 0.500000 diff --git a/src/y2024/resources/preferences_cl_backup.nform b/src/y2024/resources/preferences_cl_backup.nform index 30940ac524e8063c847b9bd8cd6e305d2f836b92..8ed13420361841693db45b799f4d5e4125063ab0 100644 GIT binary patch delta 13 Ucmey^&v(hw)=S Date: Tue, 17 Sep 2024 17:01:36 -0700 Subject: [PATCH 03/12] Added autos back --- .../cpp/frc846/other/trajectory_generator.cc | 36 ++--- src/frc846/include/frc846/base/loggable.h | 21 +++ .../frc846/other/trajectory_generator.h | 4 +- .../include/frc846/robot/GenericRobot.h | 4 + src/y2024/cpp/FunkyRobot.cc | 55 ++++---- src/y2024/cpp/autos/drive_auto.cc | 18 +-- src/y2024/cpp/autos/five_piece_auto.cc | 75 +++++----- .../commands/auto_intake_and_shoot_command.cc | 8 +- src/y2024/cpp/commands/basic/shoot_command.cc | 2 +- .../cpp/commands/basic/spin_up_command.cc | 13 +- .../complex/close_drive_amp_command.cc | 6 +- .../cpp/commands/complex/stow_zero_action.cc | 27 ++++ .../cpp/commands/complex/super_amp_command.cc | 4 +- .../cpp/commands/follow_trajectory_command.cc | 42 +++--- src/y2024/cpp/field.cc | 30 +--- src/y2024/cpp/subsystems/abstract/vision.cc | 4 +- src/y2024/include/FunkyRobot.h | 7 + .../commands/auto_intake_and_shoot_command.h | 5 +- .../include/commands/basic/spin_up_command.h | 5 + .../commands/complex/stow_zero_action.h | 13 ++ src/y2024/include/field.h | 130 +++++++++--------- src/y2024/include/subsystems/hardware/wrist.h | 8 +- src/y2024/resources/preferences_backup.nform | 16 ++- 23 files changed, 296 insertions(+), 237 deletions(-) create mode 100644 src/y2024/cpp/commands/complex/stow_zero_action.cc create mode 100644 src/y2024/include/commands/complex/stow_zero_action.h diff --git a/src/frc846/cpp/frc846/other/trajectory_generator.cc b/src/frc846/cpp/frc846/other/trajectory_generator.cc index f0cb89c..4b7be61 100644 --- a/src/frc846/cpp/frc846/other/trajectory_generator.cc +++ b/src/frc846/cpp/frc846/other/trajectory_generator.cc @@ -54,36 +54,36 @@ Trajectory GenerateTrajectory( for (unsigned int i = input_points.size() - 1; i > 0; --i) { auto interpolated_points = InterpolatePoints( - input_points[i].pos.point, input_points[i - 1].pos.point, - input_points[i].pos.bearing, input_points[i - 1].pos.bearing, cut); + input_points[i].point, input_points[i - 1].point, + input_points[i].bearing, input_points[i - 1].bearing, cut); interpolated_points.erase(interpolated_points.begin()); - trajectory.push_back({input_points[i].pos}); + trajectory.push_back({input_points[i]}); for (auto point : interpolated_points) { point.velocity[0] = robot_max_v; trajectory.push_back({point}); } } - trajectory.push_back({input_points[0].pos}); + trajectory.push_back({input_points[0]}); for (unsigned int i = 1; i < trajectory.size(); ++i) { auto delta_pos = - (trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude(); + (trajectory[i].point - trajectory[i - 1].point).magnitude(); // v₂² = v₁² + 2aΔx // 2aΔx = v₂² - v₁² // a = (v₂² - v₁²) / (2Δx) auto deceleration = - (units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) - - units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) / + (units::math::pow<2>(trajectory[i].velocity.magnitude()) - + units::math::pow<2>(trajectory[i - 1].velocity.magnitude())) / (2 * delta_pos); if (deceleration > robot_max_deceleration) { // v₂² = v₁² + 2aΔx // v₂² = sqrt(v₁² + 2aΔx) - trajectory[i].pos.velocity[0] = units::math::sqrt( - units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) + + trajectory[i].velocity[0] = units::math::sqrt( + units::math::pow<2>(trajectory[i - 1].velocity.magnitude()) + 2 * robot_max_deceleration * delta_pos); } } @@ -92,20 +92,20 @@ Trajectory GenerateTrajectory( for (unsigned int i = 1; i < trajectory.size(); ++i) { auto delta_pos = - (trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude(); + (trajectory[i].point - trajectory[i - 1].point).magnitude(); // v₂² = v₁² + 2aΔx // 2aΔx = v₂² - v₁² // a = (v₂² - v₁²) / (2Δx) auto acceleration = - (units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) - - units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) / + (units::math::pow<2>(trajectory[i].velocity.magnitude()) - + units::math::pow<2>(trajectory[i - 1].velocity.magnitude())) / (2 * delta_pos); if (acceleration > robot_max_acceleration) { // v₂² = v₁² + 2aΔx // v₂² = sqrt(v₁² + 2aΔx) - trajectory[i].pos.velocity[0] = units::math::sqrt( - units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) + + trajectory[i].velocity[0] = units::math::sqrt( + units::math::pow<2>(trajectory[i - 1].velocity.magnitude()) + 2 * robot_max_acceleration * delta_pos); // trajectory[i].v = @@ -117,10 +117,10 @@ Trajectory GenerateTrajectory( // If any point has 0 speed, just set it to the previous speed. for (unsigned int i = 0; i < trajectory.size(); ++i) { - if (trajectory[i].pos.velocity.magnitude() == 0_fps) { - trajectory[i].pos.velocity.magnitude() = - i == 0 ? trajectory[1].pos.velocity.magnitude() - : trajectory[i - 1].pos.velocity.magnitude(); + if (trajectory[i].velocity.magnitude() == 0_fps) { + trajectory[i].velocity.magnitude() = + i == 0 ? trajectory[1].velocity.magnitude() + : trajectory[i - 1].velocity.magnitude(); } } diff --git a/src/frc846/include/frc846/base/loggable.h b/src/frc846/include/frc846/base/loggable.h index e394d08..aee0d46 100644 --- a/src/frc846/include/frc846/base/loggable.h +++ b/src/frc846/include/frc846/base/loggable.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -43,6 +44,26 @@ class Loggable { error_count_++; } + // Puts a double entry on the smart dashboard. + void Graph(std::string key, double value) { + frc::SmartDashboard::PutNumber(name_ + "/" + key, value); + } + + // Puts an integer entry on the smart dashboard. + void Graph(std::string key, int value) { + frc::SmartDashboard::PutNumber(name_ + "/" + key, value); + } + + // Puts a boolean entry on the smart dashboard. + void Graph(std::string key, bool value) { + frc::SmartDashboard::PutBoolean(name_ + "/" + key, value); + } + + // Puts a string entry on the smart dashboard. + void Graph(std::string key, std::string value) { + frc::SmartDashboard::PutString(name_ + "/" + key, value); + } + static unsigned int GetWarnCount(); static unsigned int GetErrorCount(); diff --git a/src/frc846/include/frc846/other/trajectory_generator.h b/src/frc846/include/frc846/other/trajectory_generator.h index 4e2447d..564febc 100644 --- a/src/frc846/include/frc846/other/trajectory_generator.h +++ b/src/frc846/include/frc846/other/trajectory_generator.h @@ -7,9 +7,7 @@ namespace frc846 { -struct Waypoint { - frc846::math::FieldPoint pos; -}; +using Waypoint = frc846::math::FieldPoint; using Trajectory = std::vector; diff --git a/src/frc846/include/frc846/robot/GenericRobot.h b/src/frc846/include/frc846/robot/GenericRobot.h index 80da028..a8b09b3 100644 --- a/src/frc846/include/frc846/robot/GenericRobot.h +++ b/src/frc846/include/frc846/robot/GenericRobot.h @@ -29,6 +29,10 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable { virtual void OnInitialize() = 0; + virtual void OnDisable() = 0; + + virtual void OnPeriodic() = 0; + virtual void InitTeleop() = 0; virtual void InitTest() = 0; diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index 19000a0..01c12eb 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -13,6 +13,7 @@ #include "commands/basic/deploy_intake_command.h" #include "commands/basic/shoot_command.h" #include "commands/basic/spin_up_command.h" +#include "commands/complex/stow_zero_action.h" #include "commands/follow_trajectory_command.h" #include "commands/teleop/bracer_command.h" #include "commands/teleop/drive_command.h" @@ -59,42 +60,28 @@ void FunkyRobot::OnInitialize() { container_.wrist_.Brake(); })); - frc2::Trigger on_coast_trigger{[&] { return coasting_switch_.Get(); }}; - - on_coast_trigger.OnTrue(frc2::InstantCommand([&] { - container_.pivot_.Coast(); - container_.telescope_.Coast(); - }) - .WithTimeout(1_s) - .AndThen(frc2::WaitCommand(7_s).ToPtr()) - .AndThen(frc2::InstantCommand([&] { - container_.pivot_.Brake(); - container_.telescope_.Brake(); - }).ToPtr())); - - frc2::Trigger homing_trigger{[&] { return homing_switch_.Get(); }}; - - homing_trigger.OnTrue(frc2::InstantCommand([&] { - container_.wrist_.ZeroSubsystem(); - container_.pivot_.ZeroSubsystem(); - container_.telescope_.ZeroSubsystem(); - }).ToPtr()); - AddAutos(five_piece_auto_red.get(), {five_piece_auto_blue.get(), one_piece_auto_0.get(), one_piece_auto_1.get(), one_piece_auto_2.get(), one_piece_auto_3.get(), drive_auto_.get()}); } +void FunkyRobot::OnDisable() { + container_.pivot_.Brake(); + container_.wrist_.Brake(); + container_.telescope_.Brake(); +} + void FunkyRobot::InitTeleop() { container_.pivot_.Brake(); container_.telescope_.Brake(); - container_.wrist_.Brake(); + container_.wrist_.Coast(); container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); container_.control_input_.SetDefaultCommand( OperatorControlCommand{container_}); - container_.super_structure_.SetDefaultCommand(StowCommand{container_}); + container_.super_structure_.SetDefaultCommand( + StowZeroActionCommand{container_}); container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_}); container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); container_.bracer_.SetDefaultCommand(BracerCommand{container_}); @@ -102,4 +89,26 @@ void FunkyRobot::InitTeleop() { ControlTriggerInitializer::InitTeleopTriggers(container_); } +void FunkyRobot::OnPeriodic() { + Graph("homing_switch", homing_switch_.Get()); + Graph("coasting_switch", coasting_switch_.Get()); + + if (homing_switch_.Get()) { + container_.super_structure_.ZeroSubsystem(); + } + + if (coast_counter_ >= 1) { + coast_counter_--; + } else if (coast_counter_ == 0) { + container_.pivot_.Brake(); + container_.wrist_.Brake(); + container_.telescope_.Brake(); + } else if (coasting_switch_.Get()) { + container_.pivot_.Coast(); + container_.wrist_.Coast(); + container_.telescope_.Coast(); + coast_counter_ = start_coast_counter_.value(); + } +} + void FunkyRobot::InitTest() {} diff --git a/src/y2024/cpp/autos/drive_auto.cc b/src/y2024/cpp/autos/drive_auto.cc index 5c24688..10da722 100644 --- a/src/y2024/cpp/autos/drive_auto.cc +++ b/src/y2024/cpp/autos/drive_auto.cc @@ -16,16 +16,12 @@ DriveAuto::DriveAuto(RobotContainer& container) container, "drive_auto", frc2::SequentialCommandGroup{ - // frc2::InstantCommand{[&] { - // auto pose_ = field::points::kTestingOrigin(false); - // container.drivetrain_.SetPoint(pose_.point); - // container.drivetrain_.SetBearing(pose_.bearing); - // }}, - // FollowTrajectoryCommand{ - // container, - // { - // {field::points::kTestingPoint(false), 0_fps}, - // }, - // } + frc2::InstantCommand{[&] { + auto pose_ = field::points.kTestingOrigin(); + container.drivetrain_.SetPoint(pose_.point); + container.drivetrain_.SetBearing(pose_.bearing); + }}, + FollowTrajectoryCommand{container, + {field::points.kTestingPoint()}} }} {} \ No newline at end of file diff --git a/src/y2024/cpp/autos/five_piece_auto.cc b/src/y2024/cpp/autos/five_piece_auto.cc index c273c04..4c3d15b 100644 --- a/src/y2024/cpp/autos/five_piece_auto.cc +++ b/src/y2024/cpp/autos/five_piece_auto.cc @@ -4,13 +4,17 @@ #include #include #include +#include +#include #include "commands/auto_intake_and_shoot_command.h" #include "commands/basic/auto_shoot_command.h" #include "commands/basic/prepare_auto_shoot_command.h" +#include "commands/basic/spin_up_command.h" +#include "commands/basic/wrist_zero_command.h" #include "commands/follow_trajectory_command.h" +#include "commands/teleop/stow_command.h" #include "field.h" -#include "frc2/command/WaitCommand.h" FivePieceAuto::FivePieceAuto(RobotContainer& container, bool should_flip) : frc846::robot::GenericCommandGroup intake_path, + std::vector shoot_path) : frc846::robot::GenericCommandGroup{ @@ -22,10 +22,10 @@ AutoIntakeAndShootCommand::AutoIntakeAndShootCommand( frc2::SequentialCommandGroup{ AutoDeployIntakeCommand{container}, - FollowTrajectoryCommand{container, {mid_point, intake_point}}, + FollowTrajectoryCommand{container, intake_path}, frc2::ParallelDeadlineGroup{ frc2::SequentialCommandGroup{ - FollowTrajectoryCommand{container, {shoot_point}}, + FollowTrajectoryCommand{container, shoot_path}, frc2::WaitCommand( container.super_structure_.pre_shoot_wait_.value())}, PrepareAutoShootCommand{container}}, diff --git a/src/y2024/cpp/commands/basic/shoot_command.cc b/src/y2024/cpp/commands/basic/shoot_command.cc index ad2e698..f85b2fb 100644 --- a/src/y2024/cpp/commands/basic/shoot_command.cc +++ b/src/y2024/cpp/commands/basic/shoot_command.cc @@ -24,5 +24,5 @@ bool ShootCommand::IsFinished() { num_loops_ = 0; } - return num_loops_ > 20; + return num_loops_ > 70; } \ No newline at end of file diff --git a/src/y2024/cpp/commands/basic/spin_up_command.cc b/src/y2024/cpp/commands/basic/spin_up_command.cc index 70a42a4..a5bf2fb 100644 --- a/src/y2024/cpp/commands/basic/spin_up_command.cc +++ b/src/y2024/cpp/commands/basic/spin_up_command.cc @@ -6,10 +6,21 @@ SpinUpCommand::SpinUpCommand(RobotContainer& container) AddRequirements({&container_.shooter_}); } -void SpinUpCommand::OnInit() {} +void SpinUpCommand::OnInit() { + has_spinned_up_ = false; + start_time = frc846::wpilib::CurrentFPGATime(); +} void SpinUpCommand::Periodic() { container_.shooter_.SetTarget({ShooterState::kRun}); + if (container_.shooter_.GetReadings().error_percent < + container_.shooter_.shooter_speed_tolerance_.value()) { + if (has_spinned_up_ == false) { + Log("Shooter took {}s to spin up.", + frc846::wpilib::CurrentFPGATime() - start_time); + has_spinned_up_ = true; + } + } } void SpinUpCommand::OnEnd(bool interrupted) {} diff --git a/src/y2024/cpp/commands/complex/close_drive_amp_command.cc b/src/y2024/cpp/commands/complex/close_drive_amp_command.cc index c95bd0d..e6bf87f 100644 --- a/src/y2024/cpp/commands/complex/close_drive_amp_command.cc +++ b/src/y2024/cpp/commands/complex/close_drive_amp_command.cc @@ -11,7 +11,7 @@ CloseDriveAmpCommand::CloseDriveAmpCommand(RobotContainer& container) void CloseDriveAmpCommand::OnInit() {} void CloseDriveAmpCommand::Periodic() { - auto amp_point = field::points::kAmpNoFlip(); + auto amp_point = field::points.kAmpNoFlip(); DrivetrainTarget drivetrain_target; drivetrain_target.rotation = DrivetrainRotationPosition(amp_point.bearing); @@ -19,7 +19,7 @@ void CloseDriveAmpCommand::Periodic() { drivetrain_target.control = kClosedLoop; auto amp_drive_dist = - (field::points::kPreAmpNoFlip().point - amp_point.point).magnitude(); + (field::points.kPreAmpNoFlip().point - amp_point.point).magnitude(); auto max_speed = container_.drivetrain_.close_drive_amp_max_speed_.value(); @@ -41,7 +41,7 @@ void CloseDriveAmpCommand::Periodic() { void CloseDriveAmpCommand::OnEnd(bool interrupted) {} bool CloseDriveAmpCommand::IsFinished() { - return (field::points::kAmpNoFlip().point - + return (field::points.kAmpNoFlip().point - container_.drivetrain_.GetReadings().pose.point) .magnitude() <= 2_in; } \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/stow_zero_action.cc b/src/y2024/cpp/commands/complex/stow_zero_action.cc new file mode 100644 index 0000000..22d77bb --- /dev/null +++ b/src/y2024/cpp/commands/complex/stow_zero_action.cc @@ -0,0 +1,27 @@ +#include "commands/complex/stow_zero_action.h" + +#include +#include +#include +#include + +#include "commands/basic/wrist_zero_command.h" +#include "commands/teleop/stow_command.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +StowZeroActionCommand::StowZeroActionCommand(RobotContainer& container) + : frc846::robot::GenericCommandGroup{ + container, "super_amp_command", + frc2::SequentialCommandGroup{ + frc2::ParallelDeadlineGroup{ + frc2::WaitUntilCommand{[&] { + return container_.pivot_.WithinTolerance( + container_.super_structure_.getStowSetpoint().pivot); + }}, + StowCommand{container}}, + WristZeroCommand{container}, + StowCommand{container} /* Otherwise, this command will be + rescheduled -> infinite loop */ + }} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/super_amp_command.cc b/src/y2024/cpp/commands/complex/super_amp_command.cc index e651783..afb335c 100644 --- a/src/y2024/cpp/commands/complex/super_amp_command.cc +++ b/src/y2024/cpp/commands/complex/super_amp_command.cc @@ -26,7 +26,7 @@ SuperAmpCommand::SuperAmpCommand(RobotContainer& container, bool is_red_side) frc2::ParallelDeadlineGroup{ frc2::SequentialCommandGroup{ FollowTrajectoryCommand{ - container, {{field::points::kPreAmpNoFlip()}}}, + container, {{field::points.kPreAmpNoFlip()}}}, CloseDriveAmpCommand{container}, frc2::ParallelDeadlineGroup{ frc2::SequentialCommandGroup{ @@ -37,4 +37,4 @@ SuperAmpCommand::SuperAmpCommand(RobotContainer& container, bool is_red_side) }, AmpCommand{container}}, FollowTrajectoryCommand{container, - {{field::points::kPreAmpNoFlip()}}}}} {} \ No newline at end of file + {{field::points.kPreAmpNoFlip()}}}}} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/follow_trajectory_command.cc b/src/y2024/cpp/commands/follow_trajectory_command.cc index 4f33184..64eee5f 100644 --- a/src/y2024/cpp/commands/follow_trajectory_command.cc +++ b/src/y2024/cpp/commands/follow_trajectory_command.cc @@ -12,8 +12,7 @@ FollowTrajectoryCommand::FollowTrajectoryCommand( void FollowTrajectoryCommand::OnInit() { Log("Starting Trajectory"); for (frc846::Waypoint i : input_points_) { - Log("points x{} y{} bearing {}", i.pos.point[0], i.pos.point[1], - i.pos.bearing); + Log("points x{} y{} bearing {}", i.point[0], i.point[1], i.bearing); } Log("initial pose x{}, y{}, bearing {}", container_.drivetrain_.GetReadings().pose.point[0], @@ -38,8 +37,7 @@ void FollowTrajectoryCommand::OnInit() { } else { double extrapolation_distance = container_.drivetrain_.extrapolation_distance_.value() / 1_ft; - current_extrapolated_point_ = - trajectory_[1].pos.point - trajectory_[0].pos.point; + current_extrapolated_point_ = trajectory_[1].point - trajectory_[0].point; current_extrapolated_point_ = current_extrapolated_point_.unit() * extrapolation_distance; } @@ -65,8 +63,8 @@ void FollowTrajectoryCommand::Periodic() { double extrapolation_distance = container_.drivetrain_.extrapolation_distance_.value() / 1_ft; - current_extrapolated_point_ = trajectory_[target_idx_].pos.point - - trajectory_[target_idx_ - 1].pos.point; + current_extrapolated_point_ = + trajectory_[target_idx_].point - trajectory_[target_idx_ - 1].point; current_extrapolated_point_ = current_extrapolated_point_.unit() * extrapolation_distance; } @@ -76,13 +74,13 @@ void FollowTrajectoryCommand::Periodic() { auto direction = units::math::atan2(delta_pos[1], delta_pos[0]); DrivetrainTarget target; - target.v_x = trajectory_[target_idx_].pos.velocity.magnitude() * + target.v_x = trajectory_[target_idx_].velocity.magnitude() * units::math::cos(direction); - target.v_y = trajectory_[target_idx_].pos.velocity.magnitude() * + target.v_y = trajectory_[target_idx_].velocity.magnitude() * units::math::sin(direction); target.translation_reference = DrivetrainTranslationReference::kField; target.rotation = - DrivetrainRotationPosition(trajectory_[target_idx_].pos.bearing); + DrivetrainRotationPosition(trajectory_[target_idx_].bearing); target.control = kClosedLoop; container_.drivetrain_.SetTarget(target); @@ -103,8 +101,8 @@ bool FollowTrajectoryCommand::HasCrossedWaypoint( // fmt::print( // "\ncurrent_waypoint x {}, current_waypoint y {}, prev_waypoint x {} y " // "{}, pos x{} y{}, extrap x{}, y{}\n", - // current_waypoint.pos.point.x, current_waypoint.pos.point.y, - // prev_waypoint.pos.point.x, prev_waypoint.pos.point.y, pos.x, pos.y, + // current_waypoint.point.x, current_waypoint.point.y, + // prev_waypoint.point.x, prev_waypoint.point.y, pos.x, pos.y, // extrapolated_point.x, extrapolated_point.y); auto d = [](frc846::math::VectorND target, @@ -121,22 +119,20 @@ bool FollowTrajectoryCommand::HasCrossedWaypoint( return 0; }; - auto delta_y = current_waypoint.pos.point[1] - prev_waypoint.pos.point[1]; - auto delta_x = current_waypoint.pos.point[0] - prev_waypoint.pos.point[0]; + auto delta_y = current_waypoint.point[1] - prev_waypoint.point[1]; + auto delta_x = current_waypoint.point[0] - prev_waypoint.point[0]; auto theta = units::math::atan(-delta_x / delta_y); double cos_theta = units::math::cos(theta); double sin_theta = units::math::sin(theta); - auto p1 = - current_waypoint.pos.point - frc846::math::VectorND{ - 1_ft * cos_theta, - 1_ft * sin_theta, - }; - auto p2 = - current_waypoint.pos.point + frc846::math::VectorND{ - 1_ft * cos_theta, - 1_ft * sin_theta, - }; + auto p1 = current_waypoint.point - frc846::math::VectorND{ + 1_ft * cos_theta, + 1_ft * sin_theta, + }; + auto p2 = current_waypoint.point + frc846::math::VectorND{ + 1_ft * cos_theta, + 1_ft * sin_theta, + }; return d(pos, p1, p2) == d(extrapolated_point, p1, p2); } \ No newline at end of file diff --git a/src/y2024/cpp/field.cc b/src/y2024/cpp/field.cc index 0af1fe3..9f40fed 100644 --- a/src/y2024/cpp/field.cc +++ b/src/y2024/cpp/field.cc @@ -1,31 +1,3 @@ #include "field.h" -// frc846::util::FieldPoint field::points::testing_origin{"testing_origin", -// 0_in, -// 0_in, 0_deg}; -// frc846::util::FieldPoint field::points::testing_point{"testing_point", 0_in, -// 100_in, 0_deg}; - -// frc846::util::FieldPoint field::points::five_piece_origin{"5p_origin", -// 217.5_in, -// 49_in, 0_deg}; -// frc846::util::FieldPoint field::points::five_piece_intake_one{ -// "5p_intake_one", 217.5_in, 112_in, 0_deg}; -// frc846::util::FieldPoint field::points::five_piece_mid_one{"5p_mid_one", -// 292_in, -// 324_in, 20_deg}; -// frc846::util::FieldPoint field::points::five_piece_shoot_one{ -// "5p_shoot_one", 217.5_in, 112_in, 0_deg}; -// frc846::util::FieldPoint field::points::five_piece_intake_two{ -// "5p_intake_two", 292_in, 324_in, 20_deg}; -// frc846::util::FieldPoint field::points::five_piece_mid_two{"5p_mid_two", -// 292_in, -// 324_in, 20_deg}; -// frc846::util::FieldPoint field::points::five_piece_shoot_two{ -// "5p_shoot_two", 217.5_in, 53_in, 0_deg}; -// frc846::util::FieldPoint field::points::five_piece_intake_three{ -// "5p_intake_three", 160.5_in, 112_in, -40_deg}; -// frc846::util::FieldPoint field::points::five_piece_mid_three{ -// "5p_mid_three", 292_in, 324_in, 20_deg}; -// frc846::util::FieldPoint field::points::five_piece_shoot_three{ -// "5p_shoot_three", 217.5_in, 53_in, 0_deg}; \ No newline at end of file +field::points_cls field::points{}; \ No newline at end of file diff --git a/src/y2024/cpp/subsystems/abstract/vision.cc b/src/y2024/cpp/subsystems/abstract/vision.cc index 47691fc..1994674 100644 --- a/src/y2024/cpp/subsystems/abstract/vision.cc +++ b/src/y2024/cpp/subsystems/abstract/vision.cc @@ -120,11 +120,11 @@ VisionReadings VisionSubsystem::ReadFromHardware() { robot_x += can_bus_latency_.value() * velocity_x + x_correction; newReadings.est_dist_from_speaker_x = - robot_x - field::points::kSpeaker( + robot_x - field::points.kSpeaker( !frc846::util::ShareTables::GetBoolean("is_red_side"))[0]; newReadings.est_dist_from_speaker_y = - robot_y - field::points::kSpeaker( + robot_y - field::points.kSpeaker( !frc846::util::ShareTables::GetBoolean("is_red_side"))[1]; auto point_target = frc846::math::VectorND{ diff --git a/src/y2024/include/FunkyRobot.h b/src/y2024/include/FunkyRobot.h index ef017d0..8ac8336 100644 --- a/src/y2024/include/FunkyRobot.h +++ b/src/y2024/include/FunkyRobot.h @@ -16,6 +16,10 @@ class FunkyRobot : public frc846::robot::GenericRobot { void OnInitialize() override; + void OnDisable() override; + + void OnPeriodic() override; + void InitTeleop() override; void InitTest() override; @@ -43,5 +47,8 @@ class FunkyRobot : public frc846::robot::GenericRobot { frc::DigitalInput homing_switch_{0}; frc::DigitalInput coasting_switch_{1}; + frc846::ntinf::Pref start_coast_counter_{*this, "start_coast_counter", + 500}; + int coast_counter_ = 0; }; diff --git a/src/y2024/include/commands/auto_intake_and_shoot_command.h b/src/y2024/include/commands/auto_intake_and_shoot_command.h index 5fec77f..0064777 100644 --- a/src/y2024/include/commands/auto_intake_and_shoot_command.h +++ b/src/y2024/include/commands/auto_intake_and_shoot_command.h @@ -11,7 +11,6 @@ class AutoIntakeAndShootCommand frc2::SequentialCommandGroup> { public: AutoIntakeAndShootCommand(RobotContainer& container, - frc846::Waypoint intake_point, - frc846::Waypoint mid_point, - frc846::Waypoint shoot_point); + std::vector intake_path, + std::vector shoot_path); }; diff --git a/src/y2024/include/commands/basic/spin_up_command.h b/src/y2024/include/commands/basic/spin_up_command.h index 7bb425b..9564b20 100644 --- a/src/y2024/include/commands/basic/spin_up_command.h +++ b/src/y2024/include/commands/basic/spin_up_command.h @@ -15,4 +15,9 @@ class SpinUpCommand void OnEnd(bool interrupted) override; bool IsFinished() override; + + private: + bool has_spinned_up_; + + units::second_t start_time; }; diff --git a/src/y2024/include/commands/complex/stow_zero_action.h b/src/y2024/include/commands/complex/stow_zero_action.h new file mode 100644 index 0000000..1674aff --- /dev/null +++ b/src/y2024/include/commands/complex/stow_zero_action.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#include "frc846/robot/GenericCommand.h" +#include "subsystems/robot_container.h" + +class StowZeroActionCommand + : public frc846::robot::GenericCommandGroup< + RobotContainer, StowZeroActionCommand, frc2::SequentialCommandGroup> { + public: + StowZeroActionCommand(RobotContainer& container); +}; diff --git a/src/y2024/include/field.h b/src/y2024/include/field.h index 0c44f65..f90213c 100644 --- a/src/y2024/include/field.h +++ b/src/y2024/include/field.h @@ -6,13 +6,12 @@ // Field has blue alliance far right corner as origin struct field { - struct points { - static frc846::math::FieldPoint Origin() { + struct points_cls { + frc846::math::FieldPoint Origin() { return {{0_in, 0_in}, 0_deg, {0_fps, 0_fps}}; } - static frc846::math::VectorND kSpeaker( - bool flip = false) { + frc846::math::VectorND kSpeaker(bool flip = false) { if (!flip) { return {217.5_in, -4_in}; } else { @@ -20,74 +19,71 @@ struct field { } } - static frc846::math::FieldPoint kAmpNoFlip() { + frc846::math::FieldPoint kAmpNoFlip() { return {{0_in, 0_in}, 90_deg, {0_fps, 0_fps}}; } - static frc846::math::FieldPoint kPreAmpNoFlip() { + frc846::math::FieldPoint kPreAmpNoFlip() { return {{-2_ft, 0_in}, 90_deg, {0_fps, 0_fps}}; } - // // TESTING - // static frc846::util::FieldPoint testing_origin; - // static frc846::util::Position kTestingOrigin(bool should_flip) { - // return testing_origin.flip(should_flip); - // }; - - // static frc846::util::FieldPoint testing_point; - // static frc846::util::Position kTestingPoint(bool should_flip) { - // return testing_point.flip(should_flip); - // }; - - // // FIVE PIECE AUTO - // static frc846::util::FieldPoint five_piece_origin; - // static frc846::util::Position kFivePieceOrigin(bool should_flip) { - // return five_piece_origin.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_intake_one; - // static frc846::util::Position kFivePieceIntakeOne(bool should_flip) { - // return five_piece_intake_one.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_mid_one; - // static frc846::util::Position kFivePieceMidOne(bool should_flip) { - // return five_piece_mid_one.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_shoot_one; - // static frc846::util::Position kFivePieceShootOne(bool should_flip) { - // return five_piece_shoot_one.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_intake_two; - // static frc846::util::Position kFivePieceIntakeTwo(bool should_flip) { - // return five_piece_intake_two.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_mid_two; - // static frc846::util::Position kFivePieceMidTwo(bool should_flip) { - // return five_piece_mid_two.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_shoot_two; - // static frc846::util::Position kFivePieceShootTwo(bool should_flip) { - // return five_piece_shoot_two.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_intake_three; - // static frc846::util::Position kFivePieceIntakeThree(bool should_flip) { - // return five_piece_intake_three.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_mid_three; - // static frc846::util::Position kFivePieceMidThree(bool should_flip) { - // return five_piece_mid_three.flip(should_flip, true); - // }; - - // static frc846::util::FieldPoint five_piece_shoot_three; - // static frc846::util::Position kFivePieceShootThree(bool should_flip) { - // return five_piece_shoot_three.flip(should_flip, true); - // }; + // DRIVE AUTO - TEST POINTS + + frc846::math::FieldPointPreference testing_origin{"testing_origin", + Origin()}; + frc846::math::FieldPoint kTestingOrigin() { return testing_origin.get(); }; + + frc846::math::FieldPointPreference testing_point{ + "testing_point", {{0_in, 120_in}, 0_deg, {0_fps, 0_fps}}}; + frc846::math::FieldPoint kTestingPoint() { return testing_point.get(); }; + + // FIVE PIECE AUTO + frc846::base::Loggable five_piece_loggable{"five_piece_auto"}; + + frc846::ntinf::Pref pre_point_amt{five_piece_loggable, + "pre_point_amt", 2_ft}; + frc846::math::FieldPoint pre_point(frc846::math::FieldPoint pnt) { + return {{pnt.point[0], pnt.point[1] - pre_point_amt.value()}, + pnt.bearing, + {0_fps, 15_fps}}; + } + + frc846::math::FieldPointPreference origin_point{ + "five_piece_origin", {{217.5_in, 49_in}, 0_deg, {0_fps, 0_fps}}}; + frc846::math::FieldPoint kFivePieceOrigin(bool should_flip) { + return origin_point.get().mirrorOnlyY(should_flip); + } + + frc846::math::FieldPointPreference intake_one{ + "five_piece_intake_one", {{217.5_in, 112_in}, 0_deg, {0_fps, 0_fps}}}; + std::vector intake_one_path(bool should_flip) { + auto base_point = intake_one.get(); + return {pre_point(base_point).mirrorOnlyY(should_flip), + base_point.mirrorOnlyY(should_flip)}; + } + + frc846::math::FieldPointPreference intake_two{ + "five_piece_intake_two", {{160.5_in, 112_in}, 0_deg, {0_fps, 0_fps}}}; + std::vector intake_two_path(bool should_flip) { + auto base_point = intake_two.get(); + return {pre_point(base_point).mirrorOnlyY(should_flip), + base_point.mirrorOnlyY(should_flip)}; + } + + frc846::math::FieldPointPreference intake_three{ + "five_piece_intake_three", {{274.5_in, 112_in}, 0_deg, {0_fps, 0_fps}}}; + std::vector intake_three_path(bool should_flip) { + auto base_point = intake_three.get(); + return {pre_point(base_point).mirrorOnlyY(should_flip), + base_point.mirrorOnlyY(should_flip)}; + } + + frc846::math::FieldPointPreference finish_pt{ + "five_piece_finish", {{274.5_in, 180_in}, 0_deg, {0_fps, 0_fps}}}; + frc846::math::FieldPoint kFivePieceFinish(bool should_flip) { + return finish_pt.get().mirrorOnlyY(should_flip); + } }; + + static points_cls points; }; diff --git a/src/y2024/include/subsystems/hardware/wrist.h b/src/y2024/include/subsystems/hardware/wrist.h index 34848b6..bbc1d24 100644 --- a/src/y2024/include/subsystems/hardware/wrist.h +++ b/src/y2024/include/subsystems/hardware/wrist.h @@ -40,13 +40,13 @@ class WristSubsystem } void Coast() { - // if (auto esc = wrist_esc_.getESC()) - // esc->SetIdleMode(rev::CANSparkBase::IdleMode::kCoast); + if (auto esc = wrist_esc_.getESC()) + esc->SetIdleMode(rev::CANSparkBase::IdleMode::kCoast); } void Brake() { - // if (auto esc = wrist_esc_.getESC()) - // esc->SetIdleMode(rev::CANSparkBase::IdleMode::kBrake); + if (auto esc = wrist_esc_.getESC()) + esc->SetIdleMode(rev::CANSparkBase::IdleMode::kBrake); } void DeZero() { hasZeroed = false; } diff --git a/src/y2024/resources/preferences_backup.nform b/src/y2024/resources/preferences_backup.nform index f3c18b4..572fac9 100644 --- a/src/y2024/resources/preferences_backup.nform +++ b/src/y2024/resources/preferences_backup.nform @@ -11,13 +11,13 @@ Preferences/MotionTargets/Pivot/stow_position (deg).double | -17.000000 Preferences/MotionTargets/Pivot/trapscore_position (deg).double | 80.000000 Preferences/MotionTargets/Telescope/amp_position (in).double | 4.000000 Preferences/MotionTargets/Telescope/trapscore_position (in).double | 3.000000 -Preferences/MotionTargets/Wrist/amp_position (deg).double | 120.000000 +Preferences/MotionTargets/Wrist/amp_position (deg).double | 100.000000 Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 50.500000 Preferences/MotionTargets/Wrist/intake_position (deg).double | 32.000000 Preferences/MotionTargets/Wrist/pass_position (deg).double | 80.000000 Preferences/MotionTargets/Wrist/preclimb_position (deg).double | -30.000000 Preferences/MotionTargets/Wrist/prescore_position (deg).double | 40.000000 -Preferences/MotionTargets/Wrist/shoot_position (deg).double | 34.300000 +Preferences/MotionTargets/Wrist/shoot_position (deg).double | 36.500000 Preferences/MotionTargets/Wrist/source_position (deg).double | -10.000000 Preferences/MotionTargets/Wrist/stow_position (deg).double | -40.000000 Preferences/MotionTargets/Wrist/trapscore_position (deg).double | 80.000000 @@ -54,13 +54,14 @@ drivetrain/Configs/gear_ratio.double | 16.800000 drivetrain/Gains/kF.double | 0.000000 drivetrain/Gains/kP.double | 0.120000 drivetrain/auto_max_speed (fps).double | 15.000000 -drivetrain/bearing_gains/d.double | -17.000000 -drivetrain/bearing_gains/p.double | 5.000000 +drivetrain/bearing_gains/d.double | -13.500000 +drivetrain/bearing_gains/p.double | 2.000000 drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A).double | 240.000000 drivetrain/drive_esc/Gains/kF.double | 0.205000 drivetrain/drive_esc/Gains/kP.double | 0.010000 drivetrain/driver_speed_multiplier.double | 1.000000 drivetrain/max_acceleration (fps_sq).double | 28.000000 +drivetrain/max_speed (fps).double | 2.400000 drivetrain/module_BL/cancoder_offset (deg).double | -81.300000 drivetrain/module_BR/cancoder_offset (deg).double | -173.900000 drivetrain/module_FL/cancoder_offset (deg).double | 100.400000 @@ -99,11 +100,12 @@ robot_container/init_telescope.bool | true robot_container/init_wrist.bool | true shooter/Configs/invert.bool | true shooter/Gains/kF.double | 0.008000 -shooter/Gains/kP.double | 0.030000 +shooter/Gains/kP.double | 0.075000 shooter/shooter_speed (fps).double | 100.000000 +shooter/shooter_speed_tolerance.double | 0.030000 shooter/shooter_spin.double | 0.000000 -shooter/shooting_exit_velocity.double | 36.000000 -shooter/vFPID/smart_current_limit (A).double | 15.000000 +shooter/shooting_exit_velocity.double | 45.000000 +shooter/vFPID/smart_current_limit (A).double | 90.000000 telescope/Gains/kD.double | -0.100000 vision/camera_x_offset (in).double | -7.000000 vision/camera_y_offset (in).double | -10.000000 From cf9b5b92ed7bd2a37f08eecd61b2d41d2517848a Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Thu, 19 Sep 2024 08:13:55 -0700 Subject: [PATCH 04/12] FollowTrajectory and TrajectoryGenerator update --- .../cpp/frc846/other/trajectory_generator.cc | 130 ----------------- .../swerve/follow_trajectory_command.cc | 88 +++++++++++ .../swerve/waypt_traversal_calculator.cc | 64 ++++++++ src/frc846/include/frc846/math/vectors.h | 6 +- .../frc846/other/trajectory_generator.h | 25 ---- .../include/frc846/robot/GenericCommand.h | 23 ++- .../swerve}/follow_trajectory_command.h | 15 +- .../swerve/waypt_traversal_calculator.h | 54 +++++++ src/y2024/cpp/FunkyRobot.cc | 2 +- src/y2024/cpp/autos/drive_auto.cc | 6 +- src/y2024/cpp/autos/five_piece_auto.cc | 4 +- .../commands/auto_intake_and_shoot_command.cc | 7 +- .../cpp/commands/complex/stow_zero_action.cc | 4 +- .../cpp/commands/complex/super_amp_command.cc | 8 +- .../cpp/commands/follow_trajectory_command.cc | 138 ------------------ src/y2024/include/autos/drive_auto.h | 3 +- src/y2024/include/autos/five_piece_auto.h | 3 +- src/y2024/include/autos/one_piece_auto.h | 3 +- .../commands/auto_intake_and_shoot_command.h | 3 +- .../commands/complex/super_amp_command.h | 3 +- 20 files changed, 248 insertions(+), 341 deletions(-) delete mode 100644 src/frc846/cpp/frc846/other/trajectory_generator.cc create mode 100644 src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc create mode 100644 src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc delete mode 100644 src/frc846/include/frc846/other/trajectory_generator.h rename src/{y2024/include/commands => frc846/include/frc846/swerve}/follow_trajectory_command.h (61%) create mode 100644 src/frc846/include/frc846/swerve/waypt_traversal_calculator.h delete mode 100644 src/y2024/cpp/commands/follow_trajectory_command.cc diff --git a/src/frc846/cpp/frc846/other/trajectory_generator.cc b/src/frc846/cpp/frc846/other/trajectory_generator.cc deleted file mode 100644 index 4b7be61..0000000 --- a/src/frc846/cpp/frc846/other/trajectory_generator.cc +++ /dev/null @@ -1,130 +0,0 @@ -#include "frc846/other/trajectory_generator.h" - -#include - -#include "frc846/base/loggable.h" - -namespace frc846 { - -std::vector InterpolatePoints( - frc846::math::VectorND start, - frc846::math::VectorND end, units::degree_t start_bearing, - units::degree_t end_bearing, units::foot_t cut) { - auto distance = (end - start).magnitude(); - int n = std::max(units::math::ceil(distance / cut).to(), 1); - - std::vector points(n); - for (int i = 0; i < n; ++i) { - double weight = (double)(i) / n; - double bearingWeightMultiplier = 1.15; - points[i] = {{ - start[0] * (1 - weight) + end[0] * weight, - start[1] * (1 - weight) + end[1] * weight, - }, - start_bearing * (1 - (weight * bearingWeightMultiplier)) + - end_bearing * (weight * bearingWeightMultiplier), - {0.0_fps, 0.0_fps}}; - - if (start_bearing <= end_bearing) { - points[i].bearing = units::math::min(end_bearing, points[i].bearing); - } else { - points[i].bearing = units::math::max(end_bearing, points[i].bearing); - } - } - - return points; -} - -Trajectory GenerateTrajectory( - std::vector input_points, units::feet_per_second_t robot_max_v, - units::feet_per_second_squared_t robot_max_acceleration, - units::feet_per_second_squared_t robot_max_deceleration, - units::inch_t cut) { - frc846::base::Loggable loggable{"trajectory_generator"}; - - if (input_points.size() < 2) { - loggable.Error("Not enough input points! {} points given", - input_points.size()); - return {}; - } - - // TODO check that first v > 0 - - Trajectory trajectory; - - for (unsigned int i = input_points.size() - 1; i > 0; --i) { - auto interpolated_points = InterpolatePoints( - input_points[i].point, input_points[i - 1].point, - input_points[i].bearing, input_points[i - 1].bearing, cut); - interpolated_points.erase(interpolated_points.begin()); - - trajectory.push_back({input_points[i]}); - - for (auto point : interpolated_points) { - point.velocity[0] = robot_max_v; - trajectory.push_back({point}); - } - } - trajectory.push_back({input_points[0]}); - - for (unsigned int i = 1; i < trajectory.size(); ++i) { - auto delta_pos = - (trajectory[i].point - trajectory[i - 1].point).magnitude(); - - // v₂² = v₁² + 2aΔx - // 2aΔx = v₂² - v₁² - // a = (v₂² - v₁²) / (2Δx) - auto deceleration = - (units::math::pow<2>(trajectory[i].velocity.magnitude()) - - units::math::pow<2>(trajectory[i - 1].velocity.magnitude())) / - (2 * delta_pos); - if (deceleration > robot_max_deceleration) { - // v₂² = v₁² + 2aΔx - // v₂² = sqrt(v₁² + 2aΔx) - - trajectory[i].velocity[0] = units::math::sqrt( - units::math::pow<2>(trajectory[i - 1].velocity.magnitude()) + - 2 * robot_max_deceleration * delta_pos); - } - } - - std::reverse(trajectory.begin(), trajectory.end()); - - for (unsigned int i = 1; i < trajectory.size(); ++i) { - auto delta_pos = - (trajectory[i].point - trajectory[i - 1].point).magnitude(); - - // v₂² = v₁² + 2aΔx - // 2aΔx = v₂² - v₁² - // a = (v₂² - v₁²) / (2Δx) - auto acceleration = - (units::math::pow<2>(trajectory[i].velocity.magnitude()) - - units::math::pow<2>(trajectory[i - 1].velocity.magnitude())) / - (2 * delta_pos); - if (acceleration > robot_max_acceleration) { - // v₂² = v₁² + 2aΔx - // v₂² = sqrt(v₁² + 2aΔx) - trajectory[i].velocity[0] = units::math::sqrt( - units::math::pow<2>(trajectory[i - 1].velocity.magnitude()) + - 2 * robot_max_acceleration * delta_pos); - - // trajectory[i].v = - // units::velocity::feet_per_second_t(Find_Vsub2(units::unit_cast(trajectory[i].v), - // units::unit_cast(robot_max_acceleration), - // units::unit_cast(delta_pos))); - } - } - - // If any point has 0 speed, just set it to the previous speed. - for (unsigned int i = 0; i < trajectory.size(); ++i) { - if (trajectory[i].velocity.magnitude() == 0_fps) { - trajectory[i].velocity.magnitude() = - i == 0 ? trajectory[1].velocity.magnitude() - : trajectory[i - 1].velocity.magnitude(); - } - } - - return trajectory; -} - -} // namespace frc846 \ No newline at end of file diff --git a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc new file mode 100644 index 0000000..2a5830f --- /dev/null +++ b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc @@ -0,0 +1,88 @@ +#include "frc846/swerve/follow_trajectory_command.h" + +namespace frc846::swerve { + +FollowTrajectoryCommand::FollowTrajectoryCommand( + RobotContainer& container, std::vector input_points) + : frc846::robot::GenericCommand< + RobotContainer, FollowTrajectoryCommand>{container, + "follow_trajectory_command"}, + input_points_(input_points) { + AddRequirements({&container_.drivetrain_}); + + wtcalculator.setConstants({container_.drivetrain_.max_speed_.value(), + container_.drivetrain_.max_acceleration_.value(), + 2_in, 20_ms}); +} + +void FollowTrajectoryCommand::OnInit() { + Log("Starting Trajectory"); + for (frc846::Waypoint i : input_points_) { + Log("points x{} y{} bearing {}", i.point[0], i.point[1], i.bearing); + } + Log("initial pose x{}, y{}, bearing {}", + container_.drivetrain_.GetReadings().pose.point[0], + container_.drivetrain_.GetReadings().pose.point[1], + container_.drivetrain_.GetReadings().pose.bearing); + is_done_ = false; + + start_time_ = frc846::wpilib::CurrentFPGATime(); + + input_points_.insert(input_points_.begin(), 1, + {container_.drivetrain_.GetReadings().pose}); + + target_idx_ = 1; + + if (input_points_.size() < 2) { + Error("trajectory size ({}) is less than 2 - ending!", + input_points_.size()); + is_done_ = true; + } +} + +void FollowTrajectoryCommand::Periodic() { + // Just in case trajectory size was < 2 + if (is_done_) { + return; + } + + frc846::swerve::WTCInput lpcinput{ + input_points_[target_idx_ - 1].point, + input_points_[target_idx_].point, + container_.drivetrain_.GetReadings().pose.point, + container_.drivetrain_.GetReadings().pose.bearing, + input_points_[target_idx_].bearing, + container_.drivetrain_.GetReadings().pose.velocity.magnitude(), + input_points_[target_idx_].velocity.magnitude()}; + + frc846::swerve::WTCOutput lpcoutput = wtcalculator.calculate(lpcinput); + + if (lpcoutput.crossed_waypt) { + target_idx_++; + Log("Cross waypoint - now on {}/{}", target_idx_ + 1, input_points_.size()); + + if (target_idx_ == input_points_.size()) { + Log("Done!"); + is_done_ = true; + return; + } + } + + DrivetrainTarget target; + target.v_x = lpcoutput.target_vel[0]; + target.v_y = lpcoutput.target_vel[1]; + target.translation_reference = DrivetrainTranslationReference::kField; + target.rotation = DrivetrainRotationPosition(lpcoutput.bearing); + target.control = kClosedLoop; + + container_.drivetrain_.SetTarget(target); +} + +void FollowTrajectoryCommand::OnEnd(bool interrupted) { + (void)interrupted; + container_.drivetrain_.SetTargetZero(); +} + +bool FollowTrajectoryCommand::IsFinished() { return is_done_; } + +}; // namespace frc846::swerve \ No newline at end of file diff --git a/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc b/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc new file mode 100644 index 0000000..d4d6f9b --- /dev/null +++ b/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc @@ -0,0 +1,64 @@ +#include "frc846/swerve/waypt_traversal_calculator.h" + +namespace frc846::swerve { + +bool WayptTraversalCalculator::HasCrossedWaypt(WTCInput input) { + auto disp_vec = (input.current_pos - input.start_pos); + + auto target_vec = (input.target_pos - input.start_pos); + + if (target_vec.magnitude() <= constants_.loc_tolerance) { + return true; + } + + return disp_vec.projectOntoAnother(target_vec).magnitude() >= + target_vec.magnitude(); +}; + +WTCOutput WayptTraversalCalculator::calculate(WTCInput input) { + WTCOutput result{}; + result.target_vel = {0.0_fps, 0.0_fps}; + result.bearing = 0_deg; + + auto delta_vec = (input.target_pos - input.current_pos); + + if (delta_vec.magnitude() < constants_.loc_tolerance) { + result.crossed_waypt = true; + + return result; + } + result.crossed_waypt = HasCrossedWaypt(input); + + if (result.crossed_waypt) return result; + + auto dir_vec = delta_vec.unit(); + + units::second_t stopping_time = + units::math::abs(input.current_vel - input.target_vel) / + constants_.max_acceleration; + units::foot_t stopping_distance = + (input.current_vel + input.target_vel) / 2 * stopping_time; + + result.bearing = (input.target_bearing - input.current_bearing) * + constants_.loop_time / stopping_time; + + auto target_vel_mag = constants_.max_speed; + + if (stopping_distance > delta_vec.magnitude() - constants_.loc_tolerance) { + target_vel_mag = + input.current_vel + (input.target_vel - input.current_vel) * + constants_.loop_time / stopping_time; + + } else if (input.current_vel < constants_.max_speed) { + target_vel_mag = + input.current_vel + constants_.max_acceleration * constants_.loop_time; + } + target_vel_mag = units::math::min(target_vel_mag, constants_.max_speed); + + result.target_vel = {dir_vec[0].to() * target_vel_mag, + dir_vec[1].to() * target_vel_mag}; + + return result; +} + +}; // namespace frc846::swerve \ No newline at end of file diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h index 77669e9..0753788 100644 --- a/src/frc846/include/frc846/math/vectors.h +++ b/src/frc846/include/frc846/math/vectors.h @@ -156,7 +156,7 @@ class VectorND { T dot(const VectorND& other) const { T result = T{}; for (size_t i = 0; i < N; ++i) { - result += data[i] * other[i]; + result += data[i] * other[i].template to(); } return result; } @@ -186,12 +186,12 @@ class VectorND { // Projects this vector onto another and returns VectorND projectOntoAnother(const VectorND& other) const { - return other.unit() * dot(other.unit()).template to(); + return other.projectOntoThis(*this); } // Projects another vector onto this and returns VectorND projectOntoThis(const VectorND& other) const { - return unit() * other.dot(*this); + return unit() * dot(other).template to(); } // Returns the angle of this vector diff --git a/src/frc846/include/frc846/other/trajectory_generator.h b/src/frc846/include/frc846/other/trajectory_generator.h deleted file mode 100644 index 564febc..0000000 --- a/src/frc846/include/frc846/other/trajectory_generator.h +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include -#include - -#include "frc846/math/fieldpoints.h" - -namespace frc846 { - -using Waypoint = frc846::math::FieldPoint; - -using Trajectory = std::vector; - -std::vector InterpolatePoints( - frc846::math::VectorND start, - frc846::math::VectorND end, units::degree_t start_bearing, - units::degree_t end_bearing, units::foot_t cut); - -Trajectory GenerateTrajectory( - std::vector input_points, units::feet_per_second_t robot_max_v, - units::feet_per_second_squared_t robot_max_acceleration, - units::feet_per_second_squared_t robot_max_deceleration, - units::inch_t cut = 2_in); - -} // namespace frc846 diff --git a/src/frc846/include/frc846/robot/GenericCommand.h b/src/frc846/include/frc846/robot/GenericCommand.h index d55409a..4bb7b66 100644 --- a/src/frc846/include/frc846/robot/GenericCommand.h +++ b/src/frc846/include/frc846/robot/GenericCommand.h @@ -91,20 +91,19 @@ class GenericCommandGroup : Loggable{name}, container_{container} { frc2::Command::SetName(name); - auto start_command_added = frc2::InstantCommand([&] { - Log("Command group {} starting.", name); - command_start_time_ = frc846::wpilib::CurrentFPGATime(); - }); - auto end_command_added = frc2::InstantCommand([&] { - units::millisecond_t elapsed_time = - frc846::wpilib::CurrentFPGATime() - command_start_time_; - Log("Command group {} ending. Took {} ms to complete.", name, - elapsed_time.to()); - }); + // auto start_command_added = frc2::InstantCommand([&] { + // Log("Command group {} starting.", name); + // command_start_time_ = frc846::wpilib::CurrentFPGATime(); + // }); + // auto end_command_added = frc2::InstantCommand([&] { + // units::millisecond_t elapsed_time = + // frc846::wpilib::CurrentFPGATime() - command_start_time_; + // Log("Command group {} ending. Took {} ms to complete.", name, + // elapsed_time.to()); + // }); frc2::SequentialCommandGroup::AddCommands( - start_command_added, std::forward(commands)..., - end_command_added); + std::forward(commands)...); Log("Constructing instance of command group {}.", name); } diff --git a/src/y2024/include/commands/follow_trajectory_command.h b/src/frc846/include/frc846/swerve/follow_trajectory_command.h similarity index 61% rename from src/y2024/include/commands/follow_trajectory_command.h rename to src/frc846/include/frc846/swerve/follow_trajectory_command.h index 941d44e..58f4215 100644 --- a/src/y2024/include/commands/follow_trajectory_command.h +++ b/src/frc846/include/frc846/swerve/follow_trajectory_command.h @@ -1,9 +1,11 @@ #pragma once -#include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" +#include "frc846/swerve/waypt_traversal_calculator.h" #include "subsystems/robot_container.h" +namespace frc846::swerve { + class FollowTrajectoryCommand : public frc846::robot::GenericCommand { @@ -21,16 +23,13 @@ class FollowTrajectoryCommand private: std::vector input_points_; - frc846::Trajectory trajectory_; + unsigned int target_idx_; - unsigned int target_idx_ = 1; bool is_done_ = false; - frc846::math::VectorND current_extrapolated_point_; units::second_t start_time_; - static bool HasCrossedWaypoint( - frc846::Waypoint current_waypoint, frc846::Waypoint prev_waypoint, - frc846::math::VectorND pos, - frc846::math::VectorND test_target); + frc846::swerve::WayptTraversalCalculator wtcalculator{}; }; + +}; // namespace frc846::swerve \ No newline at end of file diff --git a/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h b/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h new file mode 100644 index 0000000..4d84dc1 --- /dev/null +++ b/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h @@ -0,0 +1,54 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "frc846/math/calculator.h" +#include "frc846/math/fieldpoints.h" +#include "frc846/math/vectors.h" + +namespace frc846 { +using Waypoint = frc846::math::FieldPoint; +}; + +namespace frc846::swerve { + +struct WTCInput { + frc846::math::VectorND start_pos; + frc846::math::VectorND target_pos; + frc846::math::VectorND current_pos; + + units::degree_t current_bearing; + units::degree_t target_bearing; + + units::feet_per_second_t current_vel; + units::feet_per_second_t target_vel; +}; + +struct WTCOutput { + bool crossed_waypt; + units::degree_t bearing; + frc846::math::VectorND target_vel; +}; + +struct WTCConstants { + units::feet_per_second_t max_speed; + units::feet_per_second_squared_t max_acceleration; + + units::inch_t loc_tolerance; + + units::millisecond_t loop_time; +}; + +class WayptTraversalCalculator + : public frc846::math::Calculator { + public: + bool HasCrossedWaypt(WTCInput input); + + WTCOutput calculate(WTCInput input) override; +}; + +}; // namespace frc846::swerve \ No newline at end of file diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index 01c12eb..dbbc297 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -14,7 +14,6 @@ #include "commands/basic/shoot_command.h" #include "commands/basic/spin_up_command.h" #include "commands/complex/stow_zero_action.h" -#include "commands/follow_trajectory_command.h" #include "commands/teleop/bracer_command.h" #include "commands/teleop/drive_command.h" #include "commands/teleop/idle_intake_command.h" @@ -24,6 +23,7 @@ #include "commands/teleop/stow_command.h" #include "control_triggers.h" #include "frc846/ntinf/ntaction.h" +#include "frc846/swerve/follow_trajectory_command.h" FunkyRobot::FunkyRobot() : GenericRobot{&container_} {} diff --git a/src/y2024/cpp/autos/drive_auto.cc b/src/y2024/cpp/autos/drive_auto.cc index 10da722..500365e 100644 --- a/src/y2024/cpp/autos/drive_auto.cc +++ b/src/y2024/cpp/autos/drive_auto.cc @@ -5,10 +5,10 @@ #include #include -#include "commands/follow_trajectory_command.h" #include "field.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" DriveAuto::DriveAuto(RobotContainer& container) : frc846::robot::GenericCommandGroup intake_path, @@ -22,10 +22,11 @@ AutoIntakeAndShootCommand::AutoIntakeAndShootCommand( frc2::SequentialCommandGroup{ AutoDeployIntakeCommand{container}, - FollowTrajectoryCommand{container, intake_path}, + frc846::swerve::FollowTrajectoryCommand{container, intake_path}, frc2::ParallelDeadlineGroup{ frc2::SequentialCommandGroup{ - FollowTrajectoryCommand{container, shoot_path}, + frc846::swerve::FollowTrajectoryCommand{container, + shoot_path}, frc2::WaitCommand( container.super_structure_.pre_shoot_wait_.value())}, PrepareAutoShootCommand{container}}, diff --git a/src/y2024/cpp/commands/complex/stow_zero_action.cc b/src/y2024/cpp/commands/complex/stow_zero_action.cc index 22d77bb..621b182 100644 --- a/src/y2024/cpp/commands/complex/stow_zero_action.cc +++ b/src/y2024/cpp/commands/complex/stow_zero_action.cc @@ -17,8 +17,8 @@ StowZeroActionCommand::StowZeroActionCommand(RobotContainer& container) frc2::SequentialCommandGroup{ frc2::ParallelDeadlineGroup{ frc2::WaitUntilCommand{[&] { - return container_.pivot_.WithinTolerance( - container_.super_structure_.getStowSetpoint().pivot); + return container.pivot_.WithinTolerance( + container.super_structure_.getStowSetpoint().pivot); }}, StowCommand{container}}, WristZeroCommand{container}, diff --git a/src/y2024/cpp/commands/complex/super_amp_command.cc b/src/y2024/cpp/commands/complex/super_amp_command.cc index afb335c..ef771bc 100644 --- a/src/y2024/cpp/commands/complex/super_amp_command.cc +++ b/src/y2024/cpp/commands/complex/super_amp_command.cc @@ -10,10 +10,10 @@ #include "commands/basic/eject_command.h" #include "commands/basic/pull_command.h" #include "commands/complex/close_drive_amp_command.h" -#include "commands/follow_trajectory_command.h" #include "field.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" SuperAmpCommand::SuperAmpCommand(RobotContainer& container, bool is_red_side) : frc846::robot::GenericCommandGroup input_points) - : frc846::robot::GenericCommand< - RobotContainer, FollowTrajectoryCommand>{container, - "follow_trajectory_command"}, - input_points_(input_points) { - AddRequirements({&container_.drivetrain_}); -} - -void FollowTrajectoryCommand::OnInit() { - Log("Starting Trajectory"); - for (frc846::Waypoint i : input_points_) { - Log("points x{} y{} bearing {}", i.point[0], i.point[1], i.bearing); - } - Log("initial pose x{}, y{}, bearing {}", - container_.drivetrain_.GetReadings().pose.point[0], - container_.drivetrain_.GetReadings().pose.point[1], - container_.drivetrain_.GetReadings().pose.bearing); - target_idx_ = 1; - is_done_ = false; - - start_time_ = frc846::wpilib::CurrentFPGATime(); - - auto points = input_points_; - points.insert(points.begin(), 1, {container_.drivetrain_.GetReadings().pose}); - - trajectory_ = frc846::GenerateTrajectory( - points, container_.drivetrain_.auto_max_speed_.value(), - container_.drivetrain_.max_acceleration_.value(), - container_.drivetrain_.max_deceleration_.value()); - - if (trajectory_.size() < 4) { - Error("trajectory size ({}) is less than 4 - ending!", trajectory_.size()); - is_done_ = true; - } else { - double extrapolation_distance = - container_.drivetrain_.extrapolation_distance_.value() / 1_ft; - current_extrapolated_point_ = trajectory_[1].point - trajectory_[0].point; - current_extrapolated_point_ = - current_extrapolated_point_.unit() * extrapolation_distance; - } -} - -void FollowTrajectoryCommand::Periodic() { - // Just in case trajectory size was < 2 - if (is_done_) { - return; - } - - if (HasCrossedWaypoint(trajectory_[target_idx_], trajectory_[target_idx_ - 1], - container_.drivetrain_.GetReadings().pose.point, - current_extrapolated_point_)) { - target_idx_++; - Log("Cross waypoint - now on {}/{}", target_idx_ + 1, trajectory_.size()); - - if (target_idx_ == trajectory_.size()) { - Log("Done!"); - is_done_ = true; - return; - } - - double extrapolation_distance = - container_.drivetrain_.extrapolation_distance_.value() / 1_ft; - current_extrapolated_point_ = - trajectory_[target_idx_].point - trajectory_[target_idx_ - 1].point; - current_extrapolated_point_ = - current_extrapolated_point_.unit() * extrapolation_distance; - } - - auto delta_pos = current_extrapolated_point_ - - container_.drivetrain_.GetReadings().pose.point; - auto direction = units::math::atan2(delta_pos[1], delta_pos[0]); - - DrivetrainTarget target; - target.v_x = trajectory_[target_idx_].velocity.magnitude() * - units::math::cos(direction); - target.v_y = trajectory_[target_idx_].velocity.magnitude() * - units::math::sin(direction); - target.translation_reference = DrivetrainTranslationReference::kField; - target.rotation = - DrivetrainRotationPosition(trajectory_[target_idx_].bearing); - target.control = kClosedLoop; - - container_.drivetrain_.SetTarget(target); -} - -void FollowTrajectoryCommand::OnEnd(bool interrupted) { - (void)interrupted; - container_.drivetrain_.SetTargetZero(); -} - -bool FollowTrajectoryCommand::IsFinished() { return is_done_; } - -// https://math.stackexchange.com/questions/274712/calculate-on-which-side-of-a-straight-line-is-a-given-point-located -bool FollowTrajectoryCommand::HasCrossedWaypoint( - frc846::Waypoint current_waypoint, frc846::Waypoint prev_waypoint, - frc846::math::VectorND pos, - frc846::math::VectorND extrapolated_point) { - // fmt::print( - // "\ncurrent_waypoint x {}, current_waypoint y {}, prev_waypoint x {} y " - // "{}, pos x{} y{}, extrap x{}, y{}\n", - // current_waypoint.point.x, current_waypoint.point.y, - // prev_waypoint.point.x, prev_waypoint.point.y, pos.x, pos.y, - // extrapolated_point.x, extrapolated_point.y); - - auto d = [](frc846::math::VectorND target, - frc846::math::VectorND p1, - frc846::math::VectorND p2) { - double x = ((target[0] - p1[0]) * (p2[1] - p1[1]) - - (target[1] - p1[1]) * (p2[0] - p1[0])) - .to(); - if (x > 0) { - return 1; - } else if (x < 0) { - return -1; - } - return 0; - }; - - auto delta_y = current_waypoint.point[1] - prev_waypoint.point[1]; - auto delta_x = current_waypoint.point[0] - prev_waypoint.point[0]; - auto theta = units::math::atan(-delta_x / delta_y); - double cos_theta = units::math::cos(theta); - double sin_theta = units::math::sin(theta); - - auto p1 = current_waypoint.point - frc846::math::VectorND{ - 1_ft * cos_theta, - 1_ft * sin_theta, - }; - auto p2 = current_waypoint.point + frc846::math::VectorND{ - 1_ft * cos_theta, - 1_ft * sin_theta, - }; - - return d(pos, p1, p2) == d(extrapolated_point, p1, p2); -} \ No newline at end of file diff --git a/src/y2024/include/autos/drive_auto.h b/src/y2024/include/autos/drive_auto.h index 897ad1b..39999aa 100644 --- a/src/y2024/include/autos/drive_auto.h +++ b/src/y2024/include/autos/drive_auto.h @@ -1,8 +1,7 @@ #pragma once -#include "commands/follow_trajectory_command.h" -#include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" #include "subsystems/robot_container.h" class DriveAuto diff --git a/src/y2024/include/autos/five_piece_auto.h b/src/y2024/include/autos/five_piece_auto.h index dd93762..055a624 100644 --- a/src/y2024/include/autos/five_piece_auto.h +++ b/src/y2024/include/autos/five_piece_auto.h @@ -1,8 +1,7 @@ #pragma once -#include "commands/follow_trajectory_command.h" -#include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" #include "subsystems/robot_container.h" class FivePieceAuto diff --git a/src/y2024/include/autos/one_piece_auto.h b/src/y2024/include/autos/one_piece_auto.h index 6e9aef8..3aafa57 100644 --- a/src/y2024/include/autos/one_piece_auto.h +++ b/src/y2024/include/autos/one_piece_auto.h @@ -1,8 +1,7 @@ #pragma once -#include "commands/follow_trajectory_command.h" -#include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" #include "subsystems/robot_container.h" class OnePieceAuto diff --git a/src/y2024/include/commands/auto_intake_and_shoot_command.h b/src/y2024/include/commands/auto_intake_and_shoot_command.h index 0064777..a41c4f2 100644 --- a/src/y2024/include/commands/auto_intake_and_shoot_command.h +++ b/src/y2024/include/commands/auto_intake_and_shoot_command.h @@ -1,8 +1,7 @@ #pragma once -#include "commands/follow_trajectory_command.h" -#include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" #include "subsystems/robot_container.h" class AutoIntakeAndShootCommand diff --git a/src/y2024/include/commands/complex/super_amp_command.h b/src/y2024/include/commands/complex/super_amp_command.h index 45c3fa2..c266f71 100644 --- a/src/y2024/include/commands/complex/super_amp_command.h +++ b/src/y2024/include/commands/complex/super_amp_command.h @@ -2,9 +2,8 @@ #include -#include "commands/follow_trajectory_command.h" -#include "frc846/other/trajectory_generator.h" #include "frc846/robot/GenericCommand.h" +#include "frc846/swerve/follow_trajectory_command.h" #include "subsystems/robot_container.h" class SuperAmpCommand From 8304748dd8b7d24ba37b8e77d3edefa95d76ee02 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Tue, 8 Oct 2024 09:35:50 -0700 Subject: [PATCH 05/12] Scriptable autos, vision autos, new trapezoidal motion profile calculations --- .vscode/settings.json | 3 +- build.gradle | 54 +- networktables.json | 2570 +++++++++++++++++ networktables.json.bck | 2570 +++++++++++++++++ simgui.json | 5 - src/deploy/autos/paths/drive_auto | 2 + src/deploy/autos/paths/fp_center_run | 1 + src/deploy/autos/paths/fp_home | 1 + src/deploy/autos/paths/fp_piece_one | 1 + src/deploy/autos/paths/fp_piece_three | 1 + src/deploy/autos/paths/fp_piece_two | 1 + src/deploy/autos/points.lst | 4 + src/deploy/autos/scripts/drive_auto | 3 + src/deploy/autos/scripts/four_piece_auto | 21 + src/deploy/info.txt | 4 - src/frc846/cpp/frc846/ntinf/fstore.cc | 17 +- src/frc846/cpp/frc846/robot/GenericRobot.cc | 19 +- .../swerve/follow_trajectory_command.cc | 89 +- .../swerve/waypt_traversal_calculator.cc | 66 +- src/frc846/include/frc846/control/motion.h | 6 +- src/frc846/include/frc846/math/fieldpoints.h | 19 +- src/frc846/include/frc846/ntinf/pref.h | 40 +- .../include/frc846/robot/GenericCommand.h | 24 +- .../include/frc846/robot/GenericRobot.h | 6 +- .../frc846/swerve/follow_trajectory_command.h | 6 +- .../swerve/waypt_traversal_calculator.h | 14 +- .../waypt_traversal_calculator_test.cpp | 145 + src/test/cpp/main.cpp | 10 + src/y2024/cpp/FunkyRobot.cc | 44 +- src/y2024/cpp/autos/ActionMaker.cc | 20 + src/y2024/cpp/autos/GenericAuto.cc | 41 + src/y2024/cpp/autos/drive_auto.cc | 27 - src/y2024/cpp/autos/five_piece_auto.cc | 58 - src/y2024/cpp/autos/one_piece.cc | 27 - .../commands/auto_intake_and_shoot_command.cc | 64 +- .../complex/close_drive_amp_command.cc | 72 +- .../complex/home_during_auto_command.cc | 25 + .../cpp/commands/complex/stow_zero_action.cc | 2 +- .../cpp/commands/complex/super_amp_command.cc | 72 +- .../cpp/commands/teleop/drive_command.cc | 1 - src/y2024/cpp/control_triggers.cc | 24 +- src/y2024/cpp/field.cc | 241 +- src/y2024/cpp/main.cpp | 131 - src/y2024/cpp/subsystems/abstract/vision.cc | 18 +- src/y2024/cpp/subsystems/hardware/wrist.cc | 19 +- src/y2024/include/FunkyRobot.h | 28 +- src/y2024/include/autos/ActionMaker.h | 14 + .../autos/{drive_auto.h => GenericAuto.h} | 11 +- src/y2024/include/autos/five_piece_auto.h | 12 - src/y2024/include/autos/one_piece_auto.h | 13 - .../complex/close_drive_amp_command.h | 26 +- .../complex/home_during_auto_command.h | 13 + .../commands/complex/super_amp_command.h | 24 +- src/y2024/include/field.h | 154 +- src/y2024/include/rsighandler.h | 60 + .../include/subsystems/abstract/vision.h | 10 + src/y2024/include/subsystems/hardware/wrist.h | 25 +- src/y2024/resources/deploy_autos.bat | 3 + src/y2024/resources/jetson/readAprilTag.py | 408 +-- src/y2024/resources/jetson/readAprilTag2.py | 155 - 60 files changed, 6443 insertions(+), 1101 deletions(-) create mode 100644 networktables.json create mode 100644 networktables.json.bck create mode 100644 src/deploy/autos/paths/drive_auto create mode 100644 src/deploy/autos/paths/fp_center_run create mode 100644 src/deploy/autos/paths/fp_home create mode 100644 src/deploy/autos/paths/fp_piece_one create mode 100644 src/deploy/autos/paths/fp_piece_three create mode 100644 src/deploy/autos/paths/fp_piece_two create mode 100644 src/deploy/autos/points.lst create mode 100644 src/deploy/autos/scripts/drive_auto create mode 100644 src/deploy/autos/scripts/four_piece_auto delete mode 100644 src/deploy/info.txt create mode 100644 src/test/cpp/arch/swerve/waypt_traversal_calculator_test.cpp create mode 100644 src/test/cpp/main.cpp create mode 100644 src/y2024/cpp/autos/ActionMaker.cc create mode 100644 src/y2024/cpp/autos/GenericAuto.cc delete mode 100644 src/y2024/cpp/autos/drive_auto.cc delete mode 100644 src/y2024/cpp/autos/five_piece_auto.cc delete mode 100644 src/y2024/cpp/autos/one_piece.cc create mode 100644 src/y2024/cpp/commands/complex/home_during_auto_command.cc delete mode 100644 src/y2024/cpp/main.cpp create mode 100644 src/y2024/include/autos/ActionMaker.h rename src/y2024/include/autos/{drive_auto.h => GenericAuto.h} (53%) delete mode 100644 src/y2024/include/autos/five_piece_auto.h delete mode 100644 src/y2024/include/autos/one_piece_auto.h create mode 100644 src/y2024/include/commands/complex/home_during_auto_command.h create mode 100644 src/y2024/include/rsighandler.h create mode 100644 src/y2024/resources/deploy_autos.bat delete mode 100644 src/y2024/resources/jetson/readAprilTag2.py diff --git a/.vscode/settings.json b/.vscode/settings.json index 3c6ded9..b795541 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -117,5 +117,6 @@ "queue": "cpp" }, "wpilib.useWindbgX": true, - "C_Cpp.errorSquiggles": "enabled" + "C_Cpp.errorSquiggles": "enabled", + "wpilib.skipTests": true } diff --git a/build.gradle b/build.gradle index b634b0e..c13ff5e 100644 --- a/build.gradle +++ b/build.gradle @@ -7,11 +7,16 @@ plugins { // Define my targets (RoboRIO) and artifacts (deployable files) deploy { + println "Setting up deploy targets and artifacts..." targets { roborio(getTargetTypeClass('RoboRIO')) { team = project.frc.getTeamNumber() debug = project.frc.getDebugOrDefault(false) + println "Deploy team # set to ${team}." + println "Debug set to ${debug} (disabled by default).\n" + + artifacts { frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { } @@ -28,22 +33,27 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcCpp // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Set to true to run simulation in debug mode -wpi.cpp.debugSimulation = true +wpi.cpp.debugSimulation = false // Default enable simgui -wpi.sim.addGui().defaultEnabled = true +wpi.sim.addGui().defaultEnabled = false // Enable DS but not by default wpi.sim.addDriverstation() model { components { frcUserProgram(NativeExecutableSpec) { + println "Configuring frcUserProgram..." targetPlatform wpi.platforms.roborio if (includeDesktopSupport) { targetPlatform wpi.platforms.desktop + binaries.all { + cppCompiler.args << '-DNOMINMAX' + println "Applied compiler argument NOMINMAX to allow for compilation on Windows platform." + } } sources.cpp { @@ -71,22 +81,28 @@ model { wpi.cpp.deps.wpilib(it) } } - // Uncomment and configure test suites if needed - // testSuites { - // frcUserProgramTest(GoogleTestTestSuiteSpec) { - // testing $.components.frcUserProgram - // sources.cpp { - // source { - // srcDir 'src/test/cpp' - // include '**/*.cpp' - // } - // } - // wpi.cpp.enableExternalTasks(it) - // wpi.cpp.vendor.cpp(it) - // wpi.cpp.deps.wpilib(it) - // wpi.cpp.deps.googleTest(it) - // } - // } + + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + println "Configuring frcUserProgramTest..." + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + binaries.all { + cppCompiler.args << '-DNOMINMAX' + println "Applied compiler argument NOMINMAX to allow for compilation on Windows platform." + } + wpi.cpp.enableExternalTasks(it) + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + wpi.cpp.deps.googleTest(it) + } + } } spotless { diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..10bfd50 --- /dev/null +++ b/networktables.json @@ -0,0 +1,2570 @@ +[ + { + "name": "/Preferences//drive_auto_origin_pt_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/five_piece_auto/pre_point_amt (ft)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_x (in)", + "type": "double", + "value": 217.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_y (in)", + "type": "double", + "value": 49.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_x (in)", + "type": "double", + "value": 217.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_y (in)", + "type": "double", + "value": 112.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_x (in)", + "type": "double", + "value": 160.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_y (in)", + "type": "double", + "value": 112.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_x (in)", + "type": "double", + "value": 274.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_y (in)", + "type": "double", + "value": 112.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_x (in)", + "type": "double", + "value": 274.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_y (in)", + "type": "double", + "value": 180.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_x (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_y (in)", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_drivetrain", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_intake", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_shooter", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_wrist", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_pivot", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_telescope", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_leds", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_bracers_", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_vision_", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/translation_deadband", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/steer_deadband", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/translation_exponent", + "type": "int", + "value": 1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/steer_exponent", + "type": "int", + "value": 2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/trigger_threshold", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/rumble_strength", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/operator/trigger_threshold", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/operator/rumble_strength", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/max_speed (fps)", + "type": "double", + "value": 14.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/close_drive_amp_max_speed (fps)", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/current_limit (A)", + "type": "double", + "value": 90.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/motor_stall_current (A)", + "type": "double", + "value": 366.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/braking_constant", + "type": "double", + "value": 0.15, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/ramp_rate_vx (fps)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/ramp_rate_vy (fps)", + "type": "double", + "value": 100.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/auto_max_speed (fps)", + "type": "double", + "value": 11.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/driver_speed_multiplier", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/slow_mode_percent", + "type": "double", + "value": 0.04, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/slow_omega_percent", + "type": "double", + "value": 0.12, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/pov_control_speed_", + "type": "double", + "value": 10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/velocity_error (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/max_acceleration (fps_sq)", + "type": "double", + "value": 8.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/max_deceleration (fps_sq)", + "type": "double", + "value": 10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/extrapolation_distance (in)", + "type": "double", + "value": 8.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/angular_velocity_threshold (deg_per_s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/align_gains/p", + "type": "double", + "value": 3.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/align_gains/align_tolerance (in)", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/width (in)", + "type": "double", + "value": 21.75, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/height (in)", + "type": "double", + "value": 26.75, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/percent_max_omega", + "type": "double", + "value": 0.45, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/wheel_radius (in)", + "type": "double", + "value": 1.93, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/bearing_gains/p", + "type": "double", + "value": 8.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/bearing_gains/d", + "type": "double", + "value": -4.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/gear_ratio", + "type": "double", + "value": 0.14979852970587568, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Gains/kP", + "type": "double", + "value": 0.0002, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Gains/kF", + "type": "double", + "value": 0.0001769, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/gear_ratio", + "type": "double", + "value": 16.8, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/default_brake_mode", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Gains/kP", + "type": "double", + "value": 0.12, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/smart_current_braking/smart_current_limit (A)", + "type": "double", + "value": 170.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/smart_current_braking/braking_p", + "type": "double", + "value": 0.45, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/cancoder_offset (deg)", + "type": "double", + "value": 3.23, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/cancoder_offset (deg)", + "type": "double", + "value": 140.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/cancoder_offset (deg)", + "type": "double", + "value": 297.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/cancoder_offset (deg)", + "type": "double", + "value": 157.89, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/base_intake_speed_ (fps)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/intake_feed_speed_ (fps)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/release_speed", + "type": "double", + "value": -0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/retract_speed", + "type": "double", + "value": -0.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/gear_ratio", + "type": "double", + "value": 0.11780972437500001, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Gains/kP", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Gains/kF", + "type": "double", + "value": 0.15, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooter_speed (fps)", + "type": "double", + "value": 65.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooter_spin", + "type": "double", + "value": 0.33, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooting_exit_velocity", + "type": "double", + "value": 47.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooter_speed_tolerance", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/gear_ratio", + "type": "double", + "value": 1.2435470906250001, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/default_brake_mode", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Gains/kP", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Gains/kF", + "type": "double", + "value": 0.15, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/vFPID/smart_current_limit (A)", + "type": "double", + "value": 35.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/vFPID/braking_p", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/wrist_tolerance (deg)", + "type": "double", + "value": 1.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/home_offset_wrist (deg)", + "type": "double", + "value": -49.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/cg_offset_wrist (deg)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/max_adjustment_rate (deg_per_s)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_velocity_tolerance (deg_per_s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/num_loops_homed", + "type": "int", + "value": 7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_speed", + "type": "double", + "value": -0.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/gains/k", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/gains/p", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/gains/d", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/gear_ratio", + "type": "double", + "value": 4.32, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Gains/kP", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 150.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 0.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/smart_current_limit (A)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/braking_p", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/pivot_tolerance (deg)", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/pivot_home_offset (deg)", + "type": "double", + "value": -17.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/max_adjustment_rate (deg_per_s)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/gear_ratio", + "type": "double", + "value": 2.084558823529412, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Gains/kP", + "type": "double", + "value": 0.047, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 110.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 0.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/DynamicFPID/smart_current_limit (A)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/DynamicFPID/braking_p", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/max_adjustment_rate (fps)", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/telescope_tolerance (in)", + "type": "double", + "value": 0.25, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/gear_ratio", + "type": "double", + "value": 0.25, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Gains/kP", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/forward (in)", + "type": "double", + "value": 6.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/reverse (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -0.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/wrist_trim_increment (deg)", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/manual_control_deadband", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/auto/post_shoot_wait (s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/auto/pre_shoot_wait (s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/teleop_shooter_height (in)", + "type": "double", + "value": 39.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/teleop_shooter_x (in)", + "type": "double", + "value": 4.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/auto_shooter_height (in)", + "type": "double", + "value": 21.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/auto_shooter_x (in)", + "type": "double", + "value": 13.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/trap/pivot_pull_target (deg)", + "type": "double", + "value": -10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/amp_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/amp_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/amp_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/intake_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/intake_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/intake_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/source_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/source_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/source_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/stow_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/stow_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/stow_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/shoot_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/auto_shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/auto_shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/auto_shoot_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/preclimb_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/preclimb_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/preclimb_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/prescore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/prescore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/prescore_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/trapscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/trapscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/trapscore_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/postscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/postscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/postscore_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/pass_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/pass_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/pass_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/using_vision_autos", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/default_is_red_side", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/can_bus_latency (ms)", + "type": "double", + "value": 20.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_height (in)", + "type": "double", + "value": 14.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_angle (deg)", + "type": "double", + "value": 37.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_y_offset (in)", + "type": "double", + "value": -10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_x_offset (in)", + "type": "double", + "value": -7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot/start_coast_counter", + "type": "int", + "value": 500, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_max_speed (deg_per_s)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_dc_cut", + "type": "double", + "value": 1.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/Gains/kP", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Robot/start_coast_counter", + "type": "int", + "value": 500, + "properties": { + "persistent": true + } + } +] diff --git a/networktables.json.bck b/networktables.json.bck new file mode 100644 index 0000000..10bfd50 --- /dev/null +++ b/networktables.json.bck @@ -0,0 +1,2570 @@ +[ + { + "name": "/Preferences//drive_auto_origin_pt_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_origin_pt_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_travel_pt_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/five_piece_auto/pre_point_amt (ft)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_x (in)", + "type": "double", + "value": 217.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_y (in)", + "type": "double", + "value": 49.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_origin_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_x (in)", + "type": "double", + "value": 217.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_y (in)", + "type": "double", + "value": 112.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_one_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_x (in)", + "type": "double", + "value": 160.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_y (in)", + "type": "double", + "value": 112.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_two_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_x (in)", + "type": "double", + "value": 274.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_y (in)", + "type": "double", + "value": 112.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_intake_three_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_x (in)", + "type": "double", + "value": 274.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_y (in)", + "type": "double", + "value": 180.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//five_piece_finish_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_origin_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_x (in)", + "type": "double", + "value": 214.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_y (in)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//drive_auto_pt_2_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_x (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_y (in)", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_deg (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_v_x (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences//testing_point_v_y (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_drivetrain", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_intake", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_shooter", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_wrist", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_pivot", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_telescope", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_leds", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_bracers_", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot_container/init_vision_", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/translation_deadband", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/steer_deadband", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/translation_exponent", + "type": "int", + "value": 1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/steer_exponent", + "type": "int", + "value": 2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/trigger_threshold", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/driver/rumble_strength", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/operator/trigger_threshold", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/operator/rumble_strength", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/max_speed (fps)", + "type": "double", + "value": 14.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/close_drive_amp_max_speed (fps)", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/current_limit (A)", + "type": "double", + "value": 90.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/motor_stall_current (A)", + "type": "double", + "value": 366.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/braking_constant", + "type": "double", + "value": 0.15, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/ramp_rate_vx (fps)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/ramp_rate_vy (fps)", + "type": "double", + "value": 100.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/auto_max_speed (fps)", + "type": "double", + "value": 11.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/driver_speed_multiplier", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/slow_mode_percent", + "type": "double", + "value": 0.04, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/slow_omega_percent", + "type": "double", + "value": 0.12, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/pov_control_speed_", + "type": "double", + "value": 10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/velocity_error (fps)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/max_acceleration (fps_sq)", + "type": "double", + "value": 8.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/max_deceleration (fps_sq)", + "type": "double", + "value": 10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/extrapolation_distance (in)", + "type": "double", + "value": 8.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/angular_velocity_threshold (deg_per_s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/align_gains/p", + "type": "double", + "value": 3.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/align_gains/align_tolerance (in)", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/width (in)", + "type": "double", + "value": 21.75, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/height (in)", + "type": "double", + "value": 26.75, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/percent_max_omega", + "type": "double", + "value": 0.45, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/wheel_radius (in)", + "type": "double", + "value": 1.93, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/bearing_gains/p", + "type": "double", + "value": 8.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/bearing_gains/d", + "type": "double", + "value": -4.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/gear_ratio", + "type": "double", + "value": 0.14979852970587568, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Gains/kP", + "type": "double", + "value": 0.0002, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/drive_esc/Gains/kF", + "type": "double", + "value": 0.0001769, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/gear_ratio", + "type": "double", + "value": 16.8, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/default_brake_mode", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Gains/kP", + "type": "double", + "value": 0.12, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/steer_esc/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/smart_current_braking/smart_current_limit (A)", + "type": "double", + "value": 170.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/smart_current_braking/braking_p", + "type": "double", + "value": 0.45, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/cancoder_offset (deg)", + "type": "double", + "value": 3.23, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FL/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/cancoder_offset (deg)", + "type": "double", + "value": 140.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_FR/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/cancoder_offset (deg)", + "type": "double", + "value": 297.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BL/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/cancoder_offset (deg)", + "type": "double", + "value": 157.89, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/drivetrain/module_BR/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/base_intake_speed_ (fps)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/intake_feed_speed_ (fps)", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/release_speed", + "type": "double", + "value": -0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/retract_speed", + "type": "double", + "value": -0.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/gear_ratio", + "type": "double", + "value": 0.11780972437500001, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Gains/kP", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Gains/kF", + "type": "double", + "value": 0.15, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/intake/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooter_speed (fps)", + "type": "double", + "value": 65.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooter_spin", + "type": "double", + "value": 0.33, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooting_exit_velocity", + "type": "double", + "value": 47.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/shooter_speed_tolerance", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/gear_ratio", + "type": "double", + "value": 1.2435470906250001, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/default_brake_mode", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Gains/kP", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Gains/kF", + "type": "double", + "value": 0.15, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/forward (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/reverse (ft)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/vFPID/smart_current_limit (A)", + "type": "double", + "value": 35.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/shooter/vFPID/braking_p", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/wrist_tolerance (deg)", + "type": "double", + "value": 1.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/home_offset_wrist (deg)", + "type": "double", + "value": -49.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/cg_offset_wrist (deg)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/max_adjustment_rate (deg_per_s)", + "type": "double", + "value": 80.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_velocity_tolerance (deg_per_s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/num_loops_homed", + "type": "int", + "value": 7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_speed", + "type": "double", + "value": -0.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/gains/k", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/gains/p", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/gains/d", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/gear_ratio", + "type": "double", + "value": 4.32, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Gains/kP", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 150.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 0.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/smart_current_limit (A)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/braking_p", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/pivot_tolerance (deg)", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/pivot_home_offset (deg)", + "type": "double", + "value": -17.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/max_adjustment_rate (deg_per_s)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/gear_ratio", + "type": "double", + "value": 2.084558823529412, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Gains/kP", + "type": "double", + "value": 0.047, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/forward (deg)", + "type": "double", + "value": 110.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/reverse (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 0.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/DynamicFPID/smart_current_limit (A)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/DynamicFPID/braking_p", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/max_adjustment_rate (fps)", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/telescope_tolerance (in)", + "type": "double", + "value": 0.25, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/invert", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/gear_ratio", + "type": "double", + "value": 0.25, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/default_brake_mode", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/CurrentLimiting/target_threshold (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/CurrentLimiting/peak_time_threshold (ms)", + "type": "double", + "value": 200.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/ramp_time (s)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/use_ramp_rate", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/voltage_compensation (V)", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/auton_voltage_compensation (V)", + "type": "double", + "value": 12.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Gains/kP", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/forward (in)", + "type": "double", + "value": 6.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/reverse (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/use_position_limits", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/peak_output_forward", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/Configs/HardLimits/peak_output_reverse", + "type": "double", + "value": -0.7, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/wrist_trim_increment (deg)", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/manual_control_deadband", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/auto/post_shoot_wait (s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/auto/pre_shoot_wait (s)", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/teleop_shooter_height (in)", + "type": "double", + "value": 39.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/teleop_shooter_x (in)", + "type": "double", + "value": 4.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/auto_shooter_height (in)", + "type": "double", + "value": 21.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/shooting/auto_shooter_x (in)", + "type": "double", + "value": 13.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SuperStructure/trap/pivot_pull_target (deg)", + "type": "double", + "value": -10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/amp_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/amp_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/amp_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/intake_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/intake_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/intake_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/source_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/source_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/source_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/stow_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/stow_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/stow_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/shoot_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/auto_shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/auto_shoot_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/auto_shoot_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/preclimb_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/preclimb_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/preclimb_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/prescore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/prescore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/prescore_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/trapscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/trapscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/trapscore_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/postscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/postscore_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/postscore_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Pivot/pass_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Wrist/pass_position (deg)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Preferences/MotionTargets/Telescope/pass_position (in)", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/using_vision_autos", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/default_is_red_side", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/can_bus_latency (ms)", + "type": "double", + "value": 20.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_height (in)", + "type": "double", + "value": 14.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_angle (deg)", + "type": "double", + "value": 37.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_y_offset (in)", + "type": "double", + "value": -10.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/vision/camera_x_offset (in)", + "type": "double", + "value": -7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/robot/start_coast_counter", + "type": "int", + "value": 500, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_max_speed (deg_per_s)", + "type": "double", + "value": 30.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_dc_cut", + "type": "double", + "value": 1.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/Gains/kP", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/Gains/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/DynamicFPID/Gains/kF", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Robot/start_coast_counter", + "type": "int", + "value": 500, + "properties": { + "persistent": true + } + } +] diff --git a/simgui.json b/simgui.json index 8ad6101..43060a7 100644 --- a/simgui.json +++ b/simgui.json @@ -6,11 +6,6 @@ "open": true } }, - " Talon FX (v6)[4]": { - "header": { - "open": true - } - }, " Talon FX (v6)[5]": { "header": { "open": true diff --git a/src/deploy/autos/paths/drive_auto b/src/deploy/autos/paths/drive_auto new file mode 100644 index 0000000..0d07b52 --- /dev/null +++ b/src/deploy/autos/paths/drive_auto @@ -0,0 +1,2 @@ +P,217.5,80,0,0 +P,217.5,180,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_center_run b/src/deploy/autos/paths/fp_center_run new file mode 100644 index 0000000..4fa6c1b --- /dev/null +++ b/src/deploy/autos/paths/fp_center_run @@ -0,0 +1 @@ +P,217.5,112.0,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_home b/src/deploy/autos/paths/fp_home new file mode 100644 index 0000000..500982f --- /dev/null +++ b/src/deploy/autos/paths/fp_home @@ -0,0 +1 @@ +F,kPointBlank \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_one b/src/deploy/autos/paths/fp_piece_one new file mode 100644 index 0000000..b66b275 --- /dev/null +++ b/src/deploy/autos/paths/fp_piece_one @@ -0,0 +1 @@ +P,217.5,112,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_three b/src/deploy/autos/paths/fp_piece_three new file mode 100644 index 0000000..b66b275 --- /dev/null +++ b/src/deploy/autos/paths/fp_piece_three @@ -0,0 +1 @@ +P,217.5,112,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_two b/src/deploy/autos/paths/fp_piece_two new file mode 100644 index 0000000..b66b275 --- /dev/null +++ b/src/deploy/autos/paths/fp_piece_two @@ -0,0 +1 @@ +P,217.5,112,0,0 \ No newline at end of file diff --git a/src/deploy/autos/points.lst b/src/deploy/autos/points.lst new file mode 100644 index 0000000..b4c5a0b --- /dev/null +++ b/src/deploy/autos/points.lst @@ -0,0 +1,4 @@ +N,kOrigin,0,0,0,0 +N,kSpeaker,217.5,-4,0,0 +N,kAmp,0,0,90,0 +N,kPointBlank,217.5,49,0,0 \ No newline at end of file diff --git a/src/deploy/autos/scripts/drive_auto b/src/deploy/autos/scripts/drive_auto new file mode 100644 index 0000000..6f87d38 --- /dev/null +++ b/src/deploy/autos/scripts/drive_auto @@ -0,0 +1,3 @@ +0,0 +F,kPointBlank +PATH,drive_auto \ No newline at end of file diff --git a/src/deploy/autos/scripts/four_piece_auto b/src/deploy/autos/scripts/four_piece_auto new file mode 100644 index 0000000..e45a6d2 --- /dev/null +++ b/src/deploy/autos/scripts/four_piece_auto @@ -0,0 +1,21 @@ +0,2 +F,kPointBlank +ACT,auto_home +ACT,prep_shoot +ACT,shoot +ACT,deploy_intake +PATH,fp_piece_one +ACT,prep_shoot +PATH,fp_home +ACT,shoot +ACT,deploy_intake +PATH,fp_piece_two +ACT,prep_shoot +PATH,fp_home +ACT,shoot +ACT,deploy_intake +PATH,fp_piece_three +ACT,prep_shoot +PATH,fp_home +ACT,shoot +PATH,fp_center_run \ No newline at end of file diff --git a/src/deploy/info.txt b/src/deploy/info.txt deleted file mode 100644 index 6839539..0000000 --- a/src/deploy/info.txt +++ /dev/null @@ -1,4 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the - 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' - function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy - directory. \ No newline at end of file diff --git a/src/frc846/cpp/frc846/ntinf/fstore.cc b/src/frc846/cpp/frc846/ntinf/fstore.cc index e16bf42..6b3d812 100644 --- a/src/frc846/cpp/frc846/ntinf/fstore.cc +++ b/src/frc846/cpp/frc846/ntinf/fstore.cc @@ -19,6 +19,9 @@ frc846::base::Loggable FunkyStore::fstore_loggable{"FunkyStore"}; bool FunkyStore::hasChanged = false; +std::map> + FunkyStore::prefs = {{"Default.int", 0.0}}; + std::string FunkyStore::variantToString( std::variant& variant) { return std::visit( @@ -75,11 +78,17 @@ std::string trim(const std::string& str) { return (start < end.base()) ? std::string(start, end.base()) : std::string(); } -std::map> - FunkyStore::prefs = {{"Default.int", 0.0}}; - bool FunkyStore::ContainsKey(std::string key) { - return (prefs.find(key) != prefs.end()); + if (!hasReadPrefs) { + return false; + } + try { + if (prefs.empty()) return false; + return (prefs.find(key) != prefs.end()); + } catch (const std::exception& exc) { + (void)exc; + } + return false; } void FunkyStore::SetDouble(std::string key, double val) { diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc index eaa203f..00150a3 100644 --- a/src/frc846/cpp/frc846/robot/GenericRobot.cc +++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc @@ -22,7 +22,7 @@ namespace frc846::robot { GenericRobot::GenericRobot(GenericRobotContainer* container) - : frc846::base::Loggable{"robot"}, generic_robot_container_{container} { + : frc846::base::Loggable{"Robot"}, generic_robot_container_{container} { next_loop_time_ = frc846::wpilib::CurrentFPGATime(); int32_t status = 0x00; @@ -119,10 +119,12 @@ void GenericRobot::StartCompetition() { loop.Clear(); } else if (mode == Mode::kAutonomous) { // Get and run selected auto command - auto_command_ = auto_chooser_.GetSelected(); + std::string option_name = auto_chooser_.GetSelected(); + auto_command_ = autos_[option_name]; if (auto_command_ != nullptr) { - Log("Running auto: {}", auto_command_->GetName()); + Log("Running auto: {}", option_name); + auto_command_->Schedule(); } else { Error("Auto command null!"); @@ -152,6 +154,8 @@ void GenericRobot::StartCompetition() { last_mode_ = mode; } + OnPeriodic(); + // Update subsystem readings generic_robot_container_->UpdateReadings(); @@ -195,12 +199,9 @@ void GenericRobot::VerifyHardware() { generic_robot_container_->VerifyHardware(); } -void GenericRobot::AddAutos(frc2::Command* defaultOption, - std::vector otherOptions) { - auto_chooser_.SetDefaultOption(defaultOption->GetName(), defaultOption); - for (auto option : otherOptions) { - auto_chooser_.AddOption(option->GetName(), option); - } +void GenericRobot::AddAuto(std::string name, frc2::Command* command) { + auto_chooser_.AddOption(name, name); + autos_[name] = command; frc::SmartDashboard::PutData(&auto_chooser_); frc::SmartDashboard::UpdateValues(); } diff --git a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc index 2a5830f..0ab5325 100644 --- a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc +++ b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc @@ -3,39 +3,58 @@ namespace frc846::swerve { FollowTrajectoryCommand::FollowTrajectoryCommand( - RobotContainer& container, std::vector input_points) + RobotContainer& container, std::vector input_points, + int mirror) : frc846::robot::GenericCommand< RobotContainer, FollowTrajectoryCommand>{container, "follow_trajectory_command"}, - input_points_(input_points) { + input_points_(input_points), + mirror_(mirror) { AddRequirements({&container_.drivetrain_}); - - wtcalculator.setConstants({container_.drivetrain_.max_speed_.value(), - container_.drivetrain_.max_acceleration_.value(), - 2_in, 20_ms}); } void FollowTrajectoryCommand::OnInit() { + wtcalculator.setConstants({container_.drivetrain_.auto_max_speed_.value(), + container_.drivetrain_.max_acceleration_.value(), + container_.drivetrain_.max_deceleration_.value(), + container_.drivetrain_.max_speed_.value(), + 80_fps_sq, 80_fps_sq, 3_in, 20_ms}); + Log("Starting Trajectory"); - for (frc846::Waypoint i : input_points_) { - Log("points x{} y{} bearing {}", i.point[0], i.point[1], i.bearing); - } - Log("initial pose x{}, y{}, bearing {}", + Log("Initial pose x{}, y{}, Bearing {}", container_.drivetrain_.GetReadings().pose.point[0], container_.drivetrain_.GetReadings().pose.point[1], container_.drivetrain_.GetReadings().pose.bearing); - is_done_ = false; start_time_ = frc846::wpilib::CurrentFPGATime(); - input_points_.insert(input_points_.begin(), 1, - {container_.drivetrain_.GetReadings().pose}); + auto vision_readings = container_.vision_.GetReadings(); + auto dtpose = container_.drivetrain_.GetReadings().pose; + auto pose = + (container_.vision_.using_vision_autos_.value() + + ? frc846::math::FieldPoint{{vision_readings.x_pos, + vision_readings.y_pos}, + dtpose.bearing, + {dtpose.velocity[0], dtpose.velocity[1]}} + : dtpose); + + path_points_.clear(); + path_points_.push_back(pose); + for (auto x : input_points_) { + path_points_.push_back( + mirror_ == 0 ? x : (mirror_ == 1 ? x.mirror() : x.mirrorOnlyY())); + } + for (frc846::Waypoint i : path_points_) { + Log("Points x{} y{} Bearing {}", i.point[0], i.point[1], i.bearing); + } + + is_done_ = false; target_idx_ = 1; - if (input_points_.size() < 2) { - Error("trajectory size ({}) is less than 2 - ending!", - input_points_.size()); + if (path_points_.size() < 2) { + Error("Trajectory size ({}) is less than 2 - ending!", path_points_.size()); is_done_ = true; } } @@ -46,22 +65,38 @@ void FollowTrajectoryCommand::Periodic() { return; } + auto vision_readings = container_.vision_.GetReadings(); + auto dtpose = container_.drivetrain_.GetReadings().pose; + auto pose = + (container_.vision_.using_vision_autos_.value() + + ? frc846::math::FieldPoint{{vision_readings.x_pos, + vision_readings.y_pos}, + dtpose.bearing, + {dtpose.velocity[0], dtpose.velocity[1]}} + : dtpose); + frc846::swerve::WTCInput lpcinput{ - input_points_[target_idx_ - 1].point, - input_points_[target_idx_].point, - container_.drivetrain_.GetReadings().pose.point, - container_.drivetrain_.GetReadings().pose.bearing, - input_points_[target_idx_].bearing, - container_.drivetrain_.GetReadings().pose.velocity.magnitude(), - input_points_[target_idx_].velocity.magnitude()}; + path_points_[target_idx_ - 1].point, + path_points_[target_idx_].point, + pose.point, + pose.bearing, + path_points_[target_idx_].bearing, + pose.velocity.magnitude(), + path_points_[target_idx_].velocity.magnitude()}; frc846::swerve::WTCOutput lpcoutput = wtcalculator.calculate(lpcinput); if (lpcoutput.crossed_waypt) { + Log("Initial x{} y{}.", path_points_[target_idx_ - 1].point[0], + path_points_[target_idx_ - 1].point[1]); + Log("Target x{} y{}.", path_points_[target_idx_].point[0], + path_points_[target_idx_].point[1]); target_idx_++; - Log("Cross waypoint - now on {}/{}", target_idx_ + 1, input_points_.size()); + Log("Cross waypoint - now on {}/{}", target_idx_ + 1, path_points_.size()); + Log("Position x{} y{}", pose.point[0], pose.point[1]); - if (target_idx_ == input_points_.size()) { + if (target_idx_ == path_points_.size()) { Log("Done!"); is_done_ = true; return; @@ -72,8 +107,8 @@ void FollowTrajectoryCommand::Periodic() { target.v_x = lpcoutput.target_vel[0]; target.v_y = lpcoutput.target_vel[1]; target.translation_reference = DrivetrainTranslationReference::kField; - target.rotation = DrivetrainRotationPosition(lpcoutput.bearing); - target.control = kClosedLoop; + target.rotation = DrivetrainRotationVelocity(lpcoutput.rotational_vel); + target.control = kOpenLoop; container_.drivetrain_.SetTarget(target); } diff --git a/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc b/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc index d4d6f9b..917f56a 100644 --- a/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc +++ b/src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc @@ -3,22 +3,22 @@ namespace frc846::swerve { bool WayptTraversalCalculator::HasCrossedWaypt(WTCInput input) { - auto disp_vec = (input.current_pos - input.start_pos); + auto full_disp_vec = (input.target_pos - input.start_pos); - auto target_vec = (input.target_pos - input.start_pos); + auto current_disp_vec = (input.current_pos - input.start_pos); - if (target_vec.magnitude() <= constants_.loc_tolerance) { + if (full_disp_vec.magnitude() <= constants_.loc_tolerance) { return true; } - return disp_vec.projectOntoAnother(target_vec).magnitude() >= - target_vec.magnitude(); + return current_disp_vec.magnitude() >= full_disp_vec.magnitude(); }; WTCOutput WayptTraversalCalculator::calculate(WTCInput input) { WTCOutput result{}; result.target_vel = {0.0_fps, 0.0_fps}; - result.bearing = 0_deg; + result.rotational_vel = units::degrees_per_second_t{0.0}; + result.crossed_waypt = false; auto delta_vec = (input.target_pos - input.current_pos); @@ -33,32 +33,52 @@ WTCOutput WayptTraversalCalculator::calculate(WTCInput input) { auto dir_vec = delta_vec.unit(); - units::second_t stopping_time = - units::math::abs(input.current_vel - input.target_vel) / - constants_.max_acceleration; - units::foot_t stopping_distance = - (input.current_vel + input.target_vel) / 2 * stopping_time; + if (delta_vec.magnitude() < 4 * constants_.loc_tolerance) { + dir_vec = prev_dir_vec; + } + + prev_dir_vec = dir_vec; - result.bearing = (input.target_bearing - input.current_bearing) * - constants_.loop_time / stopping_time; + units::second_t reach_time_min = + delta_vec.magnitude() / + units::math::abs(input.current_vel - input.target_vel); + units::feet_per_second_squared_t required_deceleration = + units::math::abs(input.current_vel - input.target_vel) / reach_time_min; - auto target_vel_mag = constants_.max_speed; + result.rotational_vel = + (input.target_bearing - input.current_bearing) / reach_time_min; - if (stopping_distance > delta_vec.magnitude() - constants_.loc_tolerance) { - target_vel_mag = - input.current_vel + (input.target_vel - input.current_vel) * - constants_.loop_time / stopping_time; + units::feet_per_second_squared_t target_acc{0.0}; + if (required_deceleration >= constants_.max_deceleration) { + target_acc = -required_deceleration; } else if (input.current_vel < constants_.max_speed) { - target_vel_mag = - input.current_vel + constants_.max_acceleration * constants_.loop_time; + target_acc = constants_.max_acceleration; } - target_vel_mag = units::math::min(target_vel_mag, constants_.max_speed); - result.target_vel = {dir_vec[0].to() * target_vel_mag, - dir_vec[1].to() * target_vel_mag}; + units::feet_per_second_t target_dc = + acceleration_to_dc(target_acc, input.current_vel); + + result.target_vel = {dir_vec[0].to() * target_dc, + dir_vec[1].to() * target_dc}; return result; } +units::feet_per_second_t WayptTraversalCalculator::acceleration_to_dc( + units::feet_per_second_squared_t acc, units::feet_per_second_t vel) { + double current_vel_pct = vel / constants_.true_max_spd; + + double acc_pct = acc / ((acc >= units::feet_per_second_squared_t(0.0)) + ? constants_.true_max_acc + : constants_.true_max_dec); + + double dc = current_vel_pct + acc_pct; + + if (dc <= 0.0) dc = 0.0; + + return units::math::min(constants_.true_max_spd, + dc * constants_.true_max_spd); +} + }; // namespace frc846::swerve \ No newline at end of file diff --git a/src/frc846/include/frc846/control/motion.h b/src/frc846/include/frc846/control/motion.h index e997d79..8454ca0 100644 --- a/src/frc846/include/frc846/control/motion.h +++ b/src/frc846/include/frc846/control/motion.h @@ -5,6 +5,8 @@ #include #include +#include + #include "config.h" #include "frc846/ntinf/pref.h" #include "frc846/util/share_tables.h" @@ -118,8 +120,8 @@ class BrakingPositionDyFPID { double error = target_pos_capped - current_pos.template to(); - double target_output = g.kF * prop_ff_function_(current_pos) + - g.kP * error + g.kD * current_velocity_percentage; + double target_output = (g.kF * prop_ff_function_(current_pos) + + g.kP * error + g.kD * current_velocity_percentage); return current_control_.calculate(current_velocity_percentage, target_output); diff --git a/src/frc846/include/frc846/math/fieldpoints.h b/src/frc846/include/frc846/math/fieldpoints.h index b8efe08..db89644 100644 --- a/src/frc846/include/frc846/math/fieldpoints.h +++ b/src/frc846/include/frc846/math/fieldpoints.h @@ -46,12 +46,19 @@ struct FieldPoint { class FieldPointPreference { public: + std::string name_; + FieldPointPreference(std::string name, FieldPoint backup) - : x{table_name_, name + "_x", backup.point[0]}, - y{table_name_, name + "_y", backup.point[1]}, - bearing{table_name_, name + "_deg", backup.bearing}, - v_x{table_name_, name + "_v_x", backup.velocity[0]}, - v_y{table_name_, name + "_v_y", backup.velocity[1]} {} + : name_{name}, + x{table_loggable_, name + "_x", backup.point[0]}, + y{table_loggable_, name + "_y", backup.point[1]}, + bearing{table_loggable_, name + "_deg", backup.bearing}, + v_x{table_loggable_, name + "_v_x", backup.velocity[0]}, + v_y{table_loggable_, name + "_v_y", backup.velocity[1]} {} + + ~FieldPointPreference() { + table_loggable_.Warn("FieldPointPreference destructor was called."); + } FieldPoint get() { return { @@ -65,7 +72,7 @@ class FieldPointPreference { frc846::ntinf::Pref v_x; frc846::ntinf::Pref v_y; - const std::string table_name_ = "Preferences/field_points"; + frc846::base::Loggable table_loggable_{"field_points_2"}; }; } // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/ntinf/pref.h b/src/frc846/include/frc846/ntinf/pref.h index 80255f4..c08cd3d 100644 --- a/src/frc846/include/frc846/ntinf/pref.h +++ b/src/frc846/include/frc846/ntinf/pref.h @@ -22,17 +22,21 @@ class Pref { // Construct a new pref for a unit type with a fallback value. // // The pref name will be post-fixed with the unit (e.g. `length (in)`). - Pref(const frc846::base::Loggable& parent, std::string name, T fallback); - Pref(const frc846::base::Loggable& parent, std::string name); + Pref(frc846::base::Loggable& parent, std::string name, T fallback); + Pref(frc846::base::Loggable& parent, std::string name); - ~Pref() { pref_table_->RemoveListener(entry_listener_); } + ~Pref() { + parent_.Warn("Preference {} destructor was called.", f_ptr_.key_); + pref_table_->RemoveListener(entry_listener_); + } private: - Pref(const frc846::base::Loggable& parent, std::string name, T fallback, + Pref(frc846::base::Loggable& parent, std::string name, T fallback, std::function init, std::function set, std::function get) - : f_ptr_{frc846::base::Loggable::Join(parent.name(), name)} { + : parent_{parent}, + f_ptr_{frc846::base::Loggable::Join(parent.name(), name)} { FunkyStore::FP_HardReadPrefs(); // Full networktables key (parent name + pref name) auto full_key = frc846::base::Loggable::Join(parent.name(), name); @@ -62,6 +66,8 @@ class Pref { T value() const { return value_; } private: + frc846::base::Loggable& parent_; + T value_; NT_Listener entry_listener_; @@ -73,8 +79,7 @@ class Pref { }; template -Pref::Pref(const frc846::base::Loggable& parent, std::string name, - U fallback) +Pref::Pref(frc846::base::Loggable& parent, std::string name, U fallback) : Pref{parent, fmt::format("{} ({})", name, units::abbreviation(units::make_unit(0))), @@ -94,11 +99,11 @@ Pref::Pref(const frc846::base::Loggable& parent, std::string name, } template -Pref::Pref(const frc846::base::Loggable& parent, std::string name) +Pref::Pref(frc846::base::Loggable& parent, std::string name) : Pref{parent, name, units::abbreviation(units::make_unit(0))} {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, std::string name, +inline Pref::Pref(frc846::base::Loggable& parent, std::string name, bool fallback) : Pref{ parent, @@ -114,12 +119,12 @@ inline Pref::Pref(const frc846::base::Loggable& parent, std::string name, } {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, std::string name) +inline Pref::Pref(frc846::base::Loggable& parent, std::string name) : Pref{parent, name, false} {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, - std::string name, double fallback) +inline Pref::Pref(frc846::base::Loggable& parent, std::string name, + double fallback) : Pref{ parent, name, @@ -134,12 +139,11 @@ inline Pref::Pref(const frc846::base::Loggable& parent, } {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, - std::string name) +inline Pref::Pref(frc846::base::Loggable& parent, std::string name) : Pref{parent, name, 0.0} {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, std::string name, +inline Pref::Pref(frc846::base::Loggable& parent, std::string name, int fallback) : Pref{ parent, @@ -155,12 +159,12 @@ inline Pref::Pref(const frc846::base::Loggable& parent, std::string name, } {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, std::string name) +inline Pref::Pref(frc846::base::Loggable& parent, std::string name) : Pref{parent, name, 0} {} template <> -inline Pref::Pref(const frc846::base::Loggable& parent, - std::string name, std::string fallback) +inline Pref::Pref(frc846::base::Loggable& parent, std::string name, + std::string fallback) : Pref{ parent, name, diff --git a/src/frc846/include/frc846/robot/GenericCommand.h b/src/frc846/include/frc846/robot/GenericCommand.h index 4bb7b66..1812a5a 100644 --- a/src/frc846/include/frc846/robot/GenericCommand.h +++ b/src/frc846/include/frc846/robot/GenericCommand.h @@ -91,19 +91,10 @@ class GenericCommandGroup : Loggable{name}, container_{container} { frc2::Command::SetName(name); - // auto start_command_added = frc2::InstantCommand([&] { - // Log("Command group {} starting.", name); - // command_start_time_ = frc846::wpilib::CurrentFPGATime(); - // }); - // auto end_command_added = frc2::InstantCommand([&] { - // units::millisecond_t elapsed_time = - // frc846::wpilib::CurrentFPGATime() - command_start_time_; - // Log("Command group {} ending. Took {} ms to complete.", name, - // elapsed_time.to()); - // }); - + frc2::SequentialCommandGroup::AddCommands(start_command_addition); frc2::SequentialCommandGroup::AddCommands( std::forward(commands)...); + frc2::SequentialCommandGroup::AddCommands(end_command_addition); Log("Constructing instance of command group {}.", name); } @@ -111,6 +102,17 @@ class GenericCommandGroup protected: RobotContainer& container_; units::millisecond_t command_start_time_ = 0.0_ms; + + private: + frc2::InstantCommand end_command_addition{[&] { + Log("Command group ending. Took {} ms to complete.", + (frc846::wpilib::CurrentFPGATime() - command_start_time_).to()); + }}; + + frc2::InstantCommand start_command_addition{[&] { + Log("Command group starting."); + command_start_time_ = frc846::wpilib::CurrentFPGATime(); + }}; }; }; // namespace frc846::robot diff --git a/src/frc846/include/frc846/robot/GenericRobot.h b/src/frc846/include/frc846/robot/GenericRobot.h index a8b09b3..6980a34 100644 --- a/src/frc846/include/frc846/robot/GenericRobot.h +++ b/src/frc846/include/frc846/robot/GenericRobot.h @@ -38,8 +38,7 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable { void VerifyHardware(); - void AddAutos(frc2::Command* defaultOption, - std::vector otherOptions); + void AddAuto(std::string name, frc2::Command* command); private: hal::Handle notifier_; @@ -60,7 +59,8 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable { "loop_time"}; frc2::Command* auto_command_ = nullptr; - frc::SendableChooser auto_chooser_; + frc::SendableChooser auto_chooser_; + std::unordered_map autos_; frc846::ntinf::FunkyStore robotStore{}; }; diff --git a/src/frc846/include/frc846/swerve/follow_trajectory_command.h b/src/frc846/include/frc846/swerve/follow_trajectory_command.h index 58f4215..86173a1 100644 --- a/src/frc846/include/frc846/swerve/follow_trajectory_command.h +++ b/src/frc846/include/frc846/swerve/follow_trajectory_command.h @@ -11,7 +11,8 @@ class FollowTrajectoryCommand FollowTrajectoryCommand> { public: FollowTrajectoryCommand(RobotContainer& container, - std::vector input_points); + std::vector input_points, + int mirror = 0); void OnInit() override; @@ -23,8 +24,11 @@ class FollowTrajectoryCommand private: std::vector input_points_; + std::vector path_points_{}; unsigned int target_idx_; + int mirror_; + bool is_done_ = false; units::second_t start_time_; diff --git a/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h b/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h index 4d84dc1..81d77b0 100644 --- a/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h +++ b/src/frc846/include/frc846/swerve/waypt_traversal_calculator.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -30,13 +31,18 @@ struct WTCInput { struct WTCOutput { bool crossed_waypt; - units::degree_t bearing; + units::degrees_per_second_t rotational_vel; frc846::math::VectorND target_vel; }; struct WTCConstants { units::feet_per_second_t max_speed; units::feet_per_second_squared_t max_acceleration; + units::feet_per_second_squared_t max_deceleration; + + units::feet_per_second_t true_max_spd; + units::feet_per_second_squared_t true_max_acc; + units::feet_per_second_squared_t true_max_dec; units::inch_t loc_tolerance; @@ -49,6 +55,12 @@ class WayptTraversalCalculator bool HasCrossedWaypt(WTCInput input); WTCOutput calculate(WTCInput input) override; + + private: + units::feet_per_second_t acceleration_to_dc( + units::feet_per_second_squared_t acc, units::feet_per_second_t vel); + + frc846::math::VectorND prev_dir_vec; }; }; // namespace frc846::swerve \ No newline at end of file diff --git a/src/test/cpp/arch/swerve/waypt_traversal_calculator_test.cpp b/src/test/cpp/arch/swerve/waypt_traversal_calculator_test.cpp new file mode 100644 index 0000000..7aae025 --- /dev/null +++ b/src/test/cpp/arch/swerve/waypt_traversal_calculator_test.cpp @@ -0,0 +1,145 @@ +#include "frc846/swerve/waypt_traversal_calculator.h" + +#include "gtest/gtest.h" + +class WayptTraversalCalculatorTest : public ::testing::Test { + protected: + frc846::swerve::WTCConstants constants_; + WayptTraversalCalculatorTest() + : calculator_{}, + constants_{13.0_fps, 16.0_fps_sq, 16.0_fps_sq, 15.0_fps, + 100.0_fps_sq, 100.0_fps_sq, 3.0_in, 20_ms} { + calculator_.setConstants(constants_); + } + + frc846::swerve::WayptTraversalCalculator calculator_; +}; + +TEST_F(WayptTraversalCalculatorTest, HasCrossedWaypt) { + frc846::swerve::WTCInput input{{0.0_ft, 0.0_ft}, + {10.0_ft, 0.0_ft}, + {0.0_ft, 0.0_ft}, + 0_deg, + 0_deg, + 0.0_fps, + 0.0_fps}; + + EXPECT_FALSE(calculator_.HasCrossedWaypt(input)); + + input.current_pos = {5.0_ft, 0.0_ft}; + + EXPECT_FALSE(calculator_.HasCrossedWaypt(input)); + + input.current_pos = {10.0_ft, 0.0_ft}; + + EXPECT_TRUE(calculator_.HasCrossedWaypt(input)); + + input.current_pos = {14.0_ft, 0.0_ft}; + + EXPECT_TRUE(calculator_.HasCrossedWaypt(input)); +} + +TEST_F(WayptTraversalCalculatorTest, ReqsDecel) { + frc846::swerve::WTCInput input{{0.0_ft, 0.0_ft}, + {10.0_ft, 0.0_ft}, + {0.0_ft, 0.0_ft}, + 0_deg, + 0_deg, + 10.0_fps, + 0.0_fps}; + + frc846::swerve::WTCOutput output = calculator_.calculate(input); + + EXPECT_TRUE(output.target_vel.magnitude() > 10.0_fps); + + input.current_pos = {9.0_ft, 0.0_ft}; + + output = calculator_.calculate(input); + + EXPECT_TRUE(output.target_vel.magnitude() < 10.0_fps); +} + +TEST_F(WayptTraversalCalculatorTest, ReqsAccel) { + frc846::swerve::WTCInput input{{0.0_ft, 0.0_ft}, + {10.0_ft, 0.0_ft}, + {0.0_ft, 0.0_ft}, + 0_deg, + 0_deg, + 2.0_fps, + 0.0_fps}; + + frc846::swerve::WTCOutput output = calculator_.calculate(input); + + EXPECT_TRUE(output.target_vel.magnitude() > 2.0_fps); + + input.current_pos = {5.0_ft, 0.0_ft}; + + output = calculator_.calculate(input); + + EXPECT_TRUE(output.target_vel.magnitude() > 2.0_fps); + + input.current_vel = 4.0_fps; + input.current_pos = {9.5_ft, 0.0_ft}; + + output = calculator_.calculate(input); + + EXPECT_FALSE(output.target_vel.magnitude() > 2.0_fps); +} + +TEST_F(WayptTraversalCalculatorTest, FullPathFakeTest) { + frc846::swerve::WTCInput input{{0.0_ft, 0.0_ft}, + {10.0_ft, 0.0_ft}, + {0.0_ft, 0.0_ft}, + 0_deg, + 0_deg, + 0.0_fps, + 0.0_fps}; + + std::cout << "\n-----------------------------------" << std::endl; + std::cout << "Starting test [FullPathFakeTest]\n" << std::endl; + + auto loop_time = 0.02_s; + + auto total_time = 0.0_s; + + for (int i{0}; i <= 100; i++) { + auto output = calculator_.calculate(input); + + double current_dc = input.current_vel / constants_.true_max_spd; + double target_dc = output.target_vel.magnitude() / constants_.true_max_spd; + + double acc_dc = (target_dc - current_dc); + auto acc = acc_dc * + (acc_dc > 0 ? constants_.true_max_acc : constants_.true_max_dec); + + if (output.crossed_waypt) { + std::cout << "\nReached waypoint\n" << std::endl; + break; + } + + auto delta_vec_unit = (input.target_pos - input.current_pos).unit(); + auto est_vel = (input.current_vel + acc * loop_time); + + frc846::math::VectorND est_vel_vec{ + est_vel * delta_vec_unit[0].to(), + est_vel * delta_vec_unit[1].to()}; + + input.current_vel = est_vel_vec.magnitude(); + + input.current_pos += + {est_vel_vec[0] * loop_time, est_vel_vec[1] * loop_time}; + + total_time += loop_time; + + std::cout << "Time [" << total_time.to() << "] - Position [" + << input.current_pos[0].to() << ", " + << input.current_pos[1].to() << "]"; + std::cout << " - Velocity [" << est_vel_vec[0].to() << ", " + << est_vel_vec[1].to() << "]" << std::endl; + } + + std::cout << "End of test [FullPathFakeTest]" << std::endl; + std::cout << "-----------------------------------\n" << std::endl; + + EXPECT_TRUE(true); +} \ No newline at end of file diff --git a/src/test/cpp/main.cpp b/src/test/cpp/main.cpp new file mode 100644 index 0000000..0ec960c --- /dev/null +++ b/src/test/cpp/main.cpp @@ -0,0 +1,10 @@ +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} \ No newline at end of file diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index dbbc297..223f6d9 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -22,12 +23,23 @@ #include "commands/teleop/operator_control.h" #include "commands/teleop/stow_command.h" #include "control_triggers.h" +#include "field.h" #include "frc846/ntinf/ntaction.h" #include "frc846/swerve/follow_trajectory_command.h" +#include "rsighandler.h" FunkyRobot::FunkyRobot() : GenericRobot{&container_} {} void FunkyRobot::OnInitialize() { + Field::Setup(); + + for (auto x : Field::getAllAutoData()) { + Log("Adding Auto: {}", x.name + "_red"); + AddAuto(x.name + "_red", new GenericAuto{container_, x, false}); + Log("Adding Auto: {}", x.name + "_blue"); + AddAuto(x.name + "_blue", new GenericAuto{container_, x, true}); + } + // Add dashboard buttons frc::SmartDashboard::PutData( "zero_modules", new frc846::ntinf::NTAction( @@ -59,11 +71,6 @@ void FunkyRobot::OnInitialize() { container_.telescope_.Brake(); container_.wrist_.Brake(); })); - - AddAutos(five_piece_auto_red.get(), - {five_piece_auto_blue.get(), one_piece_auto_0.get(), - one_piece_auto_1.get(), one_piece_auto_2.get(), - one_piece_auto_3.get(), drive_auto_.get()}); } void FunkyRobot::OnDisable() { @@ -80,8 +87,7 @@ void FunkyRobot::InitTeleop() { container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); container_.control_input_.SetDefaultCommand( OperatorControlCommand{container_}); - container_.super_structure_.SetDefaultCommand( - StowZeroActionCommand{container_}); + container_.super_structure_.SetDefaultCommand(StowCommand{container_}); container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_}); container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); container_.bracer_.SetDefaultCommand(BracerCommand{container_}); @@ -95,20 +101,36 @@ void FunkyRobot::OnPeriodic() { if (homing_switch_.Get()) { container_.super_structure_.ZeroSubsystem(); + Log("Zeroing subsystems..."); } - if (coast_counter_ >= 1) { - coast_counter_--; - } else if (coast_counter_ == 0) { + if (frc846::wpilib::CurrentFPGATime() > stop_coast_time_) { container_.pivot_.Brake(); container_.wrist_.Brake(); container_.telescope_.Brake(); + + stop_coast_time_ = + frc846::wpilib::CurrentFPGATime() + + coasting_time_ + .value(); // To prevent Brake from being called each periodic + } else if (coasting_switch_.Get()) { container_.pivot_.Coast(); container_.wrist_.Coast(); container_.telescope_.Coast(); - coast_counter_ = start_coast_counter_.value(); + + stop_coast_time_ = + frc846::wpilib::CurrentFPGATime() + coasting_time_.value(); + + Log("Coasting subsystems..."); } } void FunkyRobot::InitTest() {} + +#ifndef RUNNING_FRC_TESTS +int main() { + // configureSignalHandlers(); + return frc::StartRobot(); +} +#endif \ No newline at end of file diff --git a/src/y2024/cpp/autos/ActionMaker.cc b/src/y2024/cpp/autos/ActionMaker.cc new file mode 100644 index 0000000..2f17d13 --- /dev/null +++ b/src/y2024/cpp/autos/ActionMaker.cc @@ -0,0 +1,20 @@ +#include "autos/ActionMaker.h" + +#include "commands/basic/auto_deploy_intake_command.h" +#include "commands/basic/auto_shoot_command.h" +#include "commands/basic/prepare_auto_shoot_command.h" +#include "commands/complex/home_during_auto_command.h" + +std::unique_ptr ActionMaker::GetAction( + std::string name, RobotContainer& container) { + if (name == "shoot") { + return std::make_unique(container); + } else if (name == "prep_shoot") { + return std::make_unique(container); + } else if (name == "deploy_intake") { + return std::make_unique(container); + } else if (name == "auto_home" || true) { + return std::make_unique(container); + } + return nullptr; +} \ No newline at end of file diff --git a/src/y2024/cpp/autos/GenericAuto.cc b/src/y2024/cpp/autos/GenericAuto.cc new file mode 100644 index 0000000..465ea13 --- /dev/null +++ b/src/y2024/cpp/autos/GenericAuto.cc @@ -0,0 +1,41 @@ +#include "autos/GenericAuto.h" + +#include + +#include "autos/ActionMaker.h" + +GenericAuto::GenericAuto(RobotContainer& container, AutoData data, + bool is_blue_side) + : frc846::robot::GenericCommandGroup{ + container, data.name, + frc2::SequentialCommandGroup{ + std::move(buildActionsGroup(data, container, is_blue_side))}} {} + +std::vector> GenericAuto::buildActionsGroup( + AutoData data, RobotContainer& container, bool is_blue_side) { + std::vector> cmds{}; + cmds.push_back(std::make_unique([&, auto_data = data] { + Log("Starting Auto: {}.", auto_data.name); + + int mirror = is_blue_side ? (int)auto_data.blue : (int)auto_data.red; + + auto start = mirror == 0 ? auto_data.start + : (mirror == 1 ? auto_data.start.mirror() + : auto_data.start.mirrorOnlyY()); + + container.drivetrain_.SetPoint(start.point); + container.drivetrain_.SetBearing(start.bearing); + })); + for (auto& action : data.actions) { + if (auto* action_name = std::get_if(&action)) { + cmds.push_back(ActionMaker::GetAction(*action_name, container)); + } else if (auto* fp = std::get_if>( + &action)) { + cmds.push_back(std::make_unique( + container, *fp, is_blue_side ? (int)data.blue : (int)data.red)); + } + } + + return cmds; +} \ No newline at end of file diff --git a/src/y2024/cpp/autos/drive_auto.cc b/src/y2024/cpp/autos/drive_auto.cc deleted file mode 100644 index 500365e..0000000 --- a/src/y2024/cpp/autos/drive_auto.cc +++ /dev/null @@ -1,27 +0,0 @@ -#include "autos/drive_auto.h" - -#include -#include -#include -#include - -#include "field.h" -#include "frc2/command/WaitCommand.h" -#include "frc2/command/WaitUntilCommand.h" -#include "frc846/swerve/follow_trajectory_command.h" - -DriveAuto::DriveAuto(RobotContainer& container) - : frc846::robot::GenericCommandGroup{ - container, "drive_auto", - frc2::SequentialCommandGroup{ - - frc2::InstantCommand{[&] { - auto pose_ = field::points.kTestingOrigin(); - container.drivetrain_.SetPoint(pose_.point); - container.drivetrain_.SetBearing(pose_.bearing); - }}, - frc846::swerve::FollowTrajectoryCommand{ - container, {field::points.kTestingPoint()}} - - }} {} \ No newline at end of file diff --git a/src/y2024/cpp/autos/five_piece_auto.cc b/src/y2024/cpp/autos/five_piece_auto.cc deleted file mode 100644 index d4ed87f..0000000 --- a/src/y2024/cpp/autos/five_piece_auto.cc +++ /dev/null @@ -1,58 +0,0 @@ -#include "autos/five_piece_auto.h" - -#include -#include -#include -#include -#include -#include - -#include "commands/auto_intake_and_shoot_command.h" -#include "commands/basic/auto_shoot_command.h" -#include "commands/basic/prepare_auto_shoot_command.h" -#include "commands/basic/spin_up_command.h" -#include "commands/basic/wrist_zero_command.h" -#include "commands/teleop/stow_command.h" -#include "field.h" -#include "frc846/swerve/follow_trajectory_command.h" - -FivePieceAuto::FivePieceAuto(RobotContainer& container, bool should_flip) - : frc846::robot::GenericCommandGroup{ - container, should_flip ? "5p_auto_blue" : "5p_auto_red", - frc2::SequentialCommandGroup{ - - frc2::InstantCommand{[&, flip = should_flip] { - auto pose_ = field::points.kFivePieceOrigin(flip); - container.drivetrain_.SetPoint(pose_.point); - container.drivetrain_.SetBearing(flip ? 180_deg : 0_deg); - }}, - frc2::ParallelDeadlineGroup{ - frc2::ParallelDeadlineGroup{ - frc2::WaitUntilCommand{[&] { - return container_.pivot_.WithinTolerance( - container_.super_structure_.getStowSetpoint() - .pivot); /* homing wrist at start of auto */ - }}, - StowCommand{container}}, - SpinUpCommand{ - container}}, /* start to spin up shooter to save time */ - WristZeroCommand{container}, /* homing the wrist */ - PrepareAutoShootCommand{container}, AutoShootCommand{container}, - frc2::WaitCommand{ - container.super_structure_.post_shoot_wait_.value()}, - - AutoIntakeAndShootCommand( - container, field::points.intake_one_path(should_flip), - {field::points.kFivePieceOrigin(should_flip)}), - - AutoIntakeAndShootCommand( - container, field::points.intake_two_path(should_flip), - {field::points.kFivePieceOrigin(should_flip)}), - - AutoIntakeAndShootCommand( - container, field::points.intake_three_path(should_flip), - {field::points.kFivePieceOrigin(should_flip)}), - - frc846::swerve::FollowTrajectoryCommand( - container, {field::points.kFivePieceFinish(should_flip)})}} {} diff --git a/src/y2024/cpp/autos/one_piece.cc b/src/y2024/cpp/autos/one_piece.cc deleted file mode 100644 index e168ac3..0000000 --- a/src/y2024/cpp/autos/one_piece.cc +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include -#include -#include - -#include "autos/one_piece_auto.h" -#include "commands/basic/auto_shoot_command.h" -#include "commands/basic/prepare_auto_shoot_command.h" -#include "frc2/command/WaitCommand.h" - -OnePieceAuto::OnePieceAuto(RobotContainer& container, - units::degree_t start_angle, std::string num) - : frc846::robot::GenericCommandGroup{ - container, "1p_auto_" + num, - frc2::SequentialCommandGroup{ - - // frc2::InstantCommand{[&, s = start_angle] { - // container.drivetrain_.SetBearing(s); - // Log("OP: Zeroing to {}", s.to()); - // }}, - // PrepareAutoShootCommand{container}, - // AutoShootCommand{container}, - // frc2::WaitCommand{ - // container.super_structure_.post_shoot_wait_.value()} - - }} {} diff --git a/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc b/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc index bd6a4f4..0e3794d 100644 --- a/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc +++ b/src/y2024/cpp/commands/auto_intake_and_shoot_command.cc @@ -1,37 +1,37 @@ -#include "commands/auto_intake_and_shoot_command.h" +// #include "commands/auto_intake_and_shoot_command.h" -#include -#include -#include -#include +// #include +// #include +// #include +// #include -#include "commands/basic/auto_deploy_intake_command.h" -#include "commands/basic/auto_shoot_command.h" -#include "commands/basic/prepare_auto_shoot_command.h" -#include "field.h" -#include "frc2/command/WaitCommand.h" -#include "frc846/swerve/follow_trajectory_command.h" +// #include "commands/basic/auto_deploy_intake_command.h" +// #include "commands/basic/auto_shoot_command.h" +// #include "commands/basic/prepare_auto_shoot_command.h" +// #include "field.h" +// #include "frc2/command/WaitCommand.h" +// #include "frc846/swerve/follow_trajectory_command.h" -AutoIntakeAndShootCommand::AutoIntakeAndShootCommand( - RobotContainer& container, std::vector intake_path, - std::vector shoot_path) - : frc846::robot::GenericCommandGroup{ - container, "auto_intake_shoot_command", - frc2::SequentialCommandGroup{ +// AutoIntakeAndShootCommand::AutoIntakeAndShootCommand( +// RobotContainer& container, std::vector intake_path, +// std::vector shoot_path) +// : frc846::robot::GenericCommandGroup{ +// container, "auto_intake_shoot_command", +// frc2::SequentialCommandGroup{ - AutoDeployIntakeCommand{container}, - frc846::swerve::FollowTrajectoryCommand{container, intake_path}, - frc2::ParallelDeadlineGroup{ - frc2::SequentialCommandGroup{ - frc846::swerve::FollowTrajectoryCommand{container, - shoot_path}, - frc2::WaitCommand( - container.super_structure_.pre_shoot_wait_.value())}, - PrepareAutoShootCommand{container}}, - AutoShootCommand{container}, - frc2::WaitCommand( - container.super_structure_.post_shoot_wait_.value()) +// AutoDeployIntakeCommand{container}, +// frc846::swerve::FollowTrajectoryCommand{container, +// intake_path}, frc2::ParallelDeadlineGroup{ +// frc2::SequentialCommandGroup{ +// frc846::swerve::FollowTrajectoryCommand{container, +// shoot_path}, +// frc2::WaitCommand( +// container.super_structure_.pre_shoot_wait_.value())}, +// PrepareAutoShootCommand{container}}, +// AutoShootCommand{container}, +// frc2::WaitCommand( +// container.super_structure_.post_shoot_wait_.value()) - }} {} \ No newline at end of file +// }} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/close_drive_amp_command.cc b/src/y2024/cpp/commands/complex/close_drive_amp_command.cc index e6bf87f..5aee7d8 100644 --- a/src/y2024/cpp/commands/complex/close_drive_amp_command.cc +++ b/src/y2024/cpp/commands/complex/close_drive_amp_command.cc @@ -1,47 +1,49 @@ -#include "commands/complex/close_drive_amp_command.h" +// #include "commands/complex/close_drive_amp_command.h" -#include "field.h" +// #include "field.h" -CloseDriveAmpCommand::CloseDriveAmpCommand(RobotContainer& container) - : frc846::robot::GenericCommand{ - container, "close_drive_amp_command"} { - AddRequirements({&container_.drivetrain_}); -} +// CloseDriveAmpCommand::CloseDriveAmpCommand(RobotContainer& container) +// : frc846::robot::GenericCommand{ +// container, "close_drive_amp_command"} { +// AddRequirements({&container_.drivetrain_}); +// } -void CloseDriveAmpCommand::OnInit() {} +// void CloseDriveAmpCommand::OnInit() {} -void CloseDriveAmpCommand::Periodic() { - auto amp_point = field::points.kAmpNoFlip(); +// void CloseDriveAmpCommand::Periodic() { +// auto amp_point = field::points.kAmpNoFlip(); - DrivetrainTarget drivetrain_target; - drivetrain_target.rotation = DrivetrainRotationPosition(amp_point.bearing); - drivetrain_target.translation_reference = kField; - drivetrain_target.control = kClosedLoop; +// DrivetrainTarget drivetrain_target; +// drivetrain_target.rotation = DrivetrainRotationPosition(amp_point.bearing); +// drivetrain_target.translation_reference = kField; +// drivetrain_target.control = kClosedLoop; - auto amp_drive_dist = - (field::points.kPreAmpNoFlip().point - amp_point.point).magnitude(); +// auto amp_drive_dist = +// (field::points.kPreAmpNoFlip().point - amp_point.point).magnitude(); - auto max_speed = container_.drivetrain_.close_drive_amp_max_speed_.value(); +// auto max_speed = container_.drivetrain_.close_drive_amp_max_speed_.value(); - drivetrain_target.v_x = (amp_point.point[0] - - container_.drivetrain_.GetReadings().pose.point[0]) / - amp_drive_dist * max_speed; - drivetrain_target.v_y = (amp_point.point[1] - - container_.drivetrain_.GetReadings().pose.point[1]) / - amp_drive_dist * max_speed; +// drivetrain_target.v_x = (amp_point.point[0] - +// container_.drivetrain_.GetReadings().pose.point[0]) +// / +// amp_drive_dist * max_speed; +// drivetrain_target.v_y = (amp_point.point[1] - +// container_.drivetrain_.GetReadings().pose.point[1]) +// / +// amp_drive_dist * max_speed; - drivetrain_target.v_x = units::math::max( - -max_speed, units::math::min(max_speed, drivetrain_target.v_x)); - drivetrain_target.v_y = units::math::max( - -max_speed, units::math::min(max_speed, drivetrain_target.v_y)); +// drivetrain_target.v_x = units::math::max( +// -max_speed, units::math::min(max_speed, drivetrain_target.v_x)); +// drivetrain_target.v_y = units::math::max( +// -max_speed, units::math::min(max_speed, drivetrain_target.v_y)); - container_.drivetrain_.SetTarget(drivetrain_target); -} +// container_.drivetrain_.SetTarget(drivetrain_target); +// } -void CloseDriveAmpCommand::OnEnd(bool interrupted) {} +// void CloseDriveAmpCommand::OnEnd(bool interrupted) {} -bool CloseDriveAmpCommand::IsFinished() { - return (field::points.kAmpNoFlip().point - - container_.drivetrain_.GetReadings().pose.point) - .magnitude() <= 2_in; -} \ No newline at end of file +// bool CloseDriveAmpCommand::IsFinished() { +// return (field::points.kAmpNoFlip().point - +// container_.drivetrain_.GetReadings().pose.point) +// .magnitude() <= 2_in; +// } \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/home_during_auto_command.cc b/src/y2024/cpp/commands/complex/home_during_auto_command.cc new file mode 100644 index 0000000..1382317 --- /dev/null +++ b/src/y2024/cpp/commands/complex/home_during_auto_command.cc @@ -0,0 +1,25 @@ +#include "commands/complex/home_during_auto_command.h" + +#include +#include +#include +#include + +#include "commands/basic/wrist_zero_command.h" +#include "commands/teleop/stow_command.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +HomeDuringAutoCommand::HomeDuringAutoCommand(RobotContainer& container) + : frc846::robot::GenericCommandGroup{ + container, "home_during_auto_command", + frc2::SequentialCommandGroup{ + frc2::ParallelDeadlineGroup{ + frc2::WaitUntilCommand{[&] { + return container.pivot_.WithinTolerance( + container.super_structure_.getStowSetpoint().pivot); + }}, + StowCommand{container}}, + WristZeroCommand{container}, + }} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/stow_zero_action.cc b/src/y2024/cpp/commands/complex/stow_zero_action.cc index 621b182..dc86219 100644 --- a/src/y2024/cpp/commands/complex/stow_zero_action.cc +++ b/src/y2024/cpp/commands/complex/stow_zero_action.cc @@ -13,7 +13,7 @@ StowZeroActionCommand::StowZeroActionCommand(RobotContainer& container) : frc846::robot::GenericCommandGroup{ - container, "super_amp_command", + container, "stow_zero_action_command", frc2::SequentialCommandGroup{ frc2::ParallelDeadlineGroup{ frc2::WaitUntilCommand{[&] { diff --git a/src/y2024/cpp/commands/complex/super_amp_command.cc b/src/y2024/cpp/commands/complex/super_amp_command.cc index ef771bc..8a81961 100644 --- a/src/y2024/cpp/commands/complex/super_amp_command.cc +++ b/src/y2024/cpp/commands/complex/super_amp_command.cc @@ -1,40 +1,40 @@ -#include "commands/complex/super_amp_command.h" +// #include "commands/complex/super_amp_command.h" -#include -#include -#include -#include +// #include +// #include +// #include +// #include -#include "commands/basic/amp_command.h" -#include "commands/basic/await_piece_state_command.h" -#include "commands/basic/eject_command.h" -#include "commands/basic/pull_command.h" -#include "commands/complex/close_drive_amp_command.h" -#include "field.h" -#include "frc2/command/WaitCommand.h" -#include "frc2/command/WaitUntilCommand.h" -#include "frc846/swerve/follow_trajectory_command.h" +// #include "commands/basic/amp_command.h" +// #include "commands/basic/await_piece_state_command.h" +// #include "commands/basic/eject_command.h" +// #include "commands/basic/pull_command.h" +// #include "commands/complex/close_drive_amp_command.h" +// #include "field.h" +// #include "frc2/command/WaitCommand.h" +// #include "frc2/command/WaitUntilCommand.h" +// #include "frc846/swerve/follow_trajectory_command.h" -SuperAmpCommand::SuperAmpCommand(RobotContainer& container, bool is_red_side) - : frc846::robot::GenericCommandGroup{ - container, "super_amp_command", - frc2::SequentialCommandGroup{ +// SuperAmpCommand::SuperAmpCommand(RobotContainer& container, bool is_red_side) +// : frc846::robot::GenericCommandGroup{ +// container, "super_amp_command", +// frc2::SequentialCommandGroup{ - frc2::ParallelDeadlineGroup{frc2::WaitCommand{1_s}, - PullCommand{container}}, - frc2::ParallelDeadlineGroup{ - frc2::SequentialCommandGroup{ - frc846::swerve::FollowTrajectoryCommand{ - container, {{field::points.kPreAmpNoFlip()}}}, - CloseDriveAmpCommand{container}, - frc2::ParallelDeadlineGroup{ - frc2::SequentialCommandGroup{ - AwaitPieceStateCommand{container, false}, - frc2::WaitCommand{0.5_s}, - }, - EjectCommand{container}}, - }, - AmpCommand{container}}, - frc846::swerve::FollowTrajectoryCommand{ - container, {{field::points.kPreAmpNoFlip()}}}}} {} \ No newline at end of file +// frc2::ParallelDeadlineGroup{frc2::WaitCommand{1_s}, +// PullCommand{container}}, +// frc2::ParallelDeadlineGroup{ +// frc2::SequentialCommandGroup{ +// frc846::swerve::FollowTrajectoryCommand{ +// container, {{field::points.kPreAmpNoFlip()}}}, +// CloseDriveAmpCommand{container}, +// frc2::ParallelDeadlineGroup{ +// frc2::SequentialCommandGroup{ +// AwaitPieceStateCommand{container, false}, +// frc2::WaitCommand{0.5_s}, +// }, +// EjectCommand{container}}, +// }, +// AmpCommand{container}}, +// frc846::swerve::FollowTrajectoryCommand{ +// container, {{field::points.kPreAmpNoFlip()}}}}} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/teleop/drive_command.cc b/src/y2024/cpp/commands/teleop/drive_command.cc index 7077c3e..514cb3b 100644 --- a/src/y2024/cpp/commands/teleop/drive_command.cc +++ b/src/y2024/cpp/commands/teleop/drive_command.cc @@ -2,7 +2,6 @@ #include -#include "field.h" #include "frc846/base/loggable.h" #include "frc846/math/collection.h" #include "frc846/util/share_tables.h" diff --git a/src/y2024/cpp/control_triggers.cc b/src/y2024/cpp/control_triggers.cc index da24e6c..fec4c76 100644 --- a/src/y2024/cpp/control_triggers.cc +++ b/src/y2024/cpp/control_triggers.cc @@ -15,7 +15,6 @@ #include "commands/basic/spin_up_command.h" #include "commands/basic/trap_command.h" #include "commands/basic/wrist_zero_command.h" -#include "commands/complex/super_amp_command.h" void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { frc2::Trigger drivetrain_zero_bearing_trigger{ @@ -60,17 +59,18 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { amp_command_trigger.WhileTrue(AmpCommand{container}.ToPtr()); - frc2::Trigger super_amp_trigger_red{[&container] { - return container.control_input_.GetReadings().running_super_amp && - frc846::util::ShareTables::GetBoolean("is_red_side"); - }}; - frc2::Trigger super_amp_trigger_blue{[&container] { - return container.control_input_.GetReadings().running_super_amp && - !frc846::util::ShareTables::GetBoolean("is_red_side"); - }}; - - super_amp_trigger_red.WhileTrue(SuperAmpCommand{container, true}.ToPtr()); - super_amp_trigger_blue.WhileTrue(SuperAmpCommand{container, false}.ToPtr()); + // frc2::Trigger super_amp_trigger_red{[&container] { + // return container.control_input_.GetReadings().running_super_amp && + // frc846::util::ShareTables::GetBoolean("is_red_side"); + // }}; + // frc2::Trigger super_amp_trigger_blue{[&container] { + // return container.control_input_.GetReadings().running_super_amp && + // !frc846::util::ShareTables::GetBoolean("is_red_side"); + // }}; + + // super_amp_trigger_red.WhileTrue(SuperAmpCommand{container, true}.ToPtr()); + // super_amp_trigger_blue.WhileTrue(SuperAmpCommand{container, + // false}.ToPtr()); frc2::Trigger intake_command_trigger{[&container] { return container.control_input_.GetReadings().running_intake; diff --git a/src/y2024/cpp/field.cc b/src/y2024/cpp/field.cc index 9f40fed..c55b027 100644 --- a/src/y2024/cpp/field.cc +++ b/src/y2024/cpp/field.cc @@ -1,3 +1,242 @@ #include "field.h" -field::points_cls field::points{}; \ No newline at end of file +#include + +#include + +frc846::math::FieldPoint Field_nonstatic::getPoint(std::string name) { + if (!points.empty()) { + for (auto& point : points) { + if (point.first == name) { + return point.second; + } + } + } + Warn("Unable to access fieldpoint: {}.", name); + return frc846::math::FieldPoint{{0_in, 0_in}, 0_deg, {0_fps, 0_fps}}; +} + +std::vector Field_nonstatic::getPath( + std::string name) { + for (auto& path : paths) { + if (path.first == name) { + return path.second; + } + } + Warn("Unable to access path: {}.", name); + return {}; +} + +std::vector Field_nonstatic::split(const std::string& s, + char delimiter) { + std::vector tokens; + std::string token; + std::istringstream tokenStream(s); + while (std::getline(tokenStream, token, delimiter)) { + tokens.push_back(token); + } + return tokens; +} + +std::vector Field_nonstatic::readLines(std::string filename) { + std::ifstream file; + file.open(filename); + + std::vector lines{}; + + std::string content_string; + while (file.good()) { + file >> content_string; + lines.push_back(content_string); + } + + return lines; +} + +std::string Field_nonstatic::fixPath(std::string path) { +#ifdef _WIN32 + for (char& ch : path) { + if (ch == '/') { + ch = '\\'; + } + } + return path; +#else + for (char& ch : path) { + if (ch == '\\') { + ch = '/'; + } + } + return path; +#endif +} + +std::string Field_nonstatic::forceNormalPath(std::string path) { + for (char& ch : path) { + if (ch == '\\') { + ch = '/'; + } + } + return path; +} + +std::string Field_nonstatic::getFileDirectory() { +#ifdef _WIN32 + std::string deploy_dir = frc::filesystem::GetDeployDirectory(); + size_t start_pos = 0; + + std::vector tokens = split(deploy_dir, '\\'); + std::string rejoined{}; + for (size_t i = 0; i < tokens.size(); i++) { + if (tokens[i] != "main") { + rejoined += tokens[i]; + rejoined += "/"; + } + } + + return fixPath(rejoined + "autos/"); + +#else + + return "/home/lvuser/deploy/autos/"; +#endif +} + +frc846::math::FieldPoint Field_nonstatic::parsePoint(std::string line) { + frc846::math::FieldPoint pt{{0_in, 0_in}, 0_deg, {0_fps, 0_fps}}; + + auto tokens = split(line, ','); + if (tokens.size() != 0) { + if (tokens[0] == "N") { + auto v = 0_fps; + if (tokens.size() == 6) v = std::stod(tokens[5]) * 1_fps; + + pt = {{std::stod(tokens[2]) * 1_in, std::stod(tokens[3]) * 1_in}, + std::stod(tokens[4]) * 1_deg, + {v, 0_fps}}; + } else if (tokens[0] == "P") { + auto v = 0_fps; + if (tokens.size() == 5) v = std::stod(tokens[4]) * 1_fps; + + pt = {{std::stod(tokens[1]) * 1_in, std::stod(tokens[2]) * 1_in}, + std::stod(tokens[3]) * 1_deg, + {v, 0_fps}}; + } else if (tokens[0] == "F") { + if (tokens.size() == 2) { + pt = getPoint(tokens[1]); + } + } else { + Warn("Unable to parse point from raw: {}.", line); + } + } + return pt; +} + +void Field_nonstatic::readPointsFile() { + auto pts_us = readLines(Field_nonstatic::getFileDirectory() + "points.lst"); + + for (auto& pt : pts_us) { + auto tokens = split(pt, ','); + if (tokens.size() >= 2) { + addPoint(tokens[1], parsePoint(pt)); + Log("Added point: {}.", tokens[1]); + } else { + Warn("Invalid point: {}.", pt); + } + } +} + +void Field_nonstatic::readAllPaths() { + std::vector path_files; + try { + for (const auto& entry : std::filesystem::directory_iterator( + Field_nonstatic::getFileDirectory() + "paths")) { + if (entry.is_regular_file()) path_files.push_back(entry.path().string()); + Log("Found path file: {}.", entry.path().string()); + } + } catch (const std::exception& exc) { + (void)exc; + } + + for (const auto& filename : path_files) { + auto path_us = readLines(filename); + std::vector path; + for (auto& pt : path_us) { + path.push_back(parsePoint(pt)); + } + auto split_filename = split(forceNormalPath(filename), '/'); + + paths.push_back( + std::pair{split_filename.at(split_filename.size() - 1), path}); + Log("Added path: {}.", split_filename.at(split_filename.size() - 1)); + } +} + +Field_nonstatic::Field_nonstatic() : Loggable{"Field"}, points{}, paths{} {} + +void Field_nonstatic::Setup() { + readPointsFile(); + readAllPaths(); +}; + +std::vector Field_nonstatic::getAllAutoData() { + std::vector result; + + std::vector auto_files; + try { + for (const auto& entry : std::filesystem::directory_iterator( + Field_nonstatic::getFileDirectory() + "scripts")) { + if (entry.is_regular_file()) auto_files.push_back(entry.path().string()); + } + } catch (const std::exception& exc) { + (void)exc; + } + + for (const auto& filename : auto_files) { + auto auto_lines = readLines(filename); + + if (auto_lines.size() < 2) { + Warn("Invalid auto file: {}.", filename); + continue; + } + + auto flips = split(auto_lines[0], ','); + + AutoFlipType red = (AutoFlipType)std::stoi(flips[0]); + AutoFlipType blue = (AutoFlipType)std::stoi(flips[1]); + + auto start_point = parsePoint(auto_lines[1]); + + auto_lines.erase(auto_lines.begin(), auto_lines.begin() + 1); + + std::vector< + std::variant, std::string>> + actions; + + for (auto& line : auto_lines) { + auto split_line = split(line, ','); + if (split_line.size() != 2) { + Warn("Invalid auto action: {}.", line); + continue; + } + if (split_line[0] == "PATH") { + actions.push_back(getPath(split_line[1])); + } else if (split_line[0] == "ACT") { + actions.push_back(split_line[1]); + } + } + + Log("Added auto: {}.", filename); + + Log("Auto has {} actions.", actions.size()); + + auto split_filename = split(forceNormalPath(filename), '/'); + auto auto_name = split_filename.at(split_filename.size() - 1); + + result.push_back({auto_name, red, blue, start_point, actions}); + } + + return result; +} + +Field_nonstatic Field::instance{}; \ No newline at end of file diff --git a/src/y2024/cpp/main.cpp b/src/y2024/cpp/main.cpp deleted file mode 100644 index f92c0da..0000000 --- a/src/y2024/cpp/main.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include -#include -#include - -#include "FunkyRobot.h" - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -#include -/* -inline void ltrim(std::string &s) { - s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) { - return !std::isspace(ch); - })); -} - -inline void rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { - return !std::isspace(ch); - }).base(), s.end()); -} - -#define A 54059 -#define B 76963 -#define C 86969 -#define FIRSTH 37 -unsigned hash_str(const char* s) -{ - unsigned h = FIRSTH; - while (*s) { - h = (h * A) ^ (s[0] * B); - s++; - } - return h % C; -} - -void handler(int sig) { - std::map sigErrors; - - sigErrors[SIGFPE] = "FATAL ERROR >> Arithmetic Error, SIGFPE"; - sigErrors[SIGILL] = "FATAL ERROR >> Illegal Instruction, SIGILL"; - sigErrors[SIGSEGV] = "FATAL ERROR >> Illegal Memory Access, SIGSEGV"; - sigErrors[SIGBUS] = "FATAL ERROR >> Bus Error, SIGBUS"; - sigErrors[SIGABRT] = "FATAL ERROR >> Abort, SIGABRT"; - sigErrors[SIGSYS] = "FATAL ERROR >> Invalid system call, SIGSYS"; - sigErrors[SIGTRAP] = "FATAL ERROR >> Exception Occured, SIGTRAP"; - - void *callstack[24]; - - int frames = backtrace(callstack, 24); - - - std::cerr << "Backtrace:\n"; - char** symbols = backtrace_symbols(callstack, frames); - - if (symbols == nullptr) { - std::cerr << "Error obtaining backtrace symbols\n" << std::endl; - return; - } - - for (int i = 0; i < frames; ++i) { - std::cerr << symbols[i] << std::endl; - } - - if (sigErrors.contains(sig)) { - std::cerr << sigErrors[sig] << std::endl; - } else { - std::cerr << "? Unknown Exception Occured" << std::endl; - } - // exit(1); -}*/ - -int main() { - /*for (int i = 1; i < NSIG; ++i) { - signal(i, handler); - } - - unsigned int validDevIds[]{72214, 69948,}; - bool devIdFound = false; - - std::string file_path = frc::filesystem::GetDeployDirectory()+"/dev.id"; - try { - std::cout << "Trying to find dev.id" << std::endl; - std::ifstream ifs(file_path); - std::ostringstream oss; - oss << ifs.rdbuf(); - - std::string devId = oss.str(); - rtrim(devId); - ltrim(devId); - - auto devIdHash = hash_str(devId.c_str()); - - std::cout << "Dev.id hash key found: " << devIdHash << std::endl; - - for (auto id : validDevIds) { - if (id == devIdHash) { - devIdFound = true; - } - } - } catch (std::exception& exc) { - std::cout << "Error processing dev.id." << std::endl; - } - - if (!devIdFound) { - std::cerr << "Dev ID not set or did not match, exiting" << std::endl; - exit(0); - } else { - std::cout << "Valid dev ID found" << std::endl; - std::cout << "Last deploy: "; - - struct stat file_stat; - - if (stat(file_path.c_str(), &file_stat) == 0) { - char time_buffer[256]; - strftime(time_buffer, sizeof(time_buffer), "%F %T", - localtime(&file_stat.st_mtime)); std::cout << time_buffer << std::endl; auto - current_time = time(0); std::cout << "Current Time: " << - std::ctime(¤t_time) << std::endl; } else { std::cout << "Error getting - file status: " << std::endl; - } - }*/ - - std::cout << "Starting robot code [2024]..." << std::endl; - return frc::StartRobot(); -} \ No newline at end of file diff --git a/src/y2024/cpp/subsystems/abstract/vision.cc b/src/y2024/cpp/subsystems/abstract/vision.cc index 1994674..71a0b4f 100644 --- a/src/y2024/cpp/subsystems/abstract/vision.cc +++ b/src/y2024/cpp/subsystems/abstract/vision.cc @@ -119,13 +119,19 @@ VisionReadings VisionSubsystem::ReadFromHardware() { robot_x += can_bus_latency_.value() * velocity_x + x_correction; - newReadings.est_dist_from_speaker_x = - robot_x - field::points.kSpeaker( - !frc846::util::ShareTables::GetBoolean("is_red_side"))[0]; + newReadings.x_pos = robot_x; + newReadings.y_pos = robot_y; - newReadings.est_dist_from_speaker_y = - robot_y - field::points.kSpeaker( - !frc846::util::ShareTables::GetBoolean("is_red_side"))[1]; + robot_x_graph_.Graph(robot_x); + robot_y_graph_.Graph(robot_y); + + auto speaker_pt = + Field::getPoint("kSpeaker") + .mirrorOnlyY(!frc846::util::ShareTables::GetBoolean("is_red_side")); + + newReadings.est_dist_from_speaker_x = robot_x - speaker_pt.point[0]; + + newReadings.est_dist_from_speaker_y = robot_y - speaker_pt.point[1]; auto point_target = frc846::math::VectorND{ -newReadings.est_dist_from_speaker_x, diff --git a/src/y2024/cpp/subsystems/hardware/wrist.cc b/src/y2024/cpp/subsystems/hardware/wrist.cc index 6f04b67..e42c1e5 100644 --- a/src/y2024/cpp/subsystems/hardware/wrist.cc +++ b/src/y2024/cpp/subsystems/hardware/wrist.cc @@ -7,17 +7,14 @@ WristSubsystem::WristSubsystem(bool init) : frc846::robot::GenericSubsystem{"wrist", init} { - if (init) { - wrist_esc_.Init(frc846::control::REVSparkType::kSparkMAX); - } -} - -void WristSubsystem::Setup() { + wrist_esc_.Init(frc846::control::REVSparkType::kSparkMAX); wrist_esc_.Configure({frc846::control::DataTag::kPositionData, frc846::control::DataTag::kVelocityData}); wrist_esc_.ZeroEncoder(wrist_home_offset_.value()); } +void WristSubsystem::Setup() {} + WristTarget WristSubsystem::ZeroTarget() const { WristTarget target; target.wrist_output = wrist_home_offset_.value(); @@ -56,11 +53,15 @@ WristReadings WristSubsystem::ReadFromHardware() { } void WristSubsystem::WriteToHardware(WristTarget target) { + wrist_weight_pos_graph.Graph( + 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") - + GetReadings().wrist_position + wrist_cg_offset_.value()); + hard_limits_.OverrideLimits(target.override_limits); if (auto pos = std::get_if(&target.wrist_output)) { double output = dyFPID.calculate(*pos, GetReadings().wrist_position, wrist_esc_.GetVelocityPercentage(), - config_helper_.updateAndGetGains()); + gains_prefs_dyFPID.get()); // if (units::math::abs(*pos - readings().wrist_position) < 5_deg) { // output = dyFPIDClose.calculate(*pos, readings().wrist_position, // wrist_esc_.GetVelocityPercentage(), @@ -72,6 +73,10 @@ void WristSubsystem::WriteToHardware(WristTarget target) { target_wrist_pos_graph.Graph(*pos); } else if (auto output = std::get_if(&target.wrist_output)) { + if (units::math::abs(wrist_esc_.GetVelocity()) > + homing_max_speed_.value()) { + *output = (*output) / homing_dc_cut_.value(); + } wrist_esc_.WriteDC(*output); target_wrist_duty_cycle_graph.Graph(*output); diff --git a/src/y2024/include/FunkyRobot.h b/src/y2024/include/FunkyRobot.h index 8ac8336..601d458 100644 --- a/src/y2024/include/FunkyRobot.h +++ b/src/y2024/include/FunkyRobot.h @@ -2,9 +2,7 @@ #include -#include "autos/drive_auto.h" -#include "autos/five_piece_auto.h" -#include "autos/one_piece_auto.h" +#include "autos/GenericAuto.h" #include "frc846/ntinf/fstore.h" #include "frc846/ntinf/pref.h" #include "frc846/robot/GenericRobot.h" @@ -26,29 +24,11 @@ class FunkyRobot : public frc846::robot::GenericRobot { private: RobotContainer container_; - // Autos - frc2::CommandPtr drive_auto_ = DriveAuto{container_}.ToPtr(); - - frc2::CommandPtr five_piece_auto_red = - FivePieceAuto{container_, false}.ToPtr(); - - frc2::CommandPtr five_piece_auto_blue = - FivePieceAuto{container_, true}.ToPtr(); - - frc2::CommandPtr one_piece_auto_0 = - OnePieceAuto{container_, -60_deg, "left red"}.ToPtr(); - frc2::CommandPtr one_piece_auto_1 = - OnePieceAuto{container_, 60_deg, "right red"}.ToPtr(); - frc2::CommandPtr one_piece_auto_2 = - OnePieceAuto{container_, -60_deg + 180_deg, "left blue"}.ToPtr(); - frc2::CommandPtr one_piece_auto_3 = - OnePieceAuto{container_, 60_deg + 180_deg, "right blue"}.ToPtr(); - frc::DigitalInput homing_switch_{0}; frc::DigitalInput coasting_switch_{1}; - frc846::ntinf::Pref start_coast_counter_{*this, "start_coast_counter", - 500}; + frc846::ntinf::Pref coasting_time_{*this, "coasting_time", + 7.5_s}; - int coast_counter_ = 0; + units::second_t stop_coast_time_; }; diff --git a/src/y2024/include/autos/ActionMaker.h b/src/y2024/include/autos/ActionMaker.h new file mode 100644 index 0000000..1430584 --- /dev/null +++ b/src/y2024/include/autos/ActionMaker.h @@ -0,0 +1,14 @@ +#pragma once + +#include + +#include +#include + +#include "subsystems/robot_container.h" + +class ActionMaker { + public: + static std::unique_ptr GetAction(std::string name, + RobotContainer& container); +}; \ No newline at end of file diff --git a/src/y2024/include/autos/drive_auto.h b/src/y2024/include/autos/GenericAuto.h similarity index 53% rename from src/y2024/include/autos/drive_auto.h rename to src/y2024/include/autos/GenericAuto.h index 39999aa..ace32f9 100644 --- a/src/y2024/include/autos/drive_auto.h +++ b/src/y2024/include/autos/GenericAuto.h @@ -1,12 +1,17 @@ #pragma once +#include "field.h" #include "frc846/robot/GenericCommand.h" #include "frc846/swerve/follow_trajectory_command.h" #include "subsystems/robot_container.h" -class DriveAuto - : public frc846::robot::GenericCommandGroup { public: - DriveAuto(RobotContainer& container); + GenericAuto(RobotContainer& container, AutoData data, bool is_blue_side); + + private: + std::vector> buildActionsGroup( + AutoData data, RobotContainer& container, bool is_blue_side); }; diff --git a/src/y2024/include/autos/five_piece_auto.h b/src/y2024/include/autos/five_piece_auto.h deleted file mode 100644 index 055a624..0000000 --- a/src/y2024/include/autos/five_piece_auto.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "frc846/robot/GenericCommand.h" -#include "frc846/swerve/follow_trajectory_command.h" -#include "subsystems/robot_container.h" - -class FivePieceAuto - : public frc846::robot::GenericCommandGroup { - public: - FivePieceAuto(RobotContainer& container, bool should_flip); -}; diff --git a/src/y2024/include/autos/one_piece_auto.h b/src/y2024/include/autos/one_piece_auto.h deleted file mode 100644 index 3aafa57..0000000 --- a/src/y2024/include/autos/one_piece_auto.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "frc846/robot/GenericCommand.h" -#include "frc846/swerve/follow_trajectory_command.h" -#include "subsystems/robot_container.h" - -class OnePieceAuto - : public frc846::robot::GenericCommandGroup { - public: - OnePieceAuto(RobotContainer& container, units::degree_t start_angle, - std::string num); -}; diff --git a/src/y2024/include/commands/complex/close_drive_amp_command.h b/src/y2024/include/commands/complex/close_drive_amp_command.h index a63c247..219452f 100644 --- a/src/y2024/include/commands/complex/close_drive_amp_command.h +++ b/src/y2024/include/commands/complex/close_drive_amp_command.h @@ -1,19 +1,19 @@ -#pragma once +// #pragma once -#include "frc846/robot/GenericCommand.h" -#include "subsystems/robot_container.h" +// #include "frc846/robot/GenericCommand.h" +// #include "subsystems/robot_container.h" -class CloseDriveAmpCommand - : public frc846::robot::GenericCommand { - public: - CloseDriveAmpCommand(RobotContainer& container); +// class CloseDriveAmpCommand +// : public frc846::robot::GenericCommand { +// public: +// CloseDriveAmpCommand(RobotContainer& container); - void OnInit() override; +// void OnInit() override; - void Periodic() override; +// void Periodic() override; - void OnEnd(bool interrupted) override; +// void OnEnd(bool interrupted) override; - bool IsFinished() override; -}; +// bool IsFinished() override; +// }; diff --git a/src/y2024/include/commands/complex/home_during_auto_command.h b/src/y2024/include/commands/complex/home_during_auto_command.h new file mode 100644 index 0000000..f29e6d0 --- /dev/null +++ b/src/y2024/include/commands/complex/home_during_auto_command.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#include "frc846/robot/GenericCommand.h" +#include "subsystems/robot_container.h" + +class HomeDuringAutoCommand + : public frc846::robot::GenericCommandGroup< + RobotContainer, HomeDuringAutoCommand, frc2::SequentialCommandGroup> { + public: + HomeDuringAutoCommand(RobotContainer& container); +}; diff --git a/src/y2024/include/commands/complex/super_amp_command.h b/src/y2024/include/commands/complex/super_amp_command.h index c266f71..fe3abe1 100644 --- a/src/y2024/include/commands/complex/super_amp_command.h +++ b/src/y2024/include/commands/complex/super_amp_command.h @@ -1,14 +1,16 @@ -#pragma once +// #pragma once -#include +// #include -#include "frc846/robot/GenericCommand.h" -#include "frc846/swerve/follow_trajectory_command.h" -#include "subsystems/robot_container.h" +// #include "frc846/robot/GenericCommand.h" +// #include "frc846/swerve/follow_trajectory_command.h" +// #include "subsystems/robot_container.h" -class SuperAmpCommand - : public frc846::robot::GenericCommandGroup { - public: - SuperAmpCommand(RobotContainer& container, bool is_red_side); -}; +// class SuperAmpCommand +// : public frc846::robot::GenericCommandGroup +// { +// public: +// SuperAmpCommand(RobotContainer& container, bool is_red_side); +// }; diff --git a/src/y2024/include/field.h b/src/y2024/include/field.h index f90213c..ef115eb 100644 --- a/src/y2024/include/field.h +++ b/src/y2024/include/field.h @@ -1,89 +1,77 @@ #pragma once -#include +#include + +#include #include "frc846/math/fieldpoints.h" -// Field has blue alliance far right corner as origin -struct field { - struct points_cls { - frc846::math::FieldPoint Origin() { - return {{0_in, 0_in}, 0_deg, {0_fps, 0_fps}}; - } - - frc846::math::VectorND kSpeaker(bool flip = false) { - if (!flip) { - return {217.5_in, -4_in}; - } else { - return {217.5_in, 655.8_in}; - } - } - - frc846::math::FieldPoint kAmpNoFlip() { - return {{0_in, 0_in}, 90_deg, {0_fps, 0_fps}}; - } - - frc846::math::FieldPoint kPreAmpNoFlip() { - return {{-2_ft, 0_in}, 90_deg, {0_fps, 0_fps}}; - } - - // DRIVE AUTO - TEST POINTS - - frc846::math::FieldPointPreference testing_origin{"testing_origin", - Origin()}; - frc846::math::FieldPoint kTestingOrigin() { return testing_origin.get(); }; - - frc846::math::FieldPointPreference testing_point{ - "testing_point", {{0_in, 120_in}, 0_deg, {0_fps, 0_fps}}}; - frc846::math::FieldPoint kTestingPoint() { return testing_point.get(); }; - - // FIVE PIECE AUTO - frc846::base::Loggable five_piece_loggable{"five_piece_auto"}; - - frc846::ntinf::Pref pre_point_amt{five_piece_loggable, - "pre_point_amt", 2_ft}; - frc846::math::FieldPoint pre_point(frc846::math::FieldPoint pnt) { - return {{pnt.point[0], pnt.point[1] - pre_point_amt.value()}, - pnt.bearing, - {0_fps, 15_fps}}; - } - - frc846::math::FieldPointPreference origin_point{ - "five_piece_origin", {{217.5_in, 49_in}, 0_deg, {0_fps, 0_fps}}}; - frc846::math::FieldPoint kFivePieceOrigin(bool should_flip) { - return origin_point.get().mirrorOnlyY(should_flip); - } - - frc846::math::FieldPointPreference intake_one{ - "five_piece_intake_one", {{217.5_in, 112_in}, 0_deg, {0_fps, 0_fps}}}; - std::vector intake_one_path(bool should_flip) { - auto base_point = intake_one.get(); - return {pre_point(base_point).mirrorOnlyY(should_flip), - base_point.mirrorOnlyY(should_flip)}; - } - - frc846::math::FieldPointPreference intake_two{ - "five_piece_intake_two", {{160.5_in, 112_in}, 0_deg, {0_fps, 0_fps}}}; - std::vector intake_two_path(bool should_flip) { - auto base_point = intake_two.get(); - return {pre_point(base_point).mirrorOnlyY(should_flip), - base_point.mirrorOnlyY(should_flip)}; - } - - frc846::math::FieldPointPreference intake_three{ - "five_piece_intake_three", {{274.5_in, 112_in}, 0_deg, {0_fps, 0_fps}}}; - std::vector intake_three_path(bool should_flip) { - auto base_point = intake_three.get(); - return {pre_point(base_point).mirrorOnlyY(should_flip), - base_point.mirrorOnlyY(should_flip)}; - } - - frc846::math::FieldPointPreference finish_pt{ - "five_piece_finish", {{274.5_in, 180_in}, 0_deg, {0_fps, 0_fps}}}; - frc846::math::FieldPoint kFivePieceFinish(bool should_flip) { - return finish_pt.get().mirrorOnlyY(should_flip); - } - }; - - static points_cls points; +enum AutoFlipType { kNone, kMirror, kMirrorOnlyY }; + +struct AutoData { + std::string name; + AutoFlipType red; + AutoFlipType blue; + frc846::math::FieldPoint start; + std::vector, std::string>> + actions; +}; + +class Field_nonstatic : public frc846::base::Loggable { + public: + std::vector> points; + + std::vector>> + paths; + + frc846::math::FieldPoint getPoint(std::string name); + + std::vector getPath(std::string name); + + Field_nonstatic(); + + void Setup(); + + std::vector getAllAutoData(); + + private: + void addPoint(std::string name, frc846::math::FieldPoint point) { + points.push_back(std::pair{name, point}); + } + + static std::vector split(const std::string& s, char delimiter); + + static std::vector readLines(std::string filename); + + static std::string fixPath(std::string path); + + static std::string forceNormalPath(std::string path); + + static std::string getFileDirectory(); + + frc846::math::FieldPoint parsePoint(std::string line); + + void readPointsFile(); + + void readAllPaths(); }; + +class Field { + public: + static frc846::math::FieldPoint getPoint(std::string name) { + return instance.getPoint(name); + } + + static std::vector getPath(std::string name) { + return instance.getPath(name); + } + + static void Setup() { instance.Setup(); } + + static std::vector getAllAutoData() { + return instance.getAllAutoData(); + } + + private: + static Field_nonstatic instance; +}; \ No newline at end of file diff --git a/src/y2024/include/rsighandler.h b/src/y2024/include/rsighandler.h new file mode 100644 index 0000000..77d82f1 --- /dev/null +++ b/src/y2024/include/rsighandler.h @@ -0,0 +1,60 @@ +#pragma once + +#ifndef _WIN32 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +void handler(int sig) { + std::map sigErrors; + + sigErrors[SIGFPE] = "FATAL ERROR >> Arithmetic Error, SIGFPE"; + sigErrors[SIGILL] = "FATAL ERROR >> Illegal Instruction, SIGILL"; + sigErrors[SIGSEGV] = "FATAL ERROR >> Illegal Memory Access, SIGSEGV"; + sigErrors[SIGBUS] = "FATAL ERROR >> Bus Error, SIGBUS"; + sigErrors[SIGABRT] = "FATAL ERROR >> Abort, SIGABRT"; + sigErrors[SIGSYS] = "FATAL ERROR >> Invalid system call, SIGSYS"; + sigErrors[SIGTRAP] = "FATAL ERROR >> Exception Occured, SIGTRAP"; + + void* callstack[24]; + + int frames = backtrace(callstack, 24); + + std::cerr << "Backtrace:\n"; + char** symbols = backtrace_symbols(callstack, frames); + + if (symbols == nullptr) { + std::cerr << "Error obtaining backtrace symbols\n" << std::endl; + return; + } + + for (int i = 0; i < frames; ++i) { + std::cerr << symbols[i] << std::endl; + } + + if (sigErrors.contains(sig)) { + std::cerr << sigErrors[sig] << std::endl; + } else { + std::cerr << "? Unknown Exception Occured" << std::endl; + } + // exit(1); +} +#endif + +#ifndef _WIN32 +void configureSignalHandlers() { + for (int i = 1; i < NSIG; ++i) { + signal(i, handler); + } +} +#else +void configureSignalHandlers() {} +#endif \ No newline at end of file diff --git a/src/y2024/include/subsystems/abstract/vision.h b/src/y2024/include/subsystems/abstract/vision.h index 713b500..208828a 100644 --- a/src/y2024/include/subsystems/abstract/vision.h +++ b/src/y2024/include/subsystems/abstract/vision.h @@ -20,6 +20,9 @@ struct VisionReadings { units::inch_t tag_distance; units::degree_t tag_angle_difference; + units::foot_t x_pos; + units::foot_t y_pos; + double velocity_in_component; double velocity_orth_component; }; @@ -44,6 +47,9 @@ class VisionSubsystem bool VerifyHardware() override; + frc846::ntinf::Pref using_vision_autos_{*this, "using_vision_autos", + false}; + private: frc846::ntinf::Pref default_is_red_side_{*this, "default_is_red_side"}; @@ -53,6 +59,10 @@ class VisionSubsystem "ll_latency"}; frc846::base::Loggable readings_named{*this, "readings"}; + frc846::ntinf::Grapher robot_x_graph_{readings_named, + "robot_x"}; + frc846::ntinf::Grapher robot_y_graph_{readings_named, + "robot_y"}; frc846::ntinf::Grapher speaker_x_dist_graph_{readings_named, "speaker_dist_x"}; frc846::ntinf::Grapher speaker_y_dist_graph_{readings_named, diff --git a/src/y2024/include/subsystems/hardware/wrist.h b/src/y2024/include/subsystems/hardware/wrist.h index bbc1d24..43f70b6 100644 --- a/src/y2024/include/subsystems/hardware/wrist.h +++ b/src/y2024/include/subsystems/hardware/wrist.h @@ -24,6 +24,8 @@ struct WristTarget { class WristSubsystem : public frc846::robot::GenericSubsystem { public: + bool hasZeroed = false; + WristSubsystem(bool init); void Setup() override; @@ -78,12 +80,13 @@ class WristSubsystem frc846::ntinf::Pref homing_velocity_tolerance_{ *this, "homing_velocity_tolerance", 1.0_deg_per_s}; - frc846::ntinf::Pref num_loops_homed_{*this, "num_loops_homed", 7}; + frc846::ntinf::Pref num_loops_homed_{*this, "num_loops_homed", 20}; frc846::ntinf::Pref homing_speed_{*this, "homing_speed", -0.2}; + frc846::ntinf::Pref homing_max_speed_{ + *this, "homing_max_speed", 30.0_deg_per_s}; + frc846::ntinf::Pref homing_dc_cut_{*this, "homing_dc_cut", 1.5}; private: - bool hasZeroed = false; - frc846::base::Loggable gains_{*this, "gains"}; frc846::ntinf::Pref k_{gains_, "k", 0.0}; frc846::ntinf::Pref p_{gains_, "p", 0.0}; @@ -96,6 +99,9 @@ class WristSubsystem frc846::ntinf::Grapher wrist_error_graph{target_named_, "wrist_error"}; + frc846::ntinf::Grapher wrist_weight_pos_graph{ + target_named_, "wrist_weight_position"}; + frc846::base::Loggable target_named_{*this, "target"}; frc846::ntinf::Grapher target_wrist_duty_cycle_graph{ @@ -124,14 +130,17 @@ class WristSubsystem frc846::base::Loggable dyFPID_loggable{*this, "DynamicFPID"}; + frc846::control::GainsPrefs gains_prefs_dyFPID{dyFPID_loggable, + {0.0, 0.0, 0.0}}; + frc846::motion::BrakingPositionDyFPID dyFPID{ dyFPID_loggable, [&](units::degree_t pos) -> double { - return std::abs( - units::math::cos( - 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") - - GetReadings().wrist_position + wrist_cg_offset_.value()) - .to()); + return units::math::sin( + 1_deg * + frc846::util::ShareTables::GetDouble("pivot_position") - + GetReadings().wrist_position + wrist_cg_offset_.value()) + .to(); }, {30_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}, &hard_limits_}; diff --git a/src/y2024/resources/deploy_autos.bat b/src/y2024/resources/deploy_autos.bat new file mode 100644 index 0000000..f264a57 --- /dev/null +++ b/src/y2024/resources/deploy_autos.bat @@ -0,0 +1,3 @@ +scp ./src/deploy/autos/points.lst admin@10.8.46.2:/home/lvuser/deploy/autos +scp ./src/deploy/autos/paths/* admin@10.8.46.2:/home/lvuser/deploy/autos/paths +scp ./src/deploy/autos/scripts/* admin@10.8.46.2:/home/lvuser/deploy/autos/scripts \ No newline at end of file diff --git a/src/y2024/resources/jetson/readAprilTag.py b/src/y2024/resources/jetson/readAprilTag.py index da97730..1287a7b 100644 --- a/src/y2024/resources/jetson/readAprilTag.py +++ b/src/y2024/resources/jetson/readAprilTag.py @@ -1,308 +1,196 @@ from pupil_apriltags import Detector -from datetime import datetime import numpy as np -import math -import os -from cv2 import * +import time +import cv2 import threading from networktables import NetworkTables +import subprocess +from pref import NumericPref, BooleanPref, KillSwitch -NetworkTables.initialize(server='roborio-846-frc.local') +NetworkTables.initialize(server='10.8.46.2') table = NetworkTables.getTable("AprilTags") -validTagIds = [4, 7] +preferenceTable = NetworkTables.getTable("JetsonPreferences") -aprilTagX=[0-14/12.0]*20 -aprilTagY=[6-6/12.0]*20 -aprilAngle=[180]*20 -aprilHeights=[10]*20 +kill_switch = KillSwitch(preferenceTable) -aprilTagX[4]=-104.4/12 #RED -aprilTagY[4]=0.0/12.0 -aprilAngle[4]=0 -aprilHeights[4]=53.88 +exposure_pref = NumericPref(preferenceTable, "camera_exposure", 1) +horizontal_fov_pref = NumericPref(preferenceTable, "h_fov", 63.1) +vertical_fov_pref = NumericPref(preferenceTable, "v_fov", 50.0) -aprilTagX[3]=-126.65/12 #RED -aprilTagY[3]=0/12.0 -aprilAngle[3]=0 -aprilHeights[3]=53.88 +h_frame_size = NumericPref(preferenceTable, "h_frame", 640) +v_frame_size = NumericPref(preferenceTable, "v_frame", 480) -aprilTagX[7]=-104.4/12 #BLUE -aprilTagY[7]=651.25/12.0 -aprilAngle[7]=0 -aprilHeights[7]=53.88 +cam_latency = NumericPref(preferenceTable, "camera_latency", 0.01) -aprilTagX[8]=-127.08/12 #BLUE -aprilTagY[8]=651.25/12 -aprilAngle[8]=0 -aprilHeights[8]=53.88 +target_mean_brightness = NumericPref(preferenceTable, "target_mean_brightness", 120) +clip_limit = NumericPref(preferenceTable, "clip_limit", 2.0) +debug_mode = BooleanPref(preferenceTable, "debug_mode", False) -# aprilTagX[16]=0.3-14/12.0 -# aprilTagY[16]=9.0247+6/12.0 +at_detector = Detector(families='tag36h11', + nthreads=1, + quad_decimate=1.0, + quad_sigma=0.2, + refine_edges=1, + decode_sharpening=0.5, + debug=0) -locations=np.array([0.0, 0.0, 0.0]) +cameraMatrix = np.array([(336.7755634193813, 0.0, 333.3575643300718), (0.0, 336.02729840829176, 212.77376312080065), (0.0, 0.0, 1.0)]) -framesSeen=0 -confidence=0 +camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] ) -lower = np.array([0, 0, 0]) -upper = np.array([255, 255, 105]) +queue = [] -visualization = False -try: - import cv2 -except: - raise Exception('OpenCV not found') +def adjust_gamma(image, gamma=1.0): + invGamma = 1.0 / gamma + table = np.array([((i / 255.0) ** invGamma) * 255 + for i in np.arange(0, 256)]).astype("uint8") + return cv2.LUT(image, table) -try: - from cv2 import imshow -except: - print("The function imshow was not implemented in this installation. Rebuild OpenCV from source to use it") - print("Visualization will be disabled.") - visualization = False +def pre_process(frame): + global clip_limit + global target_mean_brightness + + clahe = cv2.createCLAHE(clipLimit=clip_limit.get(), tileGridSize=(8,8)) + tmean_b = target_mean_brightness.get() + result = clahe.apply(frame) -def getDistanceAssumingHeadOn(y, verticalPixelHeight): - heightOfObject =1 - verticalFOV = 1 * 3.14159/360 - return (heightOfObject*verticalPixelHeight)/(2*y*math.tan(verticalFOV/2)) + mean_brightness = np.mean(result) -# def getDist(xReal, yReal, verticalPixelHeight, horizontalPixelWidth, tagSize): -# x = xReal-horizontalPixelWidth/2 -# y=verticalPixelHeight/2-yReal - + if (mean_brightness != 0): result = adjust_gamma(result, tmean_b / mean_brightness) -def getDistance(xReal, yReal, verticalPixelHeight, horizontalPixelWidth, tagHeight): - x = xReal-horizontalPixelWidth/2 - y=verticalPixelHeight/2 -yReal + return result - diagonalFOV=(68.5)*(3.14159/180) - f = math.sqrt(horizontalPixelWidth*horizontalPixelWidth+verticalPixelHeight*verticalPixelHeight)/(2*(math.tan(diagonalFOV/2))) - # 587.4786864517579 - mountHeight=12 - mountAngle=(37)*(3.14159/180) +def process_frames(): + global table + global queue - VertAngle = mountAngle+math.atan(y/f) - yDist = (tagHeight-mountHeight)/math.tan(VertAngle) - xDist = ((tagHeight-mountHeight)/math.sin(VertAngle))*x/(math.sqrt(f*f+y*y)) + global cam_latency + global horizontal_fov_pref + global vertical_fov_pref + global h_frame_size + global v_frame_size + global debug_mode - return (xDist, yDist, tagHeight) + validTagIds = [4, 7] + CAM_FOV_H = horizontal_fov_pref.get() + CAM_FOV_V = vertical_fov_pref.get() -at_detector = Detector(families='tag36h11', - nthreads=1, - quad_decimate=1.0, - quad_sigma=0.0, - refine_edges=1, - decode_sharpening=0.25, - debug=0) + CAM_SZ_H = h_frame_size.get() + CAM_SZ_V = v_frame_size.get() -cameraMatrix = np.array([(336.7755634193813, 0.0, 333.3575643300718), (0.0, 336.02729840829176, 212.77376312080065), (0.0, 0.0, 1.0)]) - -camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] ) - -starttime = datetime.now() -RealStart = datetime.now() -framesPassed=0 -roboRioFrameRequests=0 -def run_april (frame): - global framesPassed - global starttime - global roboRioFrameRequests - global RealStart - global framesSeen - global confidence - global locations - data = table.getNumberArray("roboRioFrameRequest", [0, 0]) - currentRoboRioFrameRequest=data[0] - currentBearing=data[1] - # print(currentRoboRioFrameRequest) - frameRequested = (currentRoboRioFrameRequest != roboRioFrameRequests) and (currentRoboRioFrameRequest != roboRioFrameRequests -1) - # print(str(currentRoboRioFrameRequest) +" " + str(roboRioFrameRequests)) - - - if (frameRequested): - sizingFactor = 1/2 - img = frame - img = cv2.resize(img, ((int)(frame.shape[1]*sizingFactor), (int)(frame.shape[0]*sizingFactor))) - img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) - frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) - - #tags = at_detector.detect(img, True, camera_params, parameters['sample_test']['tag_size']) - tags = at_detector.detect(img, True, camera_params, 0.065) - bufferSize =4 - - if tags != []: - try: - print(tags.__getitem__(0).tag_id) - - (ptUpperL,ptUpperR,ptBottomR,ptBottomL) = tags.__getitem__(0).corners - - ptUpperL=((int)(ptUpperL[0]), (int)(ptUpperL[1])) - ptUpperR=((int)(ptUpperR[0]), (int)(ptUpperR[1])) - ptBottomL=((int)(ptBottomL[0]), (int)(ptBottomL[1])) - ptBottomR=((int)(ptBottomR[0]), (int)(ptBottomR[1])) - - upperRight = frame[int(ptUpperR[1]/sizingFactor-bufferSize):int(ptUpperR[1]/sizingFactor+bufferSize), int(ptUpperR[0]/sizingFactor-bufferSize):int(ptUpperR[0]/sizingFactor+bufferSize)] - - upperRightHsv = cv2.cvtColor(upperRight, cv2.COLOR_BGR2HSV) - upperRightMask = cv2.inRange(upperRightHsv, lower, upper) - upperRightResult =cv2.bitwise_and(upperRight, upperRight, mask=upperRightMask) - upperRight_np_result = np.array(upperRightResult) - upperRightResult = cv2.merge((upperRight_np_result, upperRight_np_result, upperRight_np_result)) - upperRightResult = cv2.Canny(upperRightResult, 60, 200) - upperRightContours, hierarchy = cv2.findContours(upperRightResult, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - upperRightRect = cv2.boundingRect(upperRightContours[0]) - bottomRightBound =(int(ptUpperR[1]/sizingFactor-bufferSize)+upperRightRect[2], int(ptUpperR[0]/sizingFactor-bufferSize)+upperRightRect[3]) - - upperLeft = frame[int(ptUpperL[1]/sizingFactor-bufferSize):int(ptUpperL[1]/sizingFactor+bufferSize), int(ptUpperL[0]/sizingFactor-bufferSize):int(ptUpperL[0]/sizingFactor+bufferSize)] - upperLeftHsv = cv2.cvtColor(upperLeft, cv2.COLOR_BGR2HSV) - upperLeftMask = cv2.inRange(upperLeftHsv, lower, upper) - upperLeftResult = cv2.bitwise_and(upperLeft, upperLeft, mask=upperLeftMask) - upperLeft_np_result = np.array(upperLeftResult) - upperLeftResult = cv2.merge((upperLeft_np_result, upperLeft_np_result, upperLeft_np_result)) - upperLeftResult = cv2.Canny(upperLeftResult, 60, 200) - - upperLeftContours, hierarchy = cv2.findContours(upperLeftResult, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - upperLeftRect = cv2.boundingRect(upperLeftContours[0]) - cv2.line(upperLeftResult,(upperLeftRect[0], upperLeftRect[1]),(upperLeftRect[0], upperLeftRect[1]+upperLeftRect[3]), color=(0, 255, 255), thickness=1) - cv2.line(upperLeftRect,(upperLeftRect[0], upperLeftRect[1]),(upperLeftRect[0]+upperLeftRect[2], upperLeftRect[1]), color=(0, 255, 255), thickness=1) - cv2.line(upperLeftResult,(upperLeftRect[0], upperLeftRect[1]+upperLeftRect[3]),(upperLeftRect[0]+upperLeftRect[2], upperLeftRect[1]+upperLeftRect[3]), color=(0, 255, 255), thickness=1) - cv2.line(upperLeftResult,(upperLeftRect[0]+upperLeftRect[2], upperLeftRect[1]+upperLeftRect[3]),(upperLeftRect[0]+upperLeftRect[2], upperLeftRect[1]), color=(0, 255, 255), thickness=1) - - bottomLeftBound=(int(ptUpperL[1]/sizingFactor-bufferSize)+upperLeftRect[0], int(ptUpperL[0]/sizingFactor-bufferSize)+upperLeftRect[3]) - - bottomLeft = frame[int(ptBottomL[1]/sizingFactor-bufferSize):int(ptBottomL[1]/sizingFactor+bufferSize), int(ptBottomL[0]/sizingFactor-bufferSize):int(ptBottomL[0]/sizingFactor+bufferSize)] - bottomLeftHsv =cv2.cvtColor(bottomLeft, cv2.COLOR_BGR2HSV) - bottomLeftMask=cv2.inRange(bottomLeftHsv, lower, upper) - bottomLeftResult =cv2.bitwise_and(bottomLeft, bottomLeft, mask=bottomLeftMask) - - bottomLeft_np_result = np.array(bottomLeftResult) - bottomLeftResult = cv2.merge((bottomLeft_np_result, bottomLeft_np_result, bottomLeft_np_result)) - bottomLeftResult = cv2.Canny(bottomLeftResult, 60, 200) - - bottomLeftContours, hierarchy = cv2.findContours(bottomLeftResult, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - bottomLeftRect = cv2.boundingRect(bottomLeftContours[0]) - upperLeftBound=(int(ptBottomL[1]/sizingFactor-bufferSize)+bottomLeftRect[0], int(ptBottomL[0]/sizingFactor-bufferSize)+bottomLeftRect[1]) - - bottomRight = frame[int(ptBottomR[1]/sizingFactor-bufferSize):int(ptBottomR[1]/sizingFactor+bufferSize), int(ptBottomR[0]/sizingFactor-bufferSize):int(ptBottomR[0]/sizingFactor+bufferSize)] - bottomRightHsv =cv2.cvtColor(bottomRight, cv2.COLOR_BGR2HSV) - bottomRightMask=cv2.inRange(bottomRightHsv, lower, upper) - bottomRightResult =cv2.bitwise_and(bottomRight, bottomRight, mask=bottomRightMask) - - bottomRight_np_result = np.array(bottomRightResult) - bottomRightResult = cv2.merge((bottomRight_np_result, bottomRight_np_result, bottomRight_np_result)) - bottomRightResult = cv2.Canny(bottomRightResult, 60, 200) - - bottomRightContours, hierarchy = cv2.findContours(bottomRightResult, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - bottomRightRect = cv2.boundingRect(bottomRightContours[0]) - print(bottomRightRect) - upperRightBound=(int(ptBottomR[1]/sizingFactor-bufferSize)+bottomRightRect[1], int(ptBottomR[0]/sizingFactor-bufferSize)+bottomRightRect[2]) - frame=cv2.circle(frame, (upperLeftBound[1], upperLeftBound[0]), radius=0, color=(0, 255, 255), thickness=-1) - frame=cv2.circle(frame, (upperRightBound[1], upperRightBound[0]), radius=0, color=(0, 255, 255), thickness=-1) - frame=cv2.circle(frame, (bottomLeftBound[1], bottomLeftBound[0]), radius=0, color=(0, 255, 255), thickness=-1) - frame=cv2.circle(frame, (bottomRightBound[1], bottomRightBound[0]), radius=0, color=(0, 255, 255), thickness=-1) - - location=(np.array(getDistance(upperRightBound[1], upperRightBound[0], frame.shape[0], frame.shape[1], 60.75)) - +np.array(getDistance(bottomLeftBound[1], bottomLeftBound[0], frame.shape[0], frame.shape[1], 54.5)+np.array(getDistance(bottomRightBound[1], bottomRightBound[0], frame.shape[0], frame.shape[1], 60.75))+np.array(getDistance(upperLeftBound[1], upperLeftBound[0], frame.shape[0], frame.shape[1], 54.5))))/4 - # ) - locations+=location - - print(location) - tagId=tags.__getitem__(0).tag_id - - table.putNumber("robotX", location[0]/12.0) - table.putNumber("robotY", location[1]/12.0) - table.putNumber("aprilTagID", tagId) - table.putNumber("aprilTagX", aprilTagX[tagId]) - table.putNumber("aprilTagAngle", aprilAngle[tagId]) - table.putNumber("aprilTagY", aprilTagY[tagId]) - - if (tagId in validTagIds): - table.putNumber("aprilTagConfidence", confidence) - else: - table.putNumber("aprilTagConfidence", 0.0) - - table.putNumber("processorFrameSent", currentRoboRioFrameRequest) - roboRioFrameRequests=currentRoboRioFrameRequest - - NetworkTables.flush() - - framesSeen+=1; - - except: - table.putNumber("aprilTagConfidence", 0.0) - table.putNumber("processorFrameSent", currentRoboRioFrameRequest) - roboRioFrameRequests=currentRoboRioFrameRequest - print("Uh oh! We encountered an error and did not send data this loop") - - else: - table.putNumber("aprilTagConfidence", 0.0) - table.putNumber("processorFrameSent", currentRoboRioFrameRequest) - roboRioFrameRequests=currentRoboRioFrameRequest - - time = datetime.now()-RealStart - - framesPassed+=1 - - - if (framesPassed >= 10): - endtime=datetime.now() - - locations=np.array([0.0, 0.0, 0.0]) - print('FPS: ' + (str)(framesPassed/(endtime-starttime).total_seconds())) - starttime=endtime - confidence =framesSeen/(1.0*framesPassed) - framesPassed=0 - framesSeen=0 + while True: + if len(queue) == 0: continue + frame = queue.pop(0) + + process_start_time = time.time() + + orig_img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + img = pre_process(orig_img) + + tags = at_detector.detect(img, False, camera_params, 0.065) + + table.putNumber("tx", 0.0) + table.putNumber("ty", 0.0) + table.putNumber("tid", -1) + + try: + for tag in tags: + if not (tag.tag_id in validTagIds): continue + + cx, cy = tag.center + tx = (cx - CAM_SZ_H/2) * (CAM_FOV_H / CAM_SZ_H) + ty = -((cy - CAM_SZ_V/2) * (CAM_FOV_V / CAM_SZ_V)) -def RUN_APRIL_846(): - print("SCRIPT: ATTEMPTING CAM 1") + table.putNumber("tid", tag.tag_id) + table.putNumber("tx", tx) + + table.putNumber("ty", ty) + + except: + pass + + latency = cam_latency.get() + time.time() - process_start_time + + + if (debug_mode.get()): + cv2.putText(orig_img, f"Original Image: ({CAM_SZ_H}, {CAM_SZ_V}).", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 3, cv2.LINE_AA) + cv2.imwrite("/home/nvidia/apriltags/orig.png", img) + + img_copy = img.copy() + + cv2.putText(img_copy, f"Processed Image: ({CAM_SZ_H}, {CAM_SZ_V}).", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 3, cv2.LINE_AA) + cv2.imwrite("/home/nvidia/apriltags/proc.png", img_copy) + + for tag in tags: + for idx in range(len(tag.corners)): + cv2.line(img, tuple(tag.corners[idx-1, :].astype(int)), + tuple(tag.corners[idx, :].astype(int)), (0, 255, 0), thickness=4) + + cv2.putText(img, f"ID: {tag.tag_id}", + org=(tag.corners[0, 0].astype(int), tag.corners[0, 1].astype(int)), + fontFace=cv2.FONT_HERSHEY_SIMPLEX, + fontScale=0.8, color=(0, 255, 0), thickness=4) + + cv2.putText(img, f"Resultant Image. {len(tags)} Detections. Latency: {latency}.", + (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 3, cv2.LINE_AA) + + cv2.imwrite("/home/nvidia/apriltags/res.png", img) + + table.putNumber("latency", latency) + NetworkTables.flush() + +def getCamera() -> cv2.VideoCapture: + print("\n") cap = cv2.VideoCapture("/dev/video0") - if cap.isOpened()==False: - print("SCRIPT: ATTEMPTING CAM 2") + if cap.isOpened() == False: + print("CAM 1 FAILURE, ATTEMPTING CAM 2.") cap = cv2.VideoCapture("/dev/video1") - if cap.isOpened()==False: - print("SCRIPT: CAM CONN FAILURE") + if cap.isOpened() == False: + print("CAM CONN FAILURE.") else: - print("SCRIPT: CAM CONN") - cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) - cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) - cap.set(cv2.CAP_PROP_BUFFERSIZE, 0) + print("CAM CONN SUCCESS.") + print("\n") - counter = 0 - otime = datetime.now() - if cap.isOpened(): - while(True): - counter+=1 + return cap - # frame = cv.imread('testing.jpg') - _, frame = cap.read() +if __name__ == "__main__": + cap = getCamera() - t1 = threading.Thread(target=run_april, args=(frame,)) - t1.start() - t1.join() + if cap is None: + print("Exiting, camera is None.") + exit(1) + subprocess.run(['v4l2-ctl', f'-c exposure_absolute={exposure_pref.get()}']) + - if counter % 10 == 0: - counter = 0 - print("FPS: " + str((datetime.now()-otime))) - otime = datetime.now() - - - #if cv.waitKey(1)== ord('q'): - # break + cap.set(cv2.CAP_PROP_FRAME_WIDTH, int(h_frame_size.get())) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, int(v_frame_size.get())) + cap.set(cv2.CAP_PROP_BUFFERSIZE, 0) + if cap.isOpened(): + t1 = threading.Thread(target=process_frames) + t1.start() - cap.release() - + while True: + _, frame = cap.read() + if frame is None: + continue + queue.append(frame) + if (len(queue) > 5): + queue.pop(0) -RUN_APRIL_846() \ No newline at end of file + cap.release() \ No newline at end of file diff --git a/src/y2024/resources/jetson/readAprilTag2.py b/src/y2024/resources/jetson/readAprilTag2.py deleted file mode 100644 index 12ea5d5..0000000 --- a/src/y2024/resources/jetson/readAprilTag2.py +++ /dev/null @@ -1,155 +0,0 @@ -from pupil_apriltags import Detector -import numpy as np -import time -import cv2 -import threading -from networktables import NetworkTables -import subprocess -from pref import NumericPref, BooleanPref, KillSwitch - -NetworkTables.initialize(server='10.8.46.2') -table = NetworkTables.getTable("AprilTags") - -preferenceTable = NetworkTables.getTable("JetsonPreferences") - -kill_switch = KillSwitch(preferenceTable) - -exposure_pref = NumericPref(preferenceTable, "camera_exposure", 1) -horizontal_fov_pref = NumericPref(preferenceTable, "h_fov", 63.1) -vertical_fov_pref = NumericPref(preferenceTable, "v_fov", 50.0) - -h_frame_size = NumericPref(preferenceTable, "h_frame", 640) -v_frame_size = NumericPref(preferenceTable, "v_frame", 480) - -cam_latency = NumericPref(preferenceTable, "camera_latency", 0.01) - -target_mean_brightness = NumericPref(preferenceTable, "target_mean_brightness", 120) - -clip_limit = NumericPref(preferenceTable, "clip_limit", 2.0) - -at_detector = Detector(families='tag36h11', - nthreads=1, - quad_decimate=1.0, - quad_sigma=0.2, - refine_edges=1, - decode_sharpening=0.5, - debug=0) - -cameraMatrix = np.array([(336.7755634193813, 0.0, 333.3575643300718), (0.0, 336.02729840829176, 212.77376312080065), (0.0, 0.0, 1.0)]) - -camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] ) - -queue = [] - -def adjust_gamma(image, gamma=1.0): - invGamma = 1.0 / gamma - table = np.array([((i / 255.0) ** invGamma) * 255 - for i in np.arange(0, 256)]).astype("uint8") - return cv2.LUT(image, table) - -def process_frames(): - global table - global queue - - global cam_latency - global horizontal_fov_pref - global vertical_fov_pref - global h_frame_size - global v_frame_size - - validTagIds = [4, 7] - - CAM_FOV_H = horizontal_fov_pref.get() - CAM_FOV_V = vertical_fov_pref.get() - - CAM_SZ_H = h_frame_size.get() - CAM_SZ_V = v_frame_size.get() - - clahe = cv2.createCLAHE(clipLimit=clip_limit.get(), tileGridSize=(8,8)) - - tmean_b = target_mean_brightness.get() - - while True: - if len(queue) == 0: continue - - frame = queue.pop(0) - - process_start_time = time.time() - - img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - img = clahe.apply(img) - - mean_brightness = np.mean(img) - - if (mean_brightness != 0): img = adjust_gamma(img, tmean_b / mean_brightness) - - tags = at_detector.detect(img, False, camera_params, 0.065) - - table.putNumber("tx", 0.0) - table.putNumber("ty", 0.0) - table.putNumber("tid", -1) - - try: - for tag in tags: - if not (tag.tag_id in validTagIds): continue - - cx, cy = tag.center - - tx = (cx - CAM_SZ_H/2) * (CAM_FOV_H / CAM_SZ_H) - ty = -((cy - CAM_SZ_V/2) * (CAM_FOV_V / CAM_SZ_V)) - - table.putNumber("tid", tag.tag_id) - table.putNumber("tx", tx) - - table.putNumber("ty", ty) - - except: - pass - - table.putNumber("latency", cam_latency.get() + time.time() - process_start_time) - NetworkTables.flush() - -def getCamera() -> cv2.VideoCapture: - print("\n") - cap = cv2.VideoCapture("/dev/video0") - if cap.isOpened() == False: - print("CAM 1 FAILURE, ATTEMPTING CAM 2.") - cap = cv2.VideoCapture("/dev/video1") - if cap.isOpened() == False: - print("CAM CONN FAILURE.") - else: - print("CAM CONN SUCCESS.") - print("\n") - - return cap - -if __name__ == "__main__": - cap = getCamera() - - if cap is None: - print("Exiting, camera is None.") - exit(1) - - subprocess.run(['v4l2-ctl', f'-c exposure_absolute={exposure_pref.get()}']) - - - cap.set(cv2.CAP_PROP_FRAME_WIDTH, int(h_frame_size.get())) - cap.set(cv2.CAP_PROP_FRAME_HEIGHT, int(v_frame_size.get())) - cap.set(cv2.CAP_PROP_BUFFERSIZE, 0) - - if cap.isOpened(): - t1 = threading.Thread(target=process_frames) - t1.start() - - while True: - _, frame = cap.read() - - if frame is None: - continue - - queue.append(frame) - - if (len(queue) > 5): - queue.pop(0) - - cap.release() \ No newline at end of file From 295c91f8af870db6058684eebe92f053fa40e74a Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Mon, 14 Oct 2024 14:37:55 -0700 Subject: [PATCH 06/12] further auto testing --- src/deploy/autos/paths/drive_auto | 3 +- src/frc846/cpp/frc846/ntinf/fstore.cc | 35 +++++++- src/frc846/cpp/frc846/robot/GenericRobot.cc | 15 ++++ .../swerve/follow_trajectory_command.cc | 2 +- .../swerve/waypt_traversal_calculator.cc | 2 +- src/frc846/include/frc846/ntinf/fstore.h | 5 ++ src/y2024/cpp/FunkyRobot.cc | 11 +-- .../cpp/commands/basic/wrist_zero_command.cc | 4 +- .../cpp/commands/complex/stow_zero_action.cc | 5 +- src/y2024/cpp/control_triggers.cc | 8 +- src/y2024/cpp/field.cc | 1 - .../subsystems/abstract/super_structure.cc | 18 +---- src/y2024/cpp/subsystems/hardware/wrist.cc | 79 +++++++++++-------- .../subsystems/abstract/super_structure.h | 5 -- src/y2024/include/subsystems/hardware/wrist.h | 65 +++------------ 15 files changed, 124 insertions(+), 134 deletions(-) diff --git a/src/deploy/autos/paths/drive_auto b/src/deploy/autos/paths/drive_auto index 0d07b52..672ff0f 100644 --- a/src/deploy/autos/paths/drive_auto +++ b/src/deploy/autos/paths/drive_auto @@ -1,2 +1 @@ -P,217.5,80,0,0 -P,217.5,180,0,0 \ No newline at end of file +P,217.5,169,0,0 \ No newline at end of file diff --git a/src/frc846/cpp/frc846/ntinf/fstore.cc b/src/frc846/cpp/frc846/ntinf/fstore.cc index 6b3d812..94fe0e0 100644 --- a/src/frc846/cpp/frc846/ntinf/fstore.cc +++ b/src/frc846/cpp/frc846/ntinf/fstore.cc @@ -22,6 +22,8 @@ bool FunkyStore::hasChanged = false; std::map> FunkyStore::prefs = {{"Default.int", 0.0}}; +std::unordered_set FunkyStore::keys_accessed{}; + std::string FunkyStore::variantToString( std::variant& variant) { return std::visit( @@ -79,6 +81,7 @@ std::string trim(const std::string& str) { } bool FunkyStore::ContainsKey(std::string key) { + keys_accessed.insert(key); if (!hasReadPrefs) { return false; } @@ -93,6 +96,7 @@ bool FunkyStore::ContainsKey(std::string key) { void FunkyStore::SetDouble(std::string key, double val) { FunkyStore::SetString(key, std::to_string(val), true); + keys_accessed.insert(key + ".double"); hasChanged = true; } @@ -122,6 +126,7 @@ double FunkyStore::GetDouble(std::string key, double fallback) { void FunkyStore::SetInt(std::string key, int val) { prefs[key + ".int"] = val; + keys_accessed.insert(key + ".int"); hasChanged = true; } @@ -142,9 +147,8 @@ int FunkyStore::GetInt(std::string key, int fallback) { } void FunkyStore::SetBoolean(std::string key, bool val) { - while (prefs.empty()) { - } prefs[key + ".bool"] = val; + keys_accessed.insert(key + ".bool"); hasChanged = true; } @@ -167,8 +171,10 @@ bool FunkyStore::GetBoolean(std::string key, bool fallback) { void FunkyStore::SetString(std::string key, std::string val, bool isDouble) { if (!isDouble) { prefs[key + ".string"] = val; + keys_accessed.insert(key + ".string"); } else { prefs[key + ".double"] = val; + keys_accessed.insert(key + ".double"); } hasChanged = true; @@ -270,6 +276,31 @@ void FunkyStore::WriteToDisk() { AppendToChangeLog(); } +std::vector FunkyStore::GetPruneList() { + std::vector result{}; + for (std::map>::iterator it = + FunkyStore::prefs.begin(); + it != FunkyStore::prefs.end(); ++it) { + bool foundKey = false; + for (const std::string& x : FunkyStore::keys_accessed) { + if (it->first == x) { + foundKey = true; + break; + } + } + if (!foundKey) result.push_back(it->first); + } + return result; +} + +void FunkyStore::Prune() { + for (const std::string& x : GetPruneList()) { + prefs.erase(x); + } + hasChanged = true; +} + void FunkyStore::AppendToChangeLog() { // auto filename = "/home/lvuser/preferences.changelog"; // std::map> prefs; + static std::unordered_set keys_accessed; + static std::vector changes; static bool ContainsKey(std::string key); @@ -65,6 +67,9 @@ class FunkyStore { void WriteToDisk(); void AppendToChangeLog(); + std::vector GetPruneList(); + void Prune(); + static void HardReadPrefs(); static void FP_HardReadPrefs(); diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index 223f6d9..f28e882 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -87,7 +87,8 @@ void FunkyRobot::InitTeleop() { container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); container_.control_input_.SetDefaultCommand( OperatorControlCommand{container_}); - container_.super_structure_.SetDefaultCommand(StowCommand{container_}); + container_.super_structure_.SetDefaultCommand( + StowZeroActionCommand{container_}); container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_}); container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); container_.bracer_.SetDefaultCommand(BracerCommand{container_}); @@ -96,10 +97,10 @@ void FunkyRobot::InitTeleop() { } void FunkyRobot::OnPeriodic() { - Graph("homing_switch", homing_switch_.Get()); - Graph("coasting_switch", coasting_switch_.Get()); + Graph("homing_switch", !homing_switch_.Get()); + Graph("coasting_switch", !coasting_switch_.Get()); - if (homing_switch_.Get()) { + if (!homing_switch_.Get()) { container_.super_structure_.ZeroSubsystem(); Log("Zeroing subsystems..."); } @@ -114,7 +115,7 @@ void FunkyRobot::OnPeriodic() { coasting_time_ .value(); // To prevent Brake from being called each periodic - } else if (coasting_switch_.Get()) { + } else if (!coasting_switch_.Get()) { container_.pivot_.Coast(); container_.wrist_.Coast(); container_.telescope_.Coast(); diff --git a/src/y2024/cpp/commands/basic/wrist_zero_command.cc b/src/y2024/cpp/commands/basic/wrist_zero_command.cc index ab416df..95ebe77 100644 --- a/src/y2024/cpp/commands/basic/wrist_zero_command.cc +++ b/src/y2024/cpp/commands/basic/wrist_zero_command.cc @@ -7,11 +7,11 @@ WristZeroCommand::WristZeroCommand(RobotContainer& container) } void WristZeroCommand::OnInit() { - container_.super_structure_.wrist_->DeZero(); + Log("Init Wrist Zero Command."); + container_.super_structure_.wrist_->HomeSubsystem(); } void WristZeroCommand::Periodic() { - container_.super_structure_.HomeWrist(); container_.super_structure_.SetTargetSetpoint( container_.super_structure_.getStowSetpoint()); } diff --git a/src/y2024/cpp/commands/complex/stow_zero_action.cc b/src/y2024/cpp/commands/complex/stow_zero_action.cc index dc86219..0f6c6ed 100644 --- a/src/y2024/cpp/commands/complex/stow_zero_action.cc +++ b/src/y2024/cpp/commands/complex/stow_zero_action.cc @@ -16,10 +16,7 @@ StowZeroActionCommand::StowZeroActionCommand(RobotContainer& container) container, "stow_zero_action_command", frc2::SequentialCommandGroup{ frc2::ParallelDeadlineGroup{ - frc2::WaitUntilCommand{[&] { - return container.pivot_.WithinTolerance( - container.super_structure_.getStowSetpoint().pivot); - }}, + frc2::WaitCommand{1.65_s}, StowCommand{container}}, WristZeroCommand{container}, StowCommand{container} /* Otherwise, this command will be diff --git a/src/y2024/cpp/control_triggers.cc b/src/y2024/cpp/control_triggers.cc index fec4c76..1a2cb89 100644 --- a/src/y2024/cpp/control_triggers.cc +++ b/src/y2024/cpp/control_triggers.cc @@ -134,12 +134,8 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { trap_stage_trigger.WhileTrue(TrapCommand{container, i}.ToPtr()); } - frc2::Trigger wrist_zero_trigger{[&] { - return container.control_input_.GetReadings().home_wrist && - units::math::abs(container.pivot_.GetReadings().pivot_position - - container.pivot_.pivot_home_offset_.value()) < - container.pivot_.pivot_tolerance_.value(); - }}; + frc2::Trigger wrist_zero_trigger{ + [&] { return container.control_input_.GetReadings().home_wrist; }}; wrist_zero_trigger.OnTrue(WristZeroCommand{container}.ToPtr()); diff --git a/src/y2024/cpp/field.cc b/src/y2024/cpp/field.cc index c55b027..02559a6 100644 --- a/src/y2024/cpp/field.cc +++ b/src/y2024/cpp/field.cc @@ -83,7 +83,6 @@ std::string Field_nonstatic::forceNormalPath(std::string path) { std::string Field_nonstatic::getFileDirectory() { #ifdef _WIN32 std::string deploy_dir = frc::filesystem::GetDeployDirectory(); - size_t start_pos = 0; std::vector tokens = split(deploy_dir, '\\'); std::string rejoined{}; diff --git a/src/y2024/cpp/subsystems/abstract/super_structure.cc b/src/y2024/cpp/subsystems/abstract/super_structure.cc index 6dc956e..30133ce 100644 --- a/src/y2024/cpp/subsystems/abstract/super_structure.cc +++ b/src/y2024/cpp/subsystems/abstract/super_structure.cc @@ -21,23 +21,7 @@ void SuperStructureSubsystem::WriteToHardware(SuperStructureTarget target) { pivot_->SetTarget({targetPos.pivot}); telescope_->SetTarget({targetPos.telescope}); - if (homing_wrist) { - wrist_->SetTarget({wrist_->homing_speed_.value(), true}); - if (wrist_->GetReadings().wrist_velocity <= - wrist_->homing_velocity_tolerance_.value()) { - wrist_home_counter++; - } else { - wrist_home_counter = 0; - } - if (wrist_home_counter >= 30) { - wrist_->ZeroSubsystem(); - homing_wrist = false; - wrist_home_counter = 0; - } - } else { - wrist_->SetTarget({targetPos.wrist}); - wrist_home_counter = 0; - } + wrist_->SetTarget({targetPos.wrist}); auto target_end_effector_positions = arm_kinematics_calculator.calculate( {targetPos.pivot, targetPos.telescope, targetPos.wrist}); diff --git a/src/y2024/cpp/subsystems/hardware/wrist.cc b/src/y2024/cpp/subsystems/hardware/wrist.cc index e42c1e5..cdc0abe 100644 --- a/src/y2024/cpp/subsystems/hardware/wrist.cc +++ b/src/y2024/cpp/subsystems/hardware/wrist.cc @@ -17,7 +17,7 @@ void WristSubsystem::Setup() {} WristTarget WristSubsystem::ZeroTarget() const { WristTarget target; - target.wrist_output = wrist_home_offset_.value(); + target.target_position = wrist_home_offset_.value(); return target; } @@ -35,50 +35,61 @@ bool WristSubsystem::VerifyHardware() { WristReadings WristSubsystem::ReadFromHardware() { WristReadings readings; - readings.wrist_position = wrist_esc_.GetPosition(); + readings.position = wrist_esc_.GetPosition(); + Graph("readings/position", readings.position.to()); - frc846::util::ShareTables::SetDouble("wrist_position", - readings.wrist_position.to()); - - wrist_pos_graph.Graph(readings.wrist_position); - - auto target_output = GetTarget().wrist_output; - if (auto target_angle = std::get_if(&target_output)) { - wrist_error_graph.Graph(*target_angle - readings.wrist_position); + readings.cg_position = + 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") - + GetReadings().position + wrist_cg_offset_.value(); + Graph("readings/cg_position", readings.cg_position.to()); + + Graph("readings/error", + (GetTarget().target_position - readings.position).to()); + + Graph("readings/velocity", wrist_esc_.GetVelocity().to()); + + if (!hasZeroed) { + if (units::math::abs(wrist_esc_.GetVelocity()) < + homing_velocity_tolerance_.value()) { + Graph("readings/within_homing_velocity", true); + homing_counter_ += 1; + } else { + Graph("readings/within_homing_velocity", false); + homing_counter_ = 0; + } + if (homing_counter_ > 30) { + hasZeroed = true; + wrist_esc_.ZeroEncoder(wrist_home_offset_.value()); + Log("Wrist homed successfully."); + } } - readings.wrist_velocity = wrist_esc_.GetVelocity(); - return readings; } void WristSubsystem::WriteToHardware(WristTarget target) { - wrist_weight_pos_graph.Graph( - 1_deg * frc846::util::ShareTables::GetDouble("pivot_position") - - GetReadings().wrist_position + wrist_cg_offset_.value()); - - hard_limits_.OverrideLimits(target.override_limits); - if (auto pos = std::get_if(&target.wrist_output)) { - double output = dyFPID.calculate(*pos, GetReadings().wrist_position, - wrist_esc_.GetVelocityPercentage(), - gains_prefs_dyFPID.get()); - // if (units::math::abs(*pos - readings().wrist_position) < 5_deg) { - // output = dyFPIDClose.calculate(*pos, readings().wrist_position, - // wrist_esc_.GetVelocityPercentage(), - // config_helper_.updateAndGetGains()); - // } + if (!hasZeroed) { + hard_limits_.OverrideLimits(true); - wrist_esc_.WriteDC(output); - - target_wrist_pos_graph.Graph(*pos); - - } else if (auto output = std::get_if(&target.wrist_output)) { + double output = homing_speed_.value(); if (units::math::abs(wrist_esc_.GetVelocity()) > homing_max_speed_.value()) { - *output = (*output) / homing_dc_cut_.value(); + output = output / homing_dc_cut_.value(); + Graph("target/homing_too_fast", true); + } else { + Graph("target/homing_too_fast", false); } - wrist_esc_.WriteDC(*output); + Graph("target/homing_output", output); + wrist_esc_.WriteDC(output); - target_wrist_duty_cycle_graph.Graph(*output); + } else { + hard_limits_.OverrideLimits(false); + + double output = dyFPID.calculate( + target.target_position, GetReadings().position, + wrist_esc_.GetVelocityPercentage(), gains_prefs_dyFPID.get()); + + Graph("target/fpid_output", output); + wrist_esc_.WriteDC(output); } } \ No newline at end of file diff --git a/src/y2024/include/subsystems/abstract/super_structure.h b/src/y2024/include/subsystems/abstract/super_structure.h index e64785a..af336c6 100644 --- a/src/y2024/include/subsystems/abstract/super_structure.h +++ b/src/y2024/include/subsystems/abstract/super_structure.h @@ -78,11 +78,8 @@ class SuperStructureSubsystem bool VerifyHardware() override; - void HomeWrist() { homing_wrist = true; } - void ZeroSubsystem() { pivot_->ZeroSubsystem(); - wrist_->ZeroSubsystem(); telescope_->ZeroSubsystem(); hasZeroed = true; } @@ -234,8 +231,6 @@ class SuperStructureSubsystem private: bool hasZeroed = false; - bool homing_wrist = false; - int wrist_home_counter = 0; ArmKinematicsCalculator arm_kinematics_calculator; diff --git a/src/y2024/include/subsystems/hardware/wrist.h b/src/y2024/include/subsystems/hardware/wrist.h index 43f70b6..4d1fb27 100644 --- a/src/y2024/include/subsystems/hardware/wrist.h +++ b/src/y2024/include/subsystems/hardware/wrist.h @@ -12,13 +12,12 @@ #include "units/math.h" struct WristReadings { - units::degree_t wrist_position; - units::degrees_per_second_t wrist_velocity; + units::degree_t position; + units::degree_t cg_position; }; struct WristTarget { - std::variant wrist_output; - bool override_limits = false; + units::degree_t target_position; }; class WristSubsystem @@ -37,8 +36,8 @@ class WristSubsystem bool GetHasZeroed() { return hasZeroed; } bool WithinTolerance(units::degree_t pos) { - return (units::math::abs(pos - GetReadings().wrist_position) < - wrist_tolerance_.value()); + return units::math::abs(GetReadings().position - pos) < + wrist_tolerance_.value(); } void Coast() { @@ -51,11 +50,9 @@ class WristSubsystem esc->SetIdleMode(rev::CANSparkBase::IdleMode::kBrake); } - void DeZero() { hasZeroed = false; } - - void ZeroSubsystem() { - hasZeroed = true; - wrist_esc_.ZeroEncoder(wrist_home_offset_.value()); + void HomeSubsystem() { + homing_counter_ = 0; + hasZeroed = false; SetTarget(ZeroTarget()); } @@ -80,35 +77,14 @@ class WristSubsystem frc846::ntinf::Pref homing_velocity_tolerance_{ *this, "homing_velocity_tolerance", 1.0_deg_per_s}; - frc846::ntinf::Pref num_loops_homed_{*this, "num_loops_homed", 20}; + frc846::ntinf::Pref homing_num_loops_{*this, "homing_num_loops", 20}; frc846::ntinf::Pref homing_speed_{*this, "homing_speed", -0.2}; frc846::ntinf::Pref homing_max_speed_{ *this, "homing_max_speed", 30.0_deg_per_s}; frc846::ntinf::Pref homing_dc_cut_{*this, "homing_dc_cut", 1.5}; private: - frc846::base::Loggable gains_{*this, "gains"}; - frc846::ntinf::Pref k_{gains_, "k", 0.0}; - frc846::ntinf::Pref p_{gains_, "p", 0.0}; - frc846::ntinf::Pref d_{gains_, "d", 0.0}; - - frc846::base::Loggable readings_named_{*this, "readings"}; - - frc846::ntinf::Grapher wrist_pos_graph{target_named_, - "wrist_pos"}; - frc846::ntinf::Grapher wrist_error_graph{target_named_, - "wrist_error"}; - - frc846::ntinf::Grapher wrist_weight_pos_graph{ - target_named_, "wrist_weight_position"}; - - frc846::base::Loggable target_named_{*this, "target"}; - - frc846::ntinf::Grapher target_wrist_duty_cycle_graph{ - target_named_, "wrist_duty_cycle"}; - - frc846::ntinf::Grapher target_wrist_pos_graph{target_named_, - "wrist_pos"}; + int homing_counter_ = 0; frc846::control::ConfigHelper config_helper_{ *this, @@ -136,27 +112,8 @@ class WristSubsystem frc846::motion::BrakingPositionDyFPID dyFPID{ dyFPID_loggable, [&](units::degree_t pos) -> double { - return units::math::sin( - 1_deg * - frc846::util::ShareTables::GetDouble("pivot_position") - - GetReadings().wrist_position + wrist_cg_offset_.value()) - .to(); + return units::math::sin(GetReadings().cg_position).to(); }, {30_A, frc846::control::DefaultSpecifications::stall_current_neo, 0.3}, &hard_limits_}; - - // frc846::base::Loggable close_dyFPID_loggable{*this, "CloseDynamicFPID"}; - - // frc846::motion::BrakingPositionDyFPID dyFPIDClose{ - // close_dyFPID_loggable, - // [this](units::degree_t pos) -> double { - // return std::abs( - // units::math::cos( - // 1_deg * - // frc846::util::ShareTables::GetDouble("pivot_position") - - // GetReadings().wrist_position + wrist_cg_offset_.value()) - // .to()); - // }, - // {15_A, frc846::control::DefaultSpecifications::stall_current_neo, - // 0.3}}; }; From 913296ea96630d13835549055cbef252bee40943 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Mon, 21 Oct 2024 18:54:12 -0700 Subject: [PATCH 07/12] Red side works --- networktables.json | 32 ++++++++++++++ networktables.json.bck | 32 ++++++++++++++ simgui.json | 2 + src/deploy/autos/paths/fp_home | 2 +- src/deploy/autos/paths/fp_piece_one | 2 +- src/deploy/autos/paths/fp_piece_three | 2 +- src/deploy/autos/paths/fp_piece_two | 2 +- src/deploy/autos/scripts/four_piece_auto | 14 +++---- src/frc846/include/frc846/math/fieldpoints.h | 8 ++-- src/y2024/cpp/FunkyRobot.cc | 4 +- src/y2024/cpp/autos/ActionMaker.cc | 3 ++ src/y2024/cpp/autos/GenericAuto.cc | 42 +++++++++++++------ src/y2024/cpp/commands/basic/trap_command.cc | 2 +- .../cpp/commands/complex/stow_zero_action.cc | 5 +-- .../cpp/commands/teleop/drive_command.cc | 31 ++++++++++---- .../subsystems/abstract/super_structure.cc | 2 +- src/y2024/cpp/subsystems/hardware/bracer.cc | 6 --- src/y2024/cpp/subsystems/hardware/pivot.cc | 16 ++++++- .../subsystems/abstract/super_structure.h | 5 ++- .../include/subsystems/hardware/bracer.h | 3 -- .../include/subsystems/hardware/drivetrain.h | 3 ++ src/y2024/include/subsystems/hardware/pivot.h | 10 +++++ 22 files changed, 174 insertions(+), 54 deletions(-) diff --git a/networktables.json b/networktables.json index 10bfd50..726d3d4 100644 --- a/networktables.json +++ b/networktables.json @@ -2566,5 +2566,37 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/drivetrain/intake_align_gain", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_num_loops", + "type": "int", + "value": 20, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/climb_duty_cycle", + "type": "double", + "value": -0.25, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Robot/coasting_time (s)", + "type": "double", + "value": 7.5, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index 10bfd50..726d3d4 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -2566,5 +2566,37 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/drivetrain/intake_align_gain", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/wrist/homing_num_loops", + "type": "int", + "value": 20, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/pivot/climb_duty_cycle", + "type": "double", + "value": -0.25, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Robot/coasting_time (s)", + "type": "double", + "value": 7.5, + "properties": { + "persistent": true + } } ] diff --git a/simgui.json b/simgui.json index 43060a7..3a86912 100644 --- a/simgui.json +++ b/simgui.json @@ -50,6 +50,8 @@ "/SmartDashboard/SendableChooser[0]": "String Chooser", "/SmartDashboard/brake_subsystems": "Command", "/SmartDashboard/coast_subsystems": "Command", + "/SmartDashboard/get_prune_list": "Command", + "/SmartDashboard/prune_prefs": "Command", "/SmartDashboard/verify_hardware": "Command", "/SmartDashboard/zero_bearing": "Command", "/SmartDashboard/zero_modules": "Command", diff --git a/src/deploy/autos/paths/fp_home b/src/deploy/autos/paths/fp_home index 500982f..660d3f4 100644 --- a/src/deploy/autos/paths/fp_home +++ b/src/deploy/autos/paths/fp_home @@ -1 +1 @@ -F,kPointBlank \ No newline at end of file +P,217.5,49.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_one b/src/deploy/autos/paths/fp_piece_one index b66b275..aaba24a 100644 --- a/src/deploy/autos/paths/fp_piece_one +++ b/src/deploy/autos/paths/fp_piece_one @@ -1 +1 @@ -P,217.5,112,0,0 \ No newline at end of file +P,217.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_three b/src/deploy/autos/paths/fp_piece_three index b66b275..73a9e6d 100644 --- a/src/deploy/autos/paths/fp_piece_three +++ b/src/deploy/autos/paths/fp_piece_three @@ -1 +1 @@ -P,217.5,112,0,0 \ No newline at end of file +P,270.5,92,20,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_piece_two b/src/deploy/autos/paths/fp_piece_two index b66b275..c0c5536 100644 --- a/src/deploy/autos/paths/fp_piece_two +++ b/src/deploy/autos/paths/fp_piece_two @@ -1 +1 @@ -P,217.5,112,0,0 \ No newline at end of file +P,153.5,90,-20,0 \ No newline at end of file diff --git a/src/deploy/autos/scripts/four_piece_auto b/src/deploy/autos/scripts/four_piece_auto index e45a6d2..c2c44c5 100644 --- a/src/deploy/autos/scripts/four_piece_auto +++ b/src/deploy/autos/scripts/four_piece_auto @@ -1,21 +1,19 @@ 0,2 F,kPointBlank -ACT,auto_home ACT,prep_shoot ACT,shoot ACT,deploy_intake PATH,fp_piece_one -ACT,prep_shoot +ACT,a_prep_shoot PATH,fp_home ACT,shoot ACT,deploy_intake PATH,fp_piece_two -ACT,prep_shoot -PATH,fp_home +ACT,a_prep_shoot +PATH,fp_home_2 ACT,shoot ACT,deploy_intake PATH,fp_piece_three -ACT,prep_shoot -PATH,fp_home -ACT,shoot -PATH,fp_center_run \ No newline at end of file +ACT,a_prep_shoot +PATH,fp_home_3 +ACT,shoot \ No newline at end of file diff --git a/src/frc846/include/frc846/math/fieldpoints.h b/src/frc846/include/frc846/math/fieldpoints.h index db89644..e278264 100644 --- a/src/frc846/include/frc846/math/fieldpoints.h +++ b/src/frc846/include/frc846/math/fieldpoints.h @@ -23,7 +23,7 @@ struct FieldPoint { if (shouldMirror) { return FieldPoint{{field_size_x - point[0], field_size_y - point[1]}, 180_deg + bearing, - {-velocity[0], -velocity[1]}}; + {velocity[0], velocity[1]}}; } return FieldPoint{ {point[0], point[1]}, bearing, {velocity[0], velocity[1]}}; @@ -34,14 +34,14 @@ struct FieldPoint { if (shouldMirror) { return FieldPoint{{point[0], field_size_y - point[1]}, 180_deg - bearing, - {velocity[0], -velocity[1]}}; + {velocity[0], velocity[1]}}; } return mirror(false); } private: - static constexpr units::inch_t field_size_x = 651.25_in; - static constexpr units::inch_t field_size_y = 315.5_in; + static constexpr units::inch_t field_size_y = 651.25_in; + static constexpr units::inch_t field_size_x = 315.5_in; }; class FieldPointPreference { diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index f28e882..fbaf3be 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -87,11 +87,11 @@ void FunkyRobot::InitTeleop() { container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); container_.control_input_.SetDefaultCommand( OperatorControlCommand{container_}); - container_.super_structure_.SetDefaultCommand( - StowZeroActionCommand{container_}); + container_.super_structure_.SetDefaultCommand(StowCommand{container_}); container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_}); container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_}); container_.bracer_.SetDefaultCommand(BracerCommand{container_}); + container_.leds_.SetDefaultCommand(LEDsCommand{container_}); ControlTriggerInitializer::InitTeleopTriggers(container_); } diff --git a/src/y2024/cpp/autos/ActionMaker.cc b/src/y2024/cpp/autos/ActionMaker.cc index 2f17d13..1780bc0 100644 --- a/src/y2024/cpp/autos/ActionMaker.cc +++ b/src/y2024/cpp/autos/ActionMaker.cc @@ -2,6 +2,7 @@ #include "commands/basic/auto_deploy_intake_command.h" #include "commands/basic/auto_shoot_command.h" +#include "commands/basic/prepare_auto_shoot_async.h" #include "commands/basic/prepare_auto_shoot_command.h" #include "commands/complex/home_during_auto_command.h" @@ -11,6 +12,8 @@ std::unique_ptr ActionMaker::GetAction( return std::make_unique(container); } else if (name == "prep_shoot") { return std::make_unique(container); + } else if (name == "a_prep_shoot") { + return std::make_unique(container); } else if (name == "deploy_intake") { return std::make_unique(container); } else if (name == "auto_home" || true) { diff --git a/src/y2024/cpp/autos/GenericAuto.cc b/src/y2024/cpp/autos/GenericAuto.cc index 465ea13..0595d79 100644 --- a/src/y2024/cpp/autos/GenericAuto.cc +++ b/src/y2024/cpp/autos/GenericAuto.cc @@ -15,18 +15,36 @@ GenericAuto::GenericAuto(RobotContainer& container, AutoData data, std::vector> GenericAuto::buildActionsGroup( AutoData data, RobotContainer& container, bool is_blue_side) { std::vector> cmds{}; - cmds.push_back(std::make_unique([&, auto_data = data] { - Log("Starting Auto: {}.", auto_data.name); - - int mirror = is_blue_side ? (int)auto_data.blue : (int)auto_data.red; - - auto start = mirror == 0 ? auto_data.start - : (mirror == 1 ? auto_data.start.mirror() - : auto_data.start.mirrorOnlyY()); - - container.drivetrain_.SetPoint(start.point); - container.drivetrain_.SetBearing(start.bearing); - })); + cmds.push_back(std::make_unique( + [&, auto_data = data, is_blue = is_blue_side] { + Log("Starting Auto: {}.", auto_data.name); + + int mirror = is_blue ? (int)auto_data.blue : (int)auto_data.red; + + auto start = auto_data.start; + + Log("Pre-flip: Setting start to x{} y{}.", start.point[0].to(), + start.point[1].to()); + Log("Pre-flip: Setting bearing to {} deg.", start.bearing.to()); + + if (mirror == 2) { + start = auto_data.start.mirrorOnlyY(true); + Log("Flipping Only Y."); + } else if (mirror == 1) { + start = auto_data.start.mirror(true); + Log("Flipping."); + } + + Log("Setting mirror to {} for blue side {}", mirror, is_blue); + Log("Default mirror red {} blue {}", (int)auto_data.red, + (int)auto_data.blue); + Log("Setting start to x{} y{}.", start.point[0].to(), + start.point[1].to()); + Log("Setting bearing to {} deg.", start.bearing.to()); + + container.drivetrain_.SetPoint(start.point); + container.drivetrain_.SetBearing(start.bearing); + })); for (auto& action : data.actions) { if (auto* action_name = std::get_if(&action)) { cmds.push_back(ActionMaker::GetAction(*action_name, container)); diff --git a/src/y2024/cpp/commands/basic/trap_command.cc b/src/y2024/cpp/commands/basic/trap_command.cc index ba2e5ff..e91d71a 100644 --- a/src/y2024/cpp/commands/basic/trap_command.cc +++ b/src/y2024/cpp/commands/basic/trap_command.cc @@ -25,7 +25,7 @@ void TrapCommand::Periodic() { auto pullClimbTarget = container_.super_structure_.getPreClimbSetpoint(); pullClimbTarget.pivot = container_.super_structure_.pivot_pull_target_.value(); - container_.super_structure_.SetTargetSetpoint(pullClimbTarget); + container_.super_structure_.SetTargetSetpoint(pullClimbTarget, true); } if (stage_ == 3) { container_.intake_.SetTarget(container_.intake_.ZeroTarget()); diff --git a/src/y2024/cpp/commands/complex/stow_zero_action.cc b/src/y2024/cpp/commands/complex/stow_zero_action.cc index 0f6c6ed..3b3afe7 100644 --- a/src/y2024/cpp/commands/complex/stow_zero_action.cc +++ b/src/y2024/cpp/commands/complex/stow_zero_action.cc @@ -15,9 +15,8 @@ StowZeroActionCommand::StowZeroActionCommand(RobotContainer& container) frc2::SequentialCommandGroup>{ container, "stow_zero_action_command", frc2::SequentialCommandGroup{ - frc2::ParallelDeadlineGroup{ - frc2::WaitCommand{1.65_s}, - StowCommand{container}}, + frc2::ParallelDeadlineGroup{frc2::WaitCommand{1.65_s}, + StowCommand{container}}, WristZeroCommand{container}, StowCommand{container} /* Otherwise, this command will be rescheduled -> infinite loop */ diff --git a/src/y2024/cpp/commands/teleop/drive_command.cc b/src/y2024/cpp/commands/teleop/drive_command.cc index 514cb3b..6becf7d 100644 --- a/src/y2024/cpp/commands/teleop/drive_command.cc +++ b/src/y2024/cpp/commands/teleop/drive_command.cc @@ -30,6 +30,7 @@ void DriveCommand::Periodic() { bool prep_align_speaker = container_.control_input_.GetReadings().running_super_shoot; bool amping = container_.control_input_.GetReadings().running_amp; + bool intaking = container_.control_input_.GetReadings().running_intake; bool sourcing = container_.control_input_.GetReadings().running_source; bool flipping_controls = @@ -77,15 +78,29 @@ void DriveCommand::Periodic() { container_.control_input_.driver_.steer_deadband_.value(), 1, container_.control_input_.driver_.steer_exponent_.value(), 1); - if (steer_x != 0) { - // Manual steer - auto target = steer_x * container_.drivetrain_.max_omega(); + // Manual steer + auto target_steer = steer_x * container_.drivetrain_.max_omega(); - drivetrain_target.rotation = DrivetrainRotationVelocity(target); - } else { - // Hold position - drivetrain_target.rotation = DrivetrainRotationVelocity(0_deg_per_s); - } + // if (intaking && + // container_.drivetrain_.GetReadings().pose.velocity.magnitude() > + // 1.0_fps) { + // auto current_velocity_bearing = + // container_.drivetrain_.GetReadings().pose.velocity.angle(true); + // auto target_velocity_bearing = 1_rad * std::atan2(translate_x, + // translate_y); + + // auto avg_bearing = + // (target_velocity_bearing + current_velocity_bearing) / 2.0; + + // auto steer_error = frc846::math::CoterminalDifference( + // target_velocity_bearing, avg_bearing); + + // target_steer += + // container_.drivetrain_.intake_align_gain_.value() * steer_error / + // 1_s; + // } + + drivetrain_target.rotation = DrivetrainRotationVelocity(target_steer); if (prep_align_speaker) { VisionReadings vision_readings = container_.vision_.GetReadings(); diff --git a/src/y2024/cpp/subsystems/abstract/super_structure.cc b/src/y2024/cpp/subsystems/abstract/super_structure.cc index 30133ce..f8173d4 100644 --- a/src/y2024/cpp/subsystems/abstract/super_structure.cc +++ b/src/y2024/cpp/subsystems/abstract/super_structure.cc @@ -18,7 +18,7 @@ SuperStructureReadings SuperStructureSubsystem::ReadFromHardware() { void SuperStructureSubsystem::WriteToHardware(SuperStructureTarget target) { PTWSetpoint targetPos = currentSetpoint + manualAdjustments; - pivot_->SetTarget({targetPos.pivot}); + pivot_->SetTarget({targetPos.pivot, climb_mode_}); telescope_->SetTarget({targetPos.telescope}); wrist_->SetTarget({targetPos.wrist}); diff --git a/src/y2024/cpp/subsystems/hardware/bracer.cc b/src/y2024/cpp/subsystems/hardware/bracer.cc index f171565..4b37888 100644 --- a/src/y2024/cpp/subsystems/hardware/bracer.cc +++ b/src/y2024/cpp/subsystems/hardware/bracer.cc @@ -29,12 +29,6 @@ bool BracerSubsystem::VerifyHardware() { BracerReadings BracerSubsystem::ReadFromHardware() { BracerReadings readings; - left_climb_.Graph(left_switch_.Get()); - right_climb_.Graph(right_switch_.Get()); - - frc846::util::ShareTables::SetBoolean( - "climb_hooks_engaged", left_switch_.Get() && right_switch_.Get()); - return readings; } diff --git a/src/y2024/cpp/subsystems/hardware/pivot.cc b/src/y2024/cpp/subsystems/hardware/pivot.cc index cc78e63..421140f 100644 --- a/src/y2024/cpp/subsystems/hardware/pivot.cc +++ b/src/y2024/cpp/subsystems/hardware/pivot.cc @@ -69,6 +69,8 @@ PivotReadings PivotSubsystem::ReadFromHardware() { readings.pivot_position = pivot_one_.GetPosition(); + readings.both_hooks_engaged = left_switch_.Get() && right_switch_.Get(); + frc846::util::ShareTables::SetDouble("pivot_position", readings.pivot_position.to()); @@ -83,7 +85,17 @@ PivotReadings PivotSubsystem::ReadFromHardware() { } void PivotSubsystem::WriteToHardware(PivotTarget target) { - if (auto pos = std::get_if(&target.pivot_output)) { + if (target.climb_mode) { + hard_limits_.OverrideLimits(true); + + pivot_one_.WriteDC(climb_duty_cycle_.value()); + pivot_two_.WriteDC(climb_duty_cycle_.value()); + pivot_three_.WriteDC(climb_duty_cycle_.value()); + pivot_four_.WriteDC(climb_duty_cycle_.value()); + + } else if (auto pos = std::get_if(&target.pivot_output)) { + hard_limits_.OverrideLimits(false); + double output = dyFPID.calculate(*pos, GetReadings().pivot_position, pivot_one_.GetVelocityPercentage(), config_helper_.updateAndGetGains()); @@ -95,6 +107,8 @@ void PivotSubsystem::WriteToHardware(PivotTarget target) { target_pivot_pos_graph.Graph(*pos); } else if (auto output = std::get_if(&target.pivot_output)) { + hard_limits_.OverrideLimits(false); + pivot_one_.WriteDC(*output); pivot_two_.WriteDC(*output); pivot_three_.WriteDC(*output); diff --git a/src/y2024/include/subsystems/abstract/super_structure.h b/src/y2024/include/subsystems/abstract/super_structure.h index af336c6..2a1ca40 100644 --- a/src/y2024/include/subsystems/abstract/super_structure.h +++ b/src/y2024/include/subsystems/abstract/super_structure.h @@ -53,6 +53,8 @@ class SuperStructureSubsystem PTWSetpoint currentSetpoint; PTWSetpoint manualAdjustments; + bool climb_mode_ = false; + units::degree_t wrist_trim_amt = 0_deg; public: @@ -86,13 +88,14 @@ class SuperStructureSubsystem bool GetHasZeroed() { return hasZeroed; } - void SetTargetSetpoint(PTWSetpoint newSetpoint) { + void SetTargetSetpoint(PTWSetpoint newSetpoint, bool climb_mode = false) { if (currentSetpoint.pivot != newSetpoint.pivot || currentSetpoint.wrist != newSetpoint.wrist || currentSetpoint.telescope != newSetpoint.telescope) { currentSetpoint = newSetpoint; ClearAdjustments(); } + climb_mode_ = climb_mode; } bool CheckValidAdjustment(PTWSetpoint adjusted) { diff --git a/src/y2024/include/subsystems/hardware/bracer.h b/src/y2024/include/subsystems/hardware/bracer.h index 36f54ba..d2a8e91 100644 --- a/src/y2024/include/subsystems/hardware/bracer.h +++ b/src/y2024/include/subsystems/hardware/bracer.h @@ -42,9 +42,6 @@ class BracerSubsystem frc::Spark bracer_{ports::bracer_::kPWM_Left}; - frc::DigitalInput left_switch_{3}; - frc::DigitalInput right_switch_{4}; - BracerReadings ReadFromHardware() override; void WriteToHardware(BracerTarget target) override; diff --git a/src/y2024/include/subsystems/hardware/drivetrain.h b/src/y2024/include/subsystems/hardware/drivetrain.h index 2cae8ee..39c8a6a 100644 --- a/src/y2024/include/subsystems/hardware/drivetrain.h +++ b/src/y2024/include/subsystems/hardware/drivetrain.h @@ -142,6 +142,9 @@ class DrivetrainSubsystem frc846::ntinf::Pref max_deceleration_{ *this, "max_deceleration", 10_fps_sq}; + frc846::ntinf::Pref intake_align_gain_{*this, "intake_align_gain", + 1.0}; + // Lookahead distance during trajectory following. frc846::ntinf::Pref extrapolation_distance_{ *this, "extrapolation_distance", 8_in}; diff --git a/src/y2024/include/subsystems/hardware/pivot.h b/src/y2024/include/subsystems/hardware/pivot.h index e62785a..a363c90 100644 --- a/src/y2024/include/subsystems/hardware/pivot.h +++ b/src/y2024/include/subsystems/hardware/pivot.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -16,10 +17,13 @@ struct PivotReadings { units::degree_t pivot_position; + + bool both_hooks_engaged; }; struct PivotTarget { std::variant pivot_output; + bool climb_mode = false; }; class PivotSubsystem @@ -88,9 +92,15 @@ class PivotSubsystem frc846::ntinf::Pref max_adjustment_rate_{ *this, "max_adjustment_rate", units::degrees_per_second_t(60.0)}; + frc846::ntinf::Pref climb_duty_cycle_{*this, "climb_duty_cycle", + -0.25}; + private: bool hasZeroed = false; + frc::DigitalInput left_switch_{3}; + frc::DigitalInput right_switch_{4}; + frc846::base::Loggable readings_named_{*this, "readings"}; frc846::ntinf::Grapher pivot_pos_graph{readings_named_, From 6857c423d9426085dba4e4d89dc56de8515a2410 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Mon, 21 Oct 2024 18:54:18 -0700 Subject: [PATCH 08/12] Red side works --- src/deploy/autos/paths/fp_home_2 | 1 + src/deploy/autos/paths/fp_home_3 | 1 + .../basic/prepare_auto_shoot_async.cc | 27 +++++++++++++++++++ .../commands/basic/prepare_auto_shoot_async.h | 19 +++++++++++++ 4 files changed, 48 insertions(+) create mode 100644 src/deploy/autos/paths/fp_home_2 create mode 100644 src/deploy/autos/paths/fp_home_3 create mode 100644 src/y2024/cpp/commands/basic/prepare_auto_shoot_async.cc create mode 100644 src/y2024/include/commands/basic/prepare_auto_shoot_async.h diff --git a/src/deploy/autos/paths/fp_home_2 b/src/deploy/autos/paths/fp_home_2 new file mode 100644 index 0000000..78a70dc --- /dev/null +++ b/src/deploy/autos/paths/fp_home_2 @@ -0,0 +1 @@ +P,219.5,54,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_home_3 b/src/deploy/autos/paths/fp_home_3 new file mode 100644 index 0000000..1f4de86 --- /dev/null +++ b/src/deploy/autos/paths/fp_home_3 @@ -0,0 +1 @@ +P,214.5,58,0,0 \ No newline at end of file diff --git a/src/y2024/cpp/commands/basic/prepare_auto_shoot_async.cc b/src/y2024/cpp/commands/basic/prepare_auto_shoot_async.cc new file mode 100644 index 0000000..4c0368c --- /dev/null +++ b/src/y2024/cpp/commands/basic/prepare_auto_shoot_async.cc @@ -0,0 +1,27 @@ +#include "commands/basic/prepare_auto_shoot_async.h" + +PrepareAutoShootAsyncCommand::PrepareAutoShootAsyncCommand( + RobotContainer& container) + : frc846::robot::GenericCommand{ + container, "prepare_auto_shoot_async_command"} { + AddRequirements({&container_.super_structure_, &container_.intake_, + &container_.shooter_}); +} + +void PrepareAutoShootAsyncCommand::OnInit() { Periodic(); } + +void PrepareAutoShootAsyncCommand::Periodic() { + auto defaultShootSetpoint{container_.super_structure_.getAutoShootSetpoint()}; + + container_.intake_.SetTarget({IntakeState::kHold}); + container_.shooter_.SetTarget({ShooterState::kRun}); + + defaultShootSetpoint.wrist += defaultShootSetpoint.pivot; + + container_.super_structure_.SetTargetSetpoint(defaultShootSetpoint); +} + +void PrepareAutoShootAsyncCommand::OnEnd(bool interrupted) {} + +bool PrepareAutoShootAsyncCommand::IsFinished() { return true; } \ No newline at end of file diff --git a/src/y2024/include/commands/basic/prepare_auto_shoot_async.h b/src/y2024/include/commands/basic/prepare_auto_shoot_async.h new file mode 100644 index 0000000..7004a3c --- /dev/null +++ b/src/y2024/include/commands/basic/prepare_auto_shoot_async.h @@ -0,0 +1,19 @@ +#pragma once + +#include "frc846/robot/GenericCommand.h" +#include "subsystems/robot_container.h" + +class PrepareAutoShootAsyncCommand + : public frc846::robot::GenericCommand { + public: + PrepareAutoShootAsyncCommand(RobotContainer& container); + + void OnInit() override; + + void Periodic() override; + + void OnEnd(bool interrupted) override; + + bool IsFinished() override; +}; From 78a960ca6b072b462f81269c98512941c6ca8b28 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Mon, 21 Oct 2024 19:16:08 -0700 Subject: [PATCH 09/12] Amp and source side autos --- src/deploy/autos/paths/aside_home | 1 + src/deploy/autos/paths/aside_piece_one | 1 + src/deploy/autos/paths/aside_piece_two | 1 + src/deploy/autos/paths/sside_home | 1 + src/deploy/autos/paths/sside_piece_one | 1 + src/deploy/autos/points.lst | 4 +++- src/deploy/autos/scripts/amp_side_auto | 14 ++++++++++++++ src/deploy/autos/scripts/source_side_auto | 9 +++++++++ 8 files changed, 31 insertions(+), 1 deletion(-) create mode 100644 src/deploy/autos/paths/aside_home create mode 100644 src/deploy/autos/paths/aside_piece_one create mode 100644 src/deploy/autos/paths/aside_piece_two create mode 100644 src/deploy/autos/paths/sside_home create mode 100644 src/deploy/autos/paths/sside_piece_one create mode 100644 src/deploy/autos/scripts/amp_side_auto create mode 100644 src/deploy/autos/scripts/source_side_auto diff --git a/src/deploy/autos/paths/aside_home b/src/deploy/autos/paths/aside_home new file mode 100644 index 0000000..cddce1b --- /dev/null +++ b/src/deploy/autos/paths/aside_home @@ -0,0 +1 @@ +F,kRightSub \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_one b/src/deploy/autos/paths/aside_piece_one new file mode 100644 index 0000000..24d8961 --- /dev/null +++ b/src/deploy/autos/paths/aside_piece_one @@ -0,0 +1 @@ +P,274.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_two b/src/deploy/autos/paths/aside_piece_two new file mode 100644 index 0000000..1defe69 --- /dev/null +++ b/src/deploy/autos/paths/aside_piece_two @@ -0,0 +1 @@ +P,292,324,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_home b/src/deploy/autos/paths/sside_home new file mode 100644 index 0000000..ea4e02b --- /dev/null +++ b/src/deploy/autos/paths/sside_home @@ -0,0 +1 @@ +F,kLeftSub \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_piece_one b/src/deploy/autos/paths/sside_piece_one new file mode 100644 index 0000000..19d8909 --- /dev/null +++ b/src/deploy/autos/paths/sside_piece_one @@ -0,0 +1 @@ +P,160.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/points.lst b/src/deploy/autos/points.lst index b4c5a0b..3125578 100644 --- a/src/deploy/autos/points.lst +++ b/src/deploy/autos/points.lst @@ -1,4 +1,6 @@ N,kOrigin,0,0,0,0 N,kSpeaker,217.5,-4,0,0 N,kAmp,0,0,90,0 -N,kPointBlank,217.5,49,0,0 \ No newline at end of file +N,kPointBlank,217.5,49,0,0 +N,kRightSub,253.15,57.75,0,60 +N,kLeftSub,222.84,57.75,0,-60 \ No newline at end of file diff --git a/src/deploy/autos/scripts/amp_side_auto b/src/deploy/autos/scripts/amp_side_auto new file mode 100644 index 0000000..58abade --- /dev/null +++ b/src/deploy/autos/scripts/amp_side_auto @@ -0,0 +1,14 @@ +0,2 +F,kRightSub +ACT,prep_shoot +ACT,shoot +ACT,deploy_intake +PATH,aside_piece_one +ACT,a_prep_shoot +PATH,aside_home +ACT,shoot +ACT,deploy_intake +PATH,aside_piece_two +ACT,a_prep_shoot +PATH,aside_home +ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_auto b/src/deploy/autos/scripts/source_side_auto new file mode 100644 index 0000000..e5fbe3e --- /dev/null +++ b/src/deploy/autos/scripts/source_side_auto @@ -0,0 +1,9 @@ +0,2 +F,kLeftSub +ACT,prep_shoot +ACT,shoot +ACT,deploy_intake +PATH,sside_piece_one +ACT,a_prep_shoot +PATH,aside_home +ACT,shoot \ No newline at end of file From 21645291a8ca53bbf7f81d24007e558b69d7f9cb Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Tue, 22 Oct 2024 18:37:31 -0700 Subject: [PATCH 10/12] Blue side works -- sketchy fix --- .../cpp/frc846/other/swerve_odometry.cc | 9 ++ src/y2024/cpp/commands/basic/pass_command.cc | 4 +- src/y2024/cpp/subsystems/hardware/bracer.cc | 4 +- .../cpp/subsystems/hardware/drivetrain.cc | 2 +- src/y2024/cpp/subsystems/hardware/pivot.cc | 6 +- .../subsystems/abstract/super_structure.h | 1 - src/y2024/resources/preferences_backup.nform | 91 +++++++----------- .../resources/preferences_cl_backup.nform | Bin 20041 -> 20055 bytes 8 files changed, 53 insertions(+), 64 deletions(-) diff --git a/src/frc846/cpp/frc846/other/swerve_odometry.cc b/src/frc846/cpp/frc846/other/swerve_odometry.cc index 7222232..4fa5871 100644 --- a/src/frc846/cpp/frc846/other/swerve_odometry.cc +++ b/src/frc846/cpp/frc846/other/swerve_odometry.cc @@ -1,5 +1,7 @@ #include "frc846/other/swerve_odometry.h" +#include + #include #include @@ -15,6 +17,12 @@ void SwerveOdometry::Update( std::array, kModuleCount> wheel_vecs, units::radian_t bearing) { + bool is_red = true; + if (auto alliance = frc::DriverStation::GetAlliance()) { + if (alliance.value() == frc::DriverStation::Alliance::kBlue) { + is_red = false; + } + } // change in distance from the last odometry update for (int i = 0; i < kModuleCount; i++) { units::foot_t wheel_dist = wheel_vecs[i].magnitude(); @@ -36,6 +44,7 @@ void SwerveOdometry::Update( relative_displacement += wheel_vecs[i] / kModuleCount; } + if (!is_red) bearing += 180_deg; position_ += relative_displacement.rotate(bearing, true); } diff --git a/src/y2024/cpp/commands/basic/pass_command.cc b/src/y2024/cpp/commands/basic/pass_command.cc index 8531bd5..db69bae 100644 --- a/src/y2024/cpp/commands/basic/pass_command.cc +++ b/src/y2024/cpp/commands/basic/pass_command.cc @@ -3,14 +3,12 @@ PassCommand::PassCommand(RobotContainer& container) : frc846::robot::GenericCommand{ container, "pass_command"} { - AddRequirements({&container_.super_structure_, &container_.intake_, - &container_.shooter_}); + AddRequirements({&container_.super_structure_}); } void PassCommand::OnInit() {} void PassCommand::Periodic() { - container_.intake_.SetTarget({IntakeState::kHold}); container_.shooter_.SetTarget({ShooterState::kRun}); container_.super_structure_.SetTargetSetpoint( diff --git a/src/y2024/cpp/subsystems/hardware/bracer.cc b/src/y2024/cpp/subsystems/hardware/bracer.cc index 4b37888..c1de1e8 100644 --- a/src/y2024/cpp/subsystems/hardware/bracer.cc +++ b/src/y2024/cpp/subsystems/hardware/bracer.cc @@ -35,10 +35,10 @@ BracerReadings BracerSubsystem::ReadFromHardware() { void BracerSubsystem::WriteToHardware(BracerTarget target) { if (target.state == BracerState::kExtend) { frc846::util::ShareTables::SetBoolean("is_climb_sequence", true); - bracer_.Set(1.0); + bracer_.Set(-0.3); } else if (target.state == BracerState::kRetract) { frc846::util::ShareTables::SetBoolean("is_climb_sequence", false); - bracer_.Set(-0.7); + bracer_.Set(0.7); } else { bracer_.Set(0.0); } diff --git a/src/y2024/cpp/subsystems/hardware/drivetrain.cc b/src/y2024/cpp/subsystems/hardware/drivetrain.cc index b846886..5b079d9 100644 --- a/src/y2024/cpp/subsystems/hardware/drivetrain.cc +++ b/src/y2024/cpp/subsystems/hardware/drivetrain.cc @@ -227,7 +227,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { readings.pose = frc846::math::FieldPoint(odometry_.position(), bearing, unfiltered_velocity); - pose_x_graph_.Graph(odometry_.position()[0]); + pose_x_graph_.Graph(odometry_.position()[0]); pose_y_graph_.Graph(odometry_.position()[1]); // pose_bearing_graph.Graph(odometry_.position().bearing); v_x_graph_.Graph(readings.pose.velocity[0]); diff --git a/src/y2024/cpp/subsystems/hardware/pivot.cc b/src/y2024/cpp/subsystems/hardware/pivot.cc index 421140f..1aec6db 100644 --- a/src/y2024/cpp/subsystems/hardware/pivot.cc +++ b/src/y2024/cpp/subsystems/hardware/pivot.cc @@ -86,7 +86,7 @@ PivotReadings PivotSubsystem::ReadFromHardware() { void PivotSubsystem::WriteToHardware(PivotTarget target) { if (target.climb_mode) { - hard_limits_.OverrideLimits(true); + // hard_limits_.OverrideLimits(true); pivot_one_.WriteDC(climb_duty_cycle_.value()); pivot_two_.WriteDC(climb_duty_cycle_.value()); @@ -94,7 +94,7 @@ void PivotSubsystem::WriteToHardware(PivotTarget target) { pivot_four_.WriteDC(climb_duty_cycle_.value()); } else if (auto pos = std::get_if(&target.pivot_output)) { - hard_limits_.OverrideLimits(false); + // hard_limits_.OverrideLimits(false); double output = dyFPID.calculate(*pos, GetReadings().pivot_position, pivot_one_.GetVelocityPercentage(), @@ -107,7 +107,7 @@ void PivotSubsystem::WriteToHardware(PivotTarget target) { target_pivot_pos_graph.Graph(*pos); } else if (auto output = std::get_if(&target.pivot_output)) { - hard_limits_.OverrideLimits(false); + // hard_limits_.OverrideLimits(false); pivot_one_.WriteDC(*output); pivot_two_.WriteDC(*output); diff --git a/src/y2024/include/subsystems/abstract/super_structure.h b/src/y2024/include/subsystems/abstract/super_structure.h index 2a1ca40..d3e779d 100644 --- a/src/y2024/include/subsystems/abstract/super_structure.h +++ b/src/y2024/include/subsystems/abstract/super_structure.h @@ -99,7 +99,6 @@ class SuperStructureSubsystem } bool CheckValidAdjustment(PTWSetpoint adjusted) { - return true; return arm_kinematics_calculator.WithinBounds( arm_kinematics_calculator.calculate( {(currentSetpoint.pivot + adjusted.pivot), diff --git a/src/y2024/resources/preferences_backup.nform b/src/y2024/resources/preferences_backup.nform index 572fac9..ea3997d 100644 --- a/src/y2024/resources/preferences_backup.nform +++ b/src/y2024/resources/preferences_backup.nform @@ -1,80 +1,56 @@ -Default.int | 0 Preferences/MotionTargets/Pivot/amp_position (deg).double | 70.000000 Preferences/MotionTargets/Pivot/auto_shoot_position (deg).double | 10.000000 Preferences/MotionTargets/Pivot/intake_position (deg).double | -17.000000 Preferences/MotionTargets/Pivot/pass_position (deg).double | 70.000000 Preferences/MotionTargets/Pivot/preclimb_position (deg).double | 83.000000 Preferences/MotionTargets/Pivot/prescore_position (deg).double | 80.000000 -Preferences/MotionTargets/Pivot/shoot_position (deg).double | 70.000000 +Preferences/MotionTargets/Pivot/shoot_position (deg).double | 7.000000 Preferences/MotionTargets/Pivot/source_position (deg).double | 36.000000 Preferences/MotionTargets/Pivot/stow_position (deg).double | -17.000000 Preferences/MotionTargets/Pivot/trapscore_position (deg).double | 80.000000 Preferences/MotionTargets/Telescope/amp_position (in).double | 4.000000 Preferences/MotionTargets/Telescope/trapscore_position (in).double | 3.000000 Preferences/MotionTargets/Wrist/amp_position (deg).double | 100.000000 -Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 50.500000 +Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 42.500000 Preferences/MotionTargets/Wrist/intake_position (deg).double | 32.000000 Preferences/MotionTargets/Wrist/pass_position (deg).double | 80.000000 Preferences/MotionTargets/Wrist/preclimb_position (deg).double | -30.000000 Preferences/MotionTargets/Wrist/prescore_position (deg).double | 40.000000 -Preferences/MotionTargets/Wrist/shoot_position (deg).double | 36.500000 +Preferences/MotionTargets/Wrist/shoot_position (deg).double | 41.000000 Preferences/MotionTargets/Wrist/source_position (deg).double | -10.000000 Preferences/MotionTargets/Wrist/stow_position (deg).double | -40.000000 Preferences/MotionTargets/Wrist/trapscore_position (deg).double | 80.000000 -Preferences/field_points/5p_intake_one_y (in).double | 94.000000 -Preferences/field_points/5p_intake_three_deg (deg).double | 0.000000 -Preferences/field_points/5p_intake_three_x (in).double | 153.000000 -Preferences/field_points/5p_intake_three_y (in).double | 94.000000 -Preferences/field_points/5p_intake_two_deg (deg).double | 50.000000 -Preferences/field_points/5p_intake_two_x (in).double | 160.500000 -Preferences/field_points/5p_intake_two_y (in).double | 112.000000 -Preferences/field_points/5p_mid_one_deg (deg).double | 0.000000 -Preferences/field_points/5p_mid_one_x (in).double | 217.500000 -Preferences/field_points/5p_mid_one_y (in).double | 70.000000 -Preferences/field_points/5p_mid_three_deg (deg).double | 0.000000 -Preferences/field_points/5p_mid_three_x (in).double | 117.000000 -Preferences/field_points/5p_mid_three_y (in).double | 70.000000 -Preferences/field_points/5p_mid_two_deg (deg).double | 0.000000 -Preferences/field_points/5p_mid_two_x (in).double | 230.000000 -Preferences/field_points/5p_mid_two_y (in).double | 70.000000 -Preferences/field_points/5p_shoot_one_y (in).double | 51.000000 -Preferences/field_points/5p_shoot_three_x (in).double | 213.000000 -Preferences/field_points/5p_shoot_three_y (in).double | 52.000000 -Preferences/field_points/5p_shoot_two_y (in).double | 52.000000 -Preferences/field_points/testing_point_y (in).double | 10000.000000 SuperStructure/auto/post_shoot_wait (s).double | 0.100000 SuperStructure/auto/pre_shoot_wait (s).double | 0.100000 SuperStructure/manual_control_deadband.double | 0.150000 SuperStructure/shooting/auto_shooter_height (in).double | 23.000000 SuperStructure/shooting/auto_shooter_x (in).double | 15.000000 -SuperStructure/shooting/teleop_shooter_height (in).double | 39.000000 -drivetrain/Configs/CurrentLimiting/target_threshold (A).double | 40.000000 -drivetrain/Configs/default_brake_mode.bool | false -drivetrain/Configs/gear_ratio.double | 16.800000 -drivetrain/Gains/kF.double | 0.000000 -drivetrain/Gains/kP.double | 0.120000 -drivetrain/auto_max_speed (fps).double | 15.000000 +SuperStructure/shooting/teleop_shooter_height (in).double | 23.650000 +SuperStructure/shooting/teleop_shooter_x (in).double | 25.000000 +drivetrain/auto_max_speed (fps).double | 13.000000 drivetrain/bearing_gains/d.double | -13.500000 drivetrain/bearing_gains/p.double | 2.000000 -drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A).double | 240.000000 +drivetrain/drive_esc/Configs/CurrentLimiting/peak_time_threshold (ms).double | 1000.000000 +drivetrain/drive_esc/Configs/CurrentLimiting/target_threshold (A).double | 480.000000 drivetrain/drive_esc/Gains/kF.double | 0.205000 drivetrain/drive_esc/Gains/kP.double | 0.010000 drivetrain/driver_speed_multiplier.double | 1.000000 -drivetrain/max_acceleration (fps_sq).double | 28.000000 -drivetrain/max_speed (fps).double | 2.400000 -drivetrain/module_BL/cancoder_offset (deg).double | -81.300000 -drivetrain/module_BR/cancoder_offset (deg).double | -173.900000 -drivetrain/module_FL/cancoder_offset (deg).double | 100.400000 -drivetrain/module_FR/cancoder_offset (deg).double | 24.400000 +drivetrain/max_acceleration (fps_sq).double | 16.000000 +drivetrain/max_deceleration (fps_sq).double | 12.000000 +drivetrain/max_speed (fps).double | 15.000000 +drivetrain/module_BL/cancoder_offset (deg).double | -82.700000 +drivetrain/module_BR/cancoder_offset (deg).double | -171.300000 +drivetrain/module_FL/cancoder_offset (deg).double | 99.800000 +drivetrain/module_FR/cancoder_offset (deg).double | 22.900000 drivetrain/percent_max_omega.double | 0.450000 drivetrain/pov_control_speed_.double | 10.000000 -drivetrain/tolerated_skid.double | 1.500000 -drivetrain/wheel_radius (in).double | 2.000000 +drivetrain/smart_current_braking/smart_current_limit (A).double | 150.000000 +drivetrain/wheel_radius (in).double | 1.910000 intake/Configs/ramp_time (s).double | 1.000000 intake/Configs/use_ramp_rate.bool | true intake/Gains/kF.double | 0.000190 intake/Gains/kP.double | 0.000000 -intake/base_intake_speed_ (fps).double | 3.000000 +intake/base_intake_speed_ (fps).double | 2.000000 intake/intake_feed_speed_ (fps).double | 6.000000 intake/release_speed.double | -0.400000 intake/retract_speed.double | 0.100000 @@ -86,11 +62,11 @@ pivot/Configs/HardLimits/use_position_limits.bool | true pivot/Configs/invert.bool | true pivot/DynamicFPID/braking_p.double | 0.400000 pivot/DynamicFPID/smart_current_limit (A).double | 30.000000 -pivot/Gains/kD.double | -1.000000 +pivot/Gains/kD.double | -1.500000 pivot/Gains/kF.double | 0.044000 pivot/Gains/kP.double | 0.045000 pivot/pivot_home_offset (deg).double | -17.000000 -pivot/pivot_tolerance (deg).double | 1.500000 +pivot/pivot_tolerance (deg).double | 2.500000 robot_container/init_bracers_.bool | false robot_container/init_drivetrain.bool | true robot_container/init_intake.bool | true @@ -100,28 +76,35 @@ robot_container/init_telescope.bool | true robot_container/init_wrist.bool | true shooter/Configs/invert.bool | true shooter/Gains/kF.double | 0.008000 -shooter/Gains/kP.double | 0.075000 +shooter/Gains/kP.double | 0.050000 shooter/shooter_speed (fps).double | 100.000000 shooter/shooter_speed_tolerance.double | 0.030000 shooter/shooter_spin.double | 0.000000 -shooter/shooting_exit_velocity.double | 45.000000 +shooter/shooting_exit_velocity.double | 80.000000 shooter/vFPID/smart_current_limit (A).double | 90.000000 telescope/Gains/kD.double | -0.100000 vision/camera_x_offset (in).double | -7.000000 vision/camera_y_offset (in).double | -10.000000 vision/can_bus_latency (ms).double | 20.000000 vision/default_is_red_side.bool | true -wrist/CloseDynamicFPID/smart_current_limit (A).double | 15.000000 -wrist/Configs/HardLimits/forward (deg).double | 130.000000 -wrist/Configs/HardLimits/reverse (deg).double | -42.000000 +vision/using_vision_autos.bool | false +wrist/Configs/HardLimits/forward (deg).double | 170.000000 +wrist/Configs/HardLimits/peak_output_reverse.double | -0.500000 +wrist/Configs/HardLimits/reverse (deg).double | -30.000000 wrist/Configs/HardLimits/use_position_limits.bool | true wrist/Configs/default_brake_mode.bool | false +wrist/Configs/gear_ratio.double | 9.000000 wrist/Configs/invert.bool | true -wrist/DynamicFPID/smart_current_limit (A).double | 50.000000 -wrist/Gains/kD.double | -1.650000 -wrist/Gains/kF.double | -0.065000 -wrist/Gains/kP.double | 0.033000 +wrist/DynamicFPID/Gains/kD.double | -2.400000 +wrist/DynamicFPID/Gains/kF.double | -0.130000 +wrist/DynamicFPID/Gains/kP.double | 0.030000 +wrist/DynamicFPID/smart_current_limit (A).double | 40.000000 +wrist/Gains/kD.double | 0.000000 +wrist/Gains/kF.double | 0.000000 +wrist/Gains/kP.double | 0.000600 wrist/cg_offset_wrist (deg).double | 155.000000 wrist/home_offset_wrist (deg).double | -42.000000 -wrist/homing_speed.double | -0.350000 +wrist/homing_dc_cut.double | 1.600000 +wrist/homing_max_speed (deg_per_s).double | 55.000000 +wrist/homing_speed.double | -0.100000 wrist/homing_velocity_tolerance (deg_per_s).double | 0.500000 diff --git a/src/y2024/resources/preferences_cl_backup.nform b/src/y2024/resources/preferences_cl_backup.nform index 8ed13420361841693db45b799f4d5e4125063ab0..6730f84622774f078dbec7b070da8ae90c77a422 100644 GIT binary patch delta 12 TcmX>(hw=Ix#toi+6ASnNCK3fP delta 9 QcmcaUhw Date: Wed, 23 Oct 2024 18:38:37 -0700 Subject: [PATCH 11/12] Four autos working --- src/deploy/autos/paths/aside_home | 2 +- src/deploy/autos/paths/aside_home_2 | 2 ++ src/deploy/autos/paths/aside_piece_one | 2 +- src/deploy/autos/paths/aside_piece_two | 2 +- src/deploy/autos/paths/fp_center_run | 2 +- src/deploy/autos/paths/sside_exit | 2 ++ src/deploy/autos/paths/sside_home | 2 +- src/deploy/autos/paths/sside_piece_one | 2 +- src/deploy/autos/points.lst | 4 ++-- src/deploy/autos/scripts/amp_side_auto | 5 ----- src/deploy/autos/scripts/four_piece_auto | 3 ++- src/deploy/autos/scripts/source_side_auto | 2 +- src/deploy/autos/scripts/source_side_mobility | 5 +++++ .../swerve/follow_trajectory_command.cc | 5 ++++- src/y2024/cpp/FunkyRobot.cc | 5 +++++ .../basic/auto_deploy_intake_command.cc | 7 ++----- .../complex/home_during_auto_command.cc | 21 +++++++++++-------- src/y2024/cpp/commands/teleop/leds_command.cc | 2 +- .../cpp/subsystems/abstract/control_input.cc | 5 ++--- .../cpp/subsystems/hardware/drivetrain.cc | 2 +- .../include/subsystems/hardware/drivetrain.h | 2 +- src/y2024/resources/preferences_backup.nform | 4 ++-- 22 files changed, 50 insertions(+), 38 deletions(-) create mode 100644 src/deploy/autos/paths/aside_home_2 create mode 100644 src/deploy/autos/paths/sside_exit create mode 100644 src/deploy/autos/scripts/source_side_mobility diff --git a/src/deploy/autos/paths/aside_home b/src/deploy/autos/paths/aside_home index cddce1b..f4dc4d5 100644 --- a/src/deploy/autos/paths/aside_home +++ b/src/deploy/autos/paths/aside_home @@ -1 +1 @@ -F,kRightSub \ No newline at end of file +P,277.15,37.5,55,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_home_2 b/src/deploy/autos/paths/aside_home_2 new file mode 100644 index 0000000..2af41d9 --- /dev/null +++ b/src/deploy/autos/paths/aside_home_2 @@ -0,0 +1,2 @@ +P,385.15,-60.5,0,3 +P,354.15,-80.5,50,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_one b/src/deploy/autos/paths/aside_piece_one index 24d8961..e83b5e5 100644 --- a/src/deploy/autos/paths/aside_piece_one +++ b/src/deploy/autos/paths/aside_piece_one @@ -1 +1 @@ -P,274.5,94,0,0 \ No newline at end of file +P,290.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_two b/src/deploy/autos/paths/aside_piece_two index 1defe69..b12764d 100644 --- a/src/deploy/autos/paths/aside_piece_two +++ b/src/deploy/autos/paths/aside_piece_two @@ -1 +1 @@ -P,292,324,0,0 \ No newline at end of file +P,424,165,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_center_run b/src/deploy/autos/paths/fp_center_run index 4fa6c1b..1871b90 100644 --- a/src/deploy/autos/paths/fp_center_run +++ b/src/deploy/autos/paths/fp_center_run @@ -1 +1 @@ -P,217.5,112.0,0,0 \ No newline at end of file +P,214.5,112.0,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_exit b/src/deploy/autos/paths/sside_exit new file mode 100644 index 0000000..8db1591 --- /dev/null +++ b/src/deploy/autos/paths/sside_exit @@ -0,0 +1,2 @@ +P,150.5,80.5,0,0 +P,150.5,115.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_home b/src/deploy/autos/paths/sside_home index ea4e02b..0631a84 100644 --- a/src/deploy/autos/paths/sside_home +++ b/src/deploy/autos/paths/sside_home @@ -1 +1 @@ -F,kLeftSub \ No newline at end of file +P,205.15,36.75,-55,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_piece_one b/src/deploy/autos/paths/sside_piece_one index 19d8909..4b23bfd 100644 --- a/src/deploy/autos/paths/sside_piece_one +++ b/src/deploy/autos/paths/sside_piece_one @@ -1 +1 @@ -P,160.5,94,0,0 \ No newline at end of file +P,186.5,95.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/points.lst b/src/deploy/autos/points.lst index 3125578..8824d81 100644 --- a/src/deploy/autos/points.lst +++ b/src/deploy/autos/points.lst @@ -2,5 +2,5 @@ N,kOrigin,0,0,0,0 N,kSpeaker,217.5,-4,0,0 N,kAmp,0,0,90,0 N,kPointBlank,217.5,49,0,0 -N,kRightSub,253.15,57.75,0,60 -N,kLeftSub,222.84,57.75,0,-60 \ No newline at end of file +N,kRightSub,253.15,57.75,60,0 +N,kLeftSub,222.84,57.75,-60,0 \ No newline at end of file diff --git a/src/deploy/autos/scripts/amp_side_auto b/src/deploy/autos/scripts/amp_side_auto index 58abade..0779e23 100644 --- a/src/deploy/autos/scripts/amp_side_auto +++ b/src/deploy/autos/scripts/amp_side_auto @@ -6,9 +6,4 @@ ACT,deploy_intake PATH,aside_piece_one ACT,a_prep_shoot PATH,aside_home -ACT,shoot -ACT,deploy_intake -PATH,aside_piece_two -ACT,a_prep_shoot -PATH,aside_home ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/four_piece_auto b/src/deploy/autos/scripts/four_piece_auto index c2c44c5..99421ec 100644 --- a/src/deploy/autos/scripts/four_piece_auto +++ b/src/deploy/autos/scripts/four_piece_auto @@ -16,4 +16,5 @@ ACT,deploy_intake PATH,fp_piece_three ACT,a_prep_shoot PATH,fp_home_3 -ACT,shoot \ No newline at end of file +ACT,shoot +PATH,fp_center_run \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_auto b/src/deploy/autos/scripts/source_side_auto index e5fbe3e..0819bd5 100644 --- a/src/deploy/autos/scripts/source_side_auto +++ b/src/deploy/autos/scripts/source_side_auto @@ -5,5 +5,5 @@ ACT,shoot ACT,deploy_intake PATH,sside_piece_one ACT,a_prep_shoot -PATH,aside_home +PATH,sside_home ACT,shoot \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_mobility b/src/deploy/autos/scripts/source_side_mobility new file mode 100644 index 0000000..ed99f1d --- /dev/null +++ b/src/deploy/autos/scripts/source_side_mobility @@ -0,0 +1,5 @@ +0,2 +F,kLeftSub +ACT,prep_shoot +ACT,shoot +PATH,sside_exit \ No newline at end of file diff --git a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc index 27b14a7..b372b8a 100644 --- a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc +++ b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc @@ -18,7 +18,7 @@ void FollowTrajectoryCommand::OnInit() { container_.drivetrain_.max_acceleration_.value(), container_.drivetrain_.max_deceleration_.value(), container_.drivetrain_.max_speed_.value(), - 100_fps_sq, 100_fps_sq, 3_in, 20_ms}); + 100_fps_sq, 100_fps_sq, 5_in, 20_ms}); Log("Starting Trajectory"); Log("Initial pose x{}, y{}, Bearing {}", @@ -56,6 +56,9 @@ void FollowTrajectoryCommand::OnInit() { if (path_points_.size() < 2) { Error("Trajectory size ({}) is less than 2 - ending!", path_points_.size()); is_done_ = true; + } else { + Log("First target point x{} y{}", path_points_[target_idx_].point[0], + path_points_[target_idx_].point[1]); } } diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index fbaf3be..211480f 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/src/y2024/cpp/FunkyRobot.cc @@ -31,6 +31,8 @@ FunkyRobot::FunkyRobot() : GenericRobot{&container_} {} void FunkyRobot::OnInitialize() { + container_.leds_.SetDefaultCommand(LEDsCommand{container_}); + Field::Setup(); for (auto x : Field::getAllAutoData()) { @@ -74,6 +76,8 @@ void FunkyRobot::OnInitialize() { } void FunkyRobot::OnDisable() { + container_.leds_.SetDefaultCommand(LEDsCommand{container_}); + container_.pivot_.Brake(); container_.wrist_.Brake(); container_.telescope_.Brake(); @@ -102,6 +106,7 @@ void FunkyRobot::OnPeriodic() { if (!homing_switch_.Get()) { container_.super_structure_.ZeroSubsystem(); + frc846::util::ShareTables::SetBoolean("zero sequence", true); Log("Zeroing subsystems..."); } diff --git a/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc b/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc index 2b90295..ac41465 100644 --- a/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc +++ b/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc @@ -7,7 +7,7 @@ AutoDeployIntakeCommand::AutoDeployIntakeCommand(RobotContainer& container) &container_.super_structure_}); } -void AutoDeployIntakeCommand::OnInit() {} +void AutoDeployIntakeCommand::OnInit() { Periodic(); } void AutoDeployIntakeCommand::Periodic() { container_.intake_.SetTarget({kIntake}); @@ -18,7 +18,4 @@ void AutoDeployIntakeCommand::Periodic() { void AutoDeployIntakeCommand::OnEnd(bool interrupted) {} -bool AutoDeployIntakeCommand::IsFinished() { - return container_.super_structure_.hasReachedSetpoint( - container_.super_structure_.getIntakeSetpoint()); -} \ No newline at end of file +bool AutoDeployIntakeCommand::IsFinished() { return true; } \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/home_during_auto_command.cc b/src/y2024/cpp/commands/complex/home_during_auto_command.cc index 1382317..f25ca0f 100644 --- a/src/y2024/cpp/commands/complex/home_during_auto_command.cc +++ b/src/y2024/cpp/commands/complex/home_during_auto_command.cc @@ -14,12 +14,15 @@ HomeDuringAutoCommand::HomeDuringAutoCommand(RobotContainer& container) : frc846::robot::GenericCommandGroup{ container, "home_during_auto_command", - frc2::SequentialCommandGroup{ - frc2::ParallelDeadlineGroup{ - frc2::WaitUntilCommand{[&] { - return container.pivot_.WithinTolerance( - container.super_structure_.getStowSetpoint().pivot); - }}, - StowCommand{container}}, - WristZeroCommand{container}, - }} {} \ No newline at end of file + frc2::SequentialCommandGroup{frc2::ParallelDeadlineGroup{ + frc2::WaitUntilCommand{[&] { + return container_.super_structure_.wrist_->GetHasZeroed(); + }}, + frc2::SequentialCommandGroup{ + frc2::ParallelDeadlineGroup{ + frc2::WaitUntilCommand{[&] { + return container.pivot_.WithinTolerance( + container.super_structure_.getStowSetpoint().pivot); + }}, + StowCommand{container}}, + WristZeroCommand{container}}}}} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/teleop/leds_command.cc b/src/y2024/cpp/commands/teleop/leds_command.cc index e5792a3..b913e5f 100644 --- a/src/y2024/cpp/commands/teleop/leds_command.cc +++ b/src/y2024/cpp/commands/teleop/leds_command.cc @@ -17,7 +17,7 @@ void LEDsCommand::Periodic() { lstate = LEDsState::kLEDSAutonomous; } else if (frc846::util::ShareTables::GetBoolean("zero sequence")) { lstate = LEDsState::kLEDSZeroing; - } else if (container_.super_structure_.GetHasZeroed()) { + } else if (!container_.super_structure_.GetHasZeroed()) { lstate = LEDsState::kLEDSNotReady; } else if (frc846::util::ShareTables::GetString("mode") == "disabled") { lstate = LEDsState::kLEDSDisabled; diff --git a/src/y2024/cpp/subsystems/abstract/control_input.cc b/src/y2024/cpp/subsystems/abstract/control_input.cc index 81dda79..18c14b0 100644 --- a/src/y2024/cpp/subsystems/abstract/control_input.cc +++ b/src/y2024/cpp/subsystems/abstract/control_input.cc @@ -167,10 +167,9 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() { // PASS ci_readings_.running_pass = op_readings.x_button; - // SHOOT + // SHOOTING ci_readings_.shooting = - ((ci_readings_.running_prep_shoot || ci_readings_.running_super_shoot || - ci_readings_.running_pass) && + ((ci_readings_.running_prep_shoot || ci_readings_.running_super_shoot) && dr_readings.right_bumper) || op_readings.pov == frc846::XboxPOV::kRight; // EJECT diff --git a/src/y2024/cpp/subsystems/hardware/drivetrain.cc b/src/y2024/cpp/subsystems/hardware/drivetrain.cc index 5b079d9..b846886 100644 --- a/src/y2024/cpp/subsystems/hardware/drivetrain.cc +++ b/src/y2024/cpp/subsystems/hardware/drivetrain.cc @@ -227,7 +227,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { readings.pose = frc846::math::FieldPoint(odometry_.position(), bearing, unfiltered_velocity); - pose_x_graph_.Graph(odometry_.position()[0]); + pose_x_graph_.Graph(odometry_.position()[0]); pose_y_graph_.Graph(odometry_.position()[1]); // pose_bearing_graph.Graph(odometry_.position().bearing); v_x_graph_.Graph(readings.pose.velocity[0]); diff --git a/src/y2024/include/subsystems/hardware/drivetrain.h b/src/y2024/include/subsystems/hardware/drivetrain.h index 39c8a6a..a879584 100644 --- a/src/y2024/include/subsystems/hardware/drivetrain.h +++ b/src/y2024/include/subsystems/hardware/drivetrain.h @@ -111,7 +111,7 @@ class DrivetrainSubsystem // Closed loop tuned for this frc846::ntinf::Pref auto_max_speed_{ - *this, "auto_max_speed", 11.2_fps}; + *this, "auto_speed", 11.2_fps}; frc846::ntinf::Pref driver_speed_multiplier_{ *this, "driver_speed_multiplier", 1.0}; diff --git a/src/y2024/resources/preferences_backup.nform b/src/y2024/resources/preferences_backup.nform index ea3997d..e56c2e2 100644 --- a/src/y2024/resources/preferences_backup.nform +++ b/src/y2024/resources/preferences_backup.nform @@ -13,7 +13,7 @@ Preferences/MotionTargets/Telescope/trapscore_position (in).double | 3.000000 Preferences/MotionTargets/Wrist/amp_position (deg).double | 100.000000 Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 42.500000 Preferences/MotionTargets/Wrist/intake_position (deg).double | 32.000000 -Preferences/MotionTargets/Wrist/pass_position (deg).double | 80.000000 +Preferences/MotionTargets/Wrist/pass_position (deg).double | 86.000000 Preferences/MotionTargets/Wrist/preclimb_position (deg).double | -30.000000 Preferences/MotionTargets/Wrist/prescore_position (deg).double | 40.000000 Preferences/MotionTargets/Wrist/shoot_position (deg).double | 41.000000 @@ -27,7 +27,7 @@ SuperStructure/shooting/auto_shooter_height (in).double | 23.000000 SuperStructure/shooting/auto_shooter_x (in).double | 15.000000 SuperStructure/shooting/teleop_shooter_height (in).double | 23.650000 SuperStructure/shooting/teleop_shooter_x (in).double | 25.000000 -drivetrain/auto_max_speed (fps).double | 13.000000 +drivetrain/auto_max_speed (fps).double | 3.000000 drivetrain/bearing_gains/d.double | -13.500000 drivetrain/bearing_gains/p.double | 2.000000 drivetrain/drive_esc/Configs/CurrentLimiting/peak_time_threshold (ms).double | 1000.000000 From 948fe89d3d6a83fe9fcfd73776ce2539085edfcb Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Thu, 24 Oct 2024 18:12:08 -0700 Subject: [PATCH 12/12] Pre-CCC --- src/y2024/cpp/commands/teleop/bracer_command.cc | 6 ++++++ src/y2024/cpp/subsystems/hardware/bracer.cc | 4 ++-- src/y2024/cpp/subsystems/hardware/pivot.cc | 4 ++-- src/y2024/include/subsystems/hardware/bracer.h | 3 +++ 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/y2024/cpp/commands/teleop/bracer_command.cc b/src/y2024/cpp/commands/teleop/bracer_command.cc index 116dc2c..494f747 100644 --- a/src/y2024/cpp/commands/teleop/bracer_command.cc +++ b/src/y2024/cpp/commands/teleop/bracer_command.cc @@ -31,6 +31,12 @@ void BracerCommand::Periodic() { else target.state = BracerState::kStow; + if (container_.control_input_.operator_.GetReadings().y_button) { + target.state = BracerState::kRetract; + } else if (container_.control_input_.operator_.GetReadings().b_button) { + target.state = BracerState::kExtend; + } + prev_ci_readings_ = ci_readings_; container_.bracer_.SetTarget(target); } diff --git a/src/y2024/cpp/subsystems/hardware/bracer.cc b/src/y2024/cpp/subsystems/hardware/bracer.cc index c1de1e8..51affec 100644 --- a/src/y2024/cpp/subsystems/hardware/bracer.cc +++ b/src/y2024/cpp/subsystems/hardware/bracer.cc @@ -35,10 +35,10 @@ BracerReadings BracerSubsystem::ReadFromHardware() { void BracerSubsystem::WriteToHardware(BracerTarget target) { if (target.state == BracerState::kExtend) { frc846::util::ShareTables::SetBoolean("is_climb_sequence", true); - bracer_.Set(-0.3); + bracer_.Set(out_speed_.value()); } else if (target.state == BracerState::kRetract) { frc846::util::ShareTables::SetBoolean("is_climb_sequence", false); - bracer_.Set(0.7); + bracer_.Set(in_speed_.value()); } else { bracer_.Set(0.0); } diff --git a/src/y2024/cpp/subsystems/hardware/pivot.cc b/src/y2024/cpp/subsystems/hardware/pivot.cc index 1aec6db..979e941 100644 --- a/src/y2024/cpp/subsystems/hardware/pivot.cc +++ b/src/y2024/cpp/subsystems/hardware/pivot.cc @@ -86,7 +86,7 @@ PivotReadings PivotSubsystem::ReadFromHardware() { void PivotSubsystem::WriteToHardware(PivotTarget target) { if (target.climb_mode) { - // hard_limits_.OverrideLimits(true); + hard_limits_.OverrideLimits(true); pivot_one_.WriteDC(climb_duty_cycle_.value()); pivot_two_.WriteDC(climb_duty_cycle_.value()); @@ -94,7 +94,7 @@ void PivotSubsystem::WriteToHardware(PivotTarget target) { pivot_four_.WriteDC(climb_duty_cycle_.value()); } else if (auto pos = std::get_if(&target.pivot_output)) { - // hard_limits_.OverrideLimits(false); + hard_limits_.OverrideLimits(false); double output = dyFPID.calculate(*pos, GetReadings().pivot_position, pivot_one_.GetVelocityPercentage(), diff --git a/src/y2024/include/subsystems/hardware/bracer.h b/src/y2024/include/subsystems/hardware/bracer.h index d2a8e91..7fcfaf8 100644 --- a/src/y2024/include/subsystems/hardware/bracer.h +++ b/src/y2024/include/subsystems/hardware/bracer.h @@ -33,6 +33,9 @@ class BracerSubsystem bool VerifyHardware() override; private: + frc846::ntinf::Pref out_speed_{*this, "bracer_out_speed", -0.3}; + frc846::ntinf::Pref in_speed_{*this, "bracer_in_speed", 0.5}; + frc846::ntinf::Grapher left_climb_{*this, "left_hook_engaged"}; frc846::ntinf::Grapher right_climb_{*this, "right_hook_engaged"};