From 432ed9a734c3099e4b90c3883c975b8cc27a03a7 Mon Sep 17 00:00:00 2001 From: Sean Chan <sean.chan3@student.education.wa.edu.au> Date: Sun, 4 Feb 2024 16:00:36 +0800 Subject: [PATCH] . --- src/main/cpp/Auto.cpp | 272 +++++++++++++++---------------- src/main/cpp/IntakeBehaviour.cpp | 2 +- src/main/cpp/Robot.cpp | 29 +++- src/main/include/Auto.h | 12 +- src/main/include/Robot.h | 28 +++- 5 files changed, 192 insertions(+), 151 deletions(-) diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp index 2007a923..ac3f4f8c 100644 --- a/src/main/cpp/Auto.cpp +++ b/src/main/cpp/Auto.cpp @@ -1,31 +1,31 @@ #include "Auto.h" -std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) { - return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); +std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { + return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); } // Shoots starting note then moves out of starting position. -std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) { - return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); +std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { + return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); /* 4N Close @@ -39,37 +39,37 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDriv */ } -std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) { - return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); // do this if possible - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); +std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { + return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); // do this if possible + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); /* 4N Far @@ -90,55 +90,55 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive */ } -std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) { - return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); +std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { + return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); // 4N Close 2N Far // 1. Shoot note @@ -162,34 +162,34 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::S } -std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) { - return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoIntake>(&_intake, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); +std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { + return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoIntake>(_intake, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); } // 4N Close 1N Far @@ -232,9 +232,9 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::S // */ // } -std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) { - return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2); - behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field); - behaviour::make<AutoShoot>(&_shooter, 8_V); - behaviour::make<AutoIntake>(&_intake, 8_V); +std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) { + return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2); + behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field); + behaviour::make<AutoShoot>(_shooter, 8_V); + behaviour::make<AutoIntake>(_intake, 8_V); } // This auto is a test for auto to see if all things work. \ No newline at end of file diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp index 31ea9fb5..3e7d55c5 100644 --- a/src/main/cpp/IntakeBehaviour.cpp +++ b/src/main/cpp/IntakeBehaviour.cpp @@ -54,4 +54,4 @@ void AutoIntake::OnTick(units::second_t dt) { // } else if (_intake->GetConfig().magSensor->Get() == 0) { // _intake->setState(IntakeState::kIdle); // } -} +} \ No newline at end of file diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 888245e6..9ea002ad 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -20,6 +20,8 @@ #include "behaviour/HasBehaviour.h" +#include "Auto.h" + static units::second_t lastPeriodic; void Robot::RobotInit() { @@ -29,9 +31,13 @@ void Robot::RobotInit() { [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); }); sched = wom::BehaviourScheduler::GetInstance(); - m_chooser.SetDefaultOption("Default Auto", "Default Auto"); + m_chooser.SetDefaultOption("kTaxi", "kTaxi"); + + for (auto &option : autoOptions) { + m_chooser.AddOption(option, option); + } - // frc::SmartDashboard::PutData("Auto Selector", &m_chooser); + frc::SmartDashboard::PutData("Auto Modes", &m_chooser); // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json"); @@ -42,7 +48,7 @@ void Robot::RobotInit() { // frc::SmartDashboard::PutData("Field", &m_field); - // simulation_timer = frc::Timer(); + timer = frc::Timer(); // robotmap.swerveBase.gyro->Reset(); @@ -105,11 +111,28 @@ void Robot::RobotPeriodic() { _swerveDrive->OnUpdate(dt); alphaArm->OnUpdate(dt); intake->OnUpdate(dt); + } void Robot::AutonomousInit() { loop.Clear(); sched->InterruptAll(); + + m_autoSelected = m_chooser.GetSelected(); + fmt::print("Auto selected: {}\n", m_autoSelected); + if (m_autoSelected == "Taxi") { + sched->Schedule(autos::Taxi(_swerveDrive, &timer, &field, shooter, intake, alphaArm)); + } else if (m_autoSelected == "Auto Test") { + sched->Schedule(autos::AutoTest(_swerveDrive, &timer, &field, shooter, intake, alphaArm)); + } else if (m_autoSelected == "Quadruple Close") { + sched->Schedule(autos::QuadrupleClose(_swerveDrive, &timer, &field, shooter, intake, alphaArm)); + } else if (m_autoSelected == "Quadruple Far") { + sched->Schedule(autos::QuadrupleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm)); + } else if (m_autoSelected == "Quadruple Close Double Far") { + sched->Schedule(autos::QuadrupleCloseDoubleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm)); + } else if (m_autoSelected == "Quadruple Close Single Far") { + sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm)); + } } void Robot::AutonomousPeriodic() {} diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h index de439b35..bd1a62f8 100644 --- a/src/main/include/Auto.h +++ b/src/main/include/Auto.h @@ -8,15 +8,15 @@ namespace autos { - std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm); + std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); - std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm); + std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); - std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm); + std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); - std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm); + std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); - std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm); + std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); - std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm); + std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm); } \ No newline at end of file diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 158a6fa6..52a7b12f 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -44,17 +44,35 @@ class Robot : public frc::TimedRobot { RobotMap robotmap; wom::BehaviourScheduler* sched; frc::EventLoop loop; - Shooter* shooter; - Intake* intake; - frc::SendableChooser<std::string> m_chooser; - frc::Field2d m_field; - frc::Timer simulation_timer; + frc::Field2d field; + frc::Timer timer; frc::SendableChooser<std::string> m_path_chooser; wom::SwerveDrive* _swerveDrive; AlphaArm* alphaArm; + Intake* intake; + Shooter* shooter; + + frc::SendableChooser<std::string> m_chooser; + const std::string kTaxi = "kTaxi"; + const std::string kAutoTest = "kAutoTest"; + const std::string kQuadrupleClose = "kQuadrupleClose"; + const std::string kQuadrupleFar = "kQuadrupleFar"; + const std::string kQuadrupleCloseDoubleFar = "kQuadrupleCloseDoubleFar"; + const std::string kQuadrupleCloseSingleFar = "kQuadrupleCloseSingleFar"; + std::string m_autoSelected; + + std::string defaultAuto = "kTaxi"; + std::vector<std::string> autoOptions = { + kTaxi, + kAutoTest, + kQuadrupleClose, + kQuadrupleFar, + kQuadrupleCloseDoubleFar, + kQuadrupleCloseSingleFar, + }; // ctre::phoenix6::hardware::TalonFX *frontLeft; // ctre::phoenix6::hardware::TalonFX *frontRight;