From 5ef788f40355fc7878abf266293671ecd160e0a2 Mon Sep 17 00:00:00 2001 From: vyaasBaskar Date: Tue, 17 Sep 2024 17:01:36 -0700 Subject: [PATCH] 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