From cf9b5b92ed7bd2a37f08eecd61b2d41d2517848a Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Thu, 19 Sep 2024 08:13:55 -0700 Subject: [PATCH] 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